理系的な戯れ

理工学系とくにロボットやドローンに関する計算・プログラミング等の話題を扱って、そのようなことに興味がある人たちのお役に立てればと思っております。

地磁気センサ校正のための楕円体の方程式について

アイキャッチ

はじめに

マルチコプターの方向を推定する。加速度センサ、角速度センサ、地磁気センサのうち、 特に地磁気センサを使用するためには校正が必要なため、校正をするソフトを構築する際の 楕円体に関する知識と、校正のための知識を整理したいと思います。

地磁気データの校正の必要性

3次元で図示するのは大変なので2次元での説明で省略させていただきますが、 地磁気センサをz軸周りに回転して得られたx軸とy軸のデータをxy平面にプロットすると 図1になることを期待します。

図1 理想の地磁気データ
図1 理想の地磁気データ

飛行中に得られた地磁気からのデータが円周上にあるとすると、原点から、その点へ引いた直線の角度が地磁気センサの傾きです。

図2 実際の地磁気データ
図2 実際の地磁気データ

しかし、実際は図2の様に中心が原点になく、斜めに傾いた楕円(楕円体)としてデータが得られることがほとんどです。 このまま地磁気から方向を推定するととんでもない方向を出してくることになります。 したがって、この楕円(楕円体)の斜めを真っ直ぐにし、中心を原点にし楕円(楕円体)を円(球)にする作業が必要となり それを校正と呼びます。

楕円体の方程式

楕円体の方程式は以下の様になります。


\begin{eqnarray}
a_{11} x^2 + a_{22} y^2 + a_{33} z^2 + 2 a_{12} x y + 2 a_{23} y z + 2 a_{13} z x  + b_1 x + b_2 y +b_3 z +c = 0
\end{eqnarray}

行列ベクトル形式で書き直すと以下の様になります。


\begin{eqnarray}
\begin{bmatrix}
x & y & z
\end{bmatrix}
\begin{bmatrix}
a_{11} & a_{12} & a_{13}\\
a_{12} & a_{22} & a_{23}\\
a_{13} & a_{23} & a_{33}
\end{bmatrix}
\begin{bmatrix}
x \\ y \\ z
\end{bmatrix}
+
\begin{bmatrix}
b_1 & b_2 & b_3
\end{bmatrix}
\begin{bmatrix}
x\\ y\\ z
\end{bmatrix}
+c=0
\end{eqnarray}

ここで、記述の簡略化のため、上記の行列やベクトルを次の様に書き換えます。


\begin{eqnarray}
{\boldsymbol{x}}&=&
\begin{bmatrix}
x & y & z
\end{bmatrix}^\mathsf{T}\\
{\boldsymbol{A}}&=&
\begin{bmatrix}
a_{11} & a_{12} & a_{13}\\
a_{12} & a_{22} & a_{23}\\
a_{13} & a_{23} & a_{33}
\end{bmatrix}\\
{\boldsymbol{B}}&=&
\begin{bmatrix}
b_1 & b_2 & b_3
\end{bmatrix}
\end{eqnarray}

以上から、楕円体の方程式は少し簡単に記述できて、次の様に記述できます。


\begin{eqnarray}
{\boldsymbol{x}}^\mathsf{T} {\boldsymbol{A}} {\boldsymbol{x}} + {\boldsymbol{B}} {\boldsymbol{x}} + c =0 
\end{eqnarray}

楕円の回転と行列の対角化

ここで話している楕円体は座標空間に対して傾いていても良いのですが、地磁気センサの校正という作業ではその傾きを明らかにしなければなりません。 そのためには以下のように行列の対角化の知識を用います。

行列の対角化

以下の様に、適当な直交行列{\boldsymbol{P}}を用いると、行列{\boldsymbol{A}}は対角行列{\boldsymbol{\Lambda}}に対角化できます。


\begin{eqnarray}
{\boldsymbol{P}}^\mathsf{T} {\boldsymbol{A}}{\boldsymbol{P}}={\boldsymbol{\Lambda}}
\end{eqnarray}

適当な直交行列{\boldsymbol{P}}と言いましたが、これは行列{\boldsymbol{A}}の固有値と固有値に対応する大きさ1の固有ベクトルによって作ることができます。

{\boldsymbol{A}}はここでは3次の実対称行列ですので、必ず実数の3つの固有値が得られます。それらを\lambda_1\lambda_2\lambda_3とし、それらの固有値に対応する大きさ1の固有ベクトル(列ベクトルとします。)をそれぞれ{\boldsymbol{p}_1}{\boldsymbol{p}_2}{\boldsymbol{p}_3}とします。そうしますと、行列{\boldsymbol{P}}は以下の様になります。


