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

アイキャッチ

はじめに

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

はじめに
地磁気データの校正の必要性
楕円体の方程式
楕円の回転と行列の対角化
- 行列の対角化
楕円体の中心と並行移動
伸縮を施して球にする
おわりに

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

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

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

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

楕円体の方程式

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

$\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}}$ の固有値と固有値に対応する大きさ１の固有ベクトルによって作ることができます。

${\boldsymbol{A}}$ はここでは３次の実対称行列ですので、必ず実数の３つの固有値が得られます。それらを $\lambda_1$ 、 $\lambda_2$ 、 $\lambda_3$ とし、それらの固有値に対応する大きさ１の固有ベクトル（列ベクトルとします。）をそれぞれ ${\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}$

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

$\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}}$ は対角行列なので $xy$ 、 $yx$ 、 $zx$ のクロスタームが消えています。

楕円体の中心と並行移動

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

$\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}$

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

$\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}}$

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

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

伸縮用の行列を ${\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}$