需要の低そうな数式の導出過程や磁気センサーのお話は後回しにしています．
1.最小二乗法による球面フィッティング
多少のノイズが存在する中で，点(a,b,c)を中心とした半径rの球面の座標データが沢山あった時にパラメータa,b,c,rを計算したいという事があるか時々あるかと思います．今回私も磁気センサーのオフセット（バイアス）を計算しなければならず，問題を単純化していくとこの問題に行き着きました．
今回，球面の座標データ\((m_{xi},m_{yi},m_{zi})\)がN個存在するとき，各パラメータは次の連立方程式を計算することで求めることができます（導出過程は後述）．
ただし変数dは次のように定義された値です．
計算結果はこのようになりました．
2.磁気センサーのオフセットの計算に適用する
さて，（僕にとっては）本題です．磁気センサーはその場所の磁束密度を計測するセンサーなのですが，生データにはオフセット（バイアス）が存在しています．実際に磁気センサーを動かしながら取得したデータをプロットしてみたものがこちらです．
概ねある点（オフセット）を中心とした球面上のデータになっていると信じます．というか，そういうモデルとします．正直センサーが悪かったとしか言いようが無い……でも一部全くプロットされていないところがあるのは何でだ pic.twitter.com/OvUckezTVp— もりとにー (@TonyMooori) 2016年5月27日
というのも，磁気センサーは磁束密度，つまり磁場のベクトルを計測しているわけで，周りに変なものがなければ定常な磁場のベクトルである地磁気が測定されるはずだからです．
しかし，僕はドローンの姿勢検出に磁気センサーを使うのですが，回転角度を求めるときにオフセット（バイアス）があると非常に困るのです．
平たく言えば球の中心を原点に持ってきたい，というわけです．
実際に先ほどのプログラムをArduino用に改良してバイアスを計測してみます．Arduinoでは大きな配列を扱うことはできません．
しかしこの連立方程式を作るのに大きな配列を用意する必要はないのでご安心ください．
使用する磁気センサーはなんでも良いのですが，手元にあるのがmpu9150なので，センサー開発元のsparkfunが公開しているライブラリを使います．
・MPU-9150_Breakout: Example code and PCB design files for the MPU-9105, 9DOF.
実行結果としては僕の環境ではうまくいきました（a,b,c,rの値が常にほぼ一定）．この記事を読んでくださっている方々はわかると思いますが，このプログラムが直接役に立つということはそんなに無いですが，参考程度にはなったかと思います．
・数式の導出
個人的にはあまり興味はないのですが（とは言うものの面白いのですが），球の最小二乗法の式の導出を示しておきます．まず，球面の座標データ\((m_{xi},m_{yi},m_{zi})\)がN個存在するとすると，全ての点で以下の数式が成り立ちます．\[(m_{xi}-a)^2+(m_{yi}-b)^2+(m_{zi}-c)^2=r^2 \] ただし点(a,b,c)は球の中心座標，rは半径です．しかし実際にはノイズが入っている上，データ数Nは非常に多いのでこのようにはなりません．なのでこの問題を次のように置き換えます．とりあえず関数を変形します．
\[ (m_{xi}-a)^2+(m_{yi}-b)^2+(m_{zi}-c)^2-r^2=0 \] ここで次の関数を最小化する最小化問題に置き換えます．
\[ J = \frac{1}{4}\sum_{i=1}^{N}{\{(m_{xi}-a)^2+(m_{yi}-b)^2+(m_{zi}-c)^2-r^2\}}^2 \] 1/4は気にしないでください．もう少し変形します．
\[ J= \frac{1}{4}\sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )}^2 \] ただし\( a^2+b^2+c^2-r^2=-2d \)です．係数は後々綺麗になるように調整しているものなので関係ないです．ここでこの関数を最小化するa,b,c,d周りでは偏微分が0になっているはずです．まぁ厳密にどうとかは私は知りませんが．なのでとりあえず偏微分します．
\[ \frac{\partial J}{\partial a} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-m_{xi})} = 0 \\ \frac{\partial J}{\partial b} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-m_{yi})} = 0 \\ \frac{\partial J}{\partial c} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-m_{zi})} = 0 \\ \frac{\partial J}{\partial d} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-1)} = 0 \] 更に変形して行きます． \[ \sum_{i=1}^{N}{( -m_{xi}^3 + 2am_{xi}^2-m_{xi}m_{yi}^2 + 2bm_{xi}m_{yi}-m_{xi}m_{zi}^2 + 2cm_{xi}m_{zi}+2m_{xi}d)} = 0 \\ \sum_{i=1}^{N}{( -m_{xi}^2m_{yi} + 2am_{xi}m_{yi}-m_{yi}^3 + 2bm_{yi}^2-m_{yi}m_{zi}^2 + 2cm_{yi}m_{zi}+2m_{yi}d)} = 0 \\ \sum_{i=1}^{N}{( -m_{xi}^2m_{zi} + 2am_{xi}m_{zi}-m_{yi}^2m_{zi} + 2bm_{yi}m_{zi}-m_{zi}^3 + 2cm_{zi}^2+2m_{zi}d)} = 0 \\ \sum_{i=1}^{N}{( -m_{xi}^2 + 2am_{xi}-m_{yi}^2 + 2bm_{yi}-m_{zi}^2 + 2cm_{zi}+2d )} = 0 \] 最後に，普通の連立1次方程式に直します． \[ \left( \begin{array}{ccc} \sum{2m_{xi}^2} & \sum{2m_{xi}m_{yi}} & \sum{2m_{xi}m_{zi}} & \sum{2m_{xi}} \\ \sum{2m_{xi}m_{yi}} & \sum{2m_{yi}^2} & \sum{2m_{yi}m_{zi}} & \sum{2m_{yi}} \\ \sum{2m_{xi}m_{zi}} & \sum{2m_{yi}m_{zi}} & \sum{2m_{zi}^2} & \sum{2m_{zi}} \\ \sum{2m_{xi}} & \sum{2m_{yi}} & \sum{2m_{zi}} & {2N} \\ \end{array} \right) \left( \begin{array}{ccc} a \\ b \\ c \\ d \\ \end{array} \right) = \left( \begin{array}{ccc} \sum{m_{xi}(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )} \\ \sum{m_{yi}(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )} \\ \sum{m_{zi}(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )} \\ \sum{(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )}\\ \end{array} \right) \] こんなところでしょうか．結局係数が消せてないのですが……
このような話を理解するには『これなら分かる最適化数学 ―基礎原理から計算手法まで― 』という本がおすすめです．
全部ではないですが，最近読みました(後半部分等，少々難しい内容がありましたので)．
ただ内容としては非常に面白かったです．
今日は以上です．
0 件のコメント:
コメントを投稿