\begin{eqnarray}
{\boldsymbol{P}}=
\begin{bmatrix}
{\boldsymbol{p}}_1 & {\boldsymbol{p}}_2 & {\boldsymbol{p}}_3
\end{bmatrix}
\end{eqnarray}

また、対角行列{\boldsymbol{\Lambda}}は固有値を使って次の様になります。


\begin{eqnarray}
{\boldsymbol{\Lambda}}=
\begin{bmatrix}
\lambda_1 & 0&0\\
0&  \lambda_2 & 0\\
0&0& \lambda_3
\end{bmatrix}
\end{eqnarray}

これから楕円の方程式の1項目は次の様になります。


\begin{eqnarray}
{\boldsymbol{x}}^\mathsf{T} {\boldsymbol{A}} {\boldsymbol{x}} = {\boldsymbol{x}}^\mathsf{T}  {\boldsymbol{P}}{\boldsymbol{\Lambda}}{\boldsymbol{P}}^\mathsf{T}   {\boldsymbol{x}}  
\end{eqnarray}

ここで、楕円体表面の点群を意味する{\boldsymbol{x}}{\boldsymbol{P}^\mathsf{T}}を用いて回転します。


\begin{eqnarray}
{\boldsymbol{x}}^\prime = {\boldsymbol{P}}^\mathsf{T}{\boldsymbol{x}}
\end{eqnarray}

この新たな{{\boldsymbol{x}}^\prime}を用いて、楕円体の方程式を書き直してみると、以下の様なものになります。


\begin{eqnarray}
{{\boldsymbol{x}}^\prime}^\mathsf{T} {\boldsymbol{\Lambda}} {\boldsymbol{x}}^\prime + {\boldsymbol{B}} {\boldsymbol{P}} {\boldsymbol{x}}^\prime + c =0 
\end{eqnarray}

上記の式は元の式とほぼ同じ形をしていますが{\boldsymbol{\Lambda}}は対角行列なのでxyyxzxのクロスタームが消えています。

楕円体の中心と並行移動

ここで、わかりやすくするために(?)上式を少し展開してみます。


\begin{eqnarray}
\lambda_1 {x^\prime}^2 + \lambda_2  {y^\prime}^2 + \lambda_3 {z^\prime}^2  
+ {\boldsymbol{B}}{\boldsymbol{p}_1} x^\prime + {\boldsymbol{B}}{\boldsymbol{p}_2} y^\prime +{\boldsymbol{B}}{\boldsymbol{p}_3} z^\prime +c = 0
\end{eqnarray}

この式を平方完成すると、楕円体の中心がわかります。ただ、平方完成は式操作が煩雑なので、 上式の左辺を各変数で偏微分したものを0とおいて方程式を解くことで、中心座標が以下の様に求まります。


\begin{eqnarray}
{x^\prime}_c&=& -\frac{{\boldsymbol{B}} {\boldsymbol{p}}_1 }{2 \lambda_1}\\
{y^\prime}_c&=& -\frac{{\boldsymbol{B}} {\boldsymbol{p}}_2 }{2 \lambda_2}\\
{z^\prime}_c&=& -\frac{{\boldsymbol{B}} {\boldsymbol{p}}_3 }{2 \lambda_3}\\
\end{eqnarray}

以下は平方完成しても求められるのですが、中心座標を明確にした記述に書き換えると以下の式が得られます。


\begin{eqnarray}
\lambda_1 \left( {x^\prime}^2 + \frac{{\boldsymbol{B}} {\boldsymbol{p}_1} }{2 \lambda_1} \right)^2
+\lambda_2 \left( {y^\prime}^2 + \frac{{\boldsymbol{B}} {\boldsymbol{p}_2} }{2 \lambda_2} \right)^2
+\lambda_3 \left( {z^\prime}^2 + \frac{{\boldsymbol{B}} {\boldsymbol{p}_3} }{2 \lambda_3} \right)^2\\
=\frac{ ({\boldsymbol{B}} {\boldsymbol{p}_1})^2 }{4 \lambda_1}
+\frac{ ({\boldsymbol{B}} {\boldsymbol{p}_2})^2 }{4 \lambda_2}
+\frac{ ({\boldsymbol{B}} {\boldsymbol{p}_3})^2 }{4 \lambda_3}-c
\end{eqnarray}

ここで楕円体の中心を座標原点に移動させる平行移動を施します。 移動後の楕円体表面の点を{\boldsymbol{X}}としますと、 これまでの話から{\boldsymbol{x}}を回転して{\boldsymbol{x}^\prime}として、その後 平行移動して{\boldsymbol{X}}となることになりますから、数式で表すと次の様になります。


\begin{eqnarray}
{\boldsymbol{X}}={\boldsymbol{P}}^\mathsf{T} {\boldsymbol{x}} + \frac{1}{2} {\boldsymbol{\Lambda}^{-1}} \left( {\boldsymbol{BP}} \right)^\mathsf{T}
\end{eqnarray}

以上から、この{\boldsymbol{X}}を用いた、楕円体の方程式を行列形式で表現すると以下になります。


\begin{eqnarray}
{\boldsymbol{X}}^\mathsf{T} {\boldsymbol{\Lambda}}{\boldsymbol{X}} = \frac{1}{4} {\boldsymbol{BP}} {\boldsymbol{\Lambda}^{-1}} \left( {\boldsymbol{BP}} \right)^\mathsf{T} - c
\end{eqnarray}

展開して書くと以下です。


\begin{eqnarray}
\lambda_1 X^2
+\lambda_2 Y^2
+\lambda_3 Z^2= W
\end{eqnarray}

ここで


\begin{eqnarray}
W=\frac{({\boldsymbol{B}} {\boldsymbol{p}_1})^2 }{4 \lambda_1}
+\frac{({\boldsymbol{B}} {\boldsymbol{p}_2})^2 }{4 \lambda_2}
+\frac{({\boldsymbol{B}} {\boldsymbol{p}_3})^2 }{4 \lambda_3}-c
\end{eqnarray}

です。

楕円や楕円体を表す標準的な方程式の形に書き直すと


\begin{eqnarray}
\frac{X^2}{ \left( \sqrt{\frac{W}{\lambda_1}} \right)^2}
+\frac{Y^2}{ \left( \sqrt{\frac{W}{\lambda_2}} \right)^2}
+\frac{Z^2}{ \left( \sqrt{\frac{W}{\lambda_3}} \right)^2}
= 1
\end{eqnarray}

伸縮を施して球にする

これより、楕円体の各軸方向の半径に当たる値はそれぞれ

  • x軸方向 \sqrt{\frac{W}{\lambda_1}}
  • y軸方向 \sqrt{\frac{W}{\lambda_2}}
  • z軸方向 \sqrt{\frac{W}{\lambda_3}}

楕円体は各軸方向に球が伸縮したものとみなせます。 地磁気センサの校正では、これを半径1の球に直す必要が出てきます。

先程の各軸方向の半径に当たる値の逆数を楕円体の各成分にかけることにより、 楕円体を各軸方向に伸縮して半径1の球に修正します。これらの逆数を正規化定数と呼ぶことにします。

伸縮用の行列を{\boldsymbol{S}}として、次の様に対角成分が正規化定数になっている行列を用意します。


\begin{eqnarray}
{\boldsymbol{S}}=
\frac{1}{\sqrt{W}}
\begin{bmatrix}
\sqrt{\lambda_1} & 0 & 0\\
0&\sqrt{\lambda_2}&0\\
0&0&\sqrt{\lambda_3}
\end{bmatrix}
\end{eqnarray}

伸縮して球に修正した球面での点を{\boldsymbol{X}^\prime}とします。

すると、以下の様に校正されたセンサデータを得ることができます。


\begin{eqnarray}
{\boldsymbol{X}}^\prime={\boldsymbol{S}} \left[ {\boldsymbol{P}}^\mathsf{T} {\boldsymbol{x}} + \frac{1}{2} {\boldsymbol{\Lambda}^{-1}} \left( {\boldsymbol{BP}} \right)^\mathsf{T} \right]
\end{eqnarray}

上記の式を校正式とします。校正式にデータに当てはめることで校正データを得られますので、校正データから、方向を推定します。

おわりに

地磁気センサの校正は楕円体の回転・並行移動・伸縮で行えることがわかりました。

そのため

  • 楕円体方程式の行列形式から係数行列を見出す
  • 係数行列から固有値・固有ベクトルをもとめ対角化行列を作成する
  • 伸縮等に用いる正規化定数をもとめる
  • 校正式にデータを当てはめ校正されたデータを得る

といった手順を使います。

今回は、楕円体の方程式の係数を得られたデータから求める話はしていません。

また、近いうちに最小二乗法の良い練習問題になるかと思いますので、地磁気データを用いて楕円体の方程式を求めてみたいと思います。

では、また!