楕円体のフィッティングと中心座標の導出

主な用途

地磁気センサのバイアス値を補正するため。
手順は以下の通り。

1. 地磁気センサのデータを収集
2. 取得データを楕円体でフィッティング
3. 楕円体の中心座標 (バイアス値) を導出
4. データを補正

本記事では、楕円体のフィッティングと中心座標の導出過程を記載する。

楕円体の一般式

今回は、**『回転の無い楕円体』**について考える。(拡大縮小、平行移動がある楕円体)
その楕円体の一般式は下記のように表される。

\frac{(x-x_0)^2}{a^2}+\frac{(y-y_0)^2}{b^2}+\frac{(z-z_0)^2}{c^2}=1 \\

式中の $a, b, c$ は各軸の半径、$x_0, y_0, z_0$ は各軸の平行移動 (中心座標) を意味する。
バイアス値は平行移動の量と同値なため、後に $x_0, y_0, z_0$ を導出する。

さて、このままの形では扱いづらいため、一般式を書き換え下記のように変更する。
(展開して係数や定数をまとめただけ)

Ax^2+By^2+Cz^2+Dx+Ey+Fz-1=0 \\

楕円体フィッティング

楕円体のフィッティングにおいて、最小二乗法を採用する。
コスト関数を下記に記す。

f_{(A,\cdots,F)} = \sum_{n=1}^{N}(Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1)^2 \\

コスト関数が最小となるよう、コスト関数を微分して = 0 となるように A～F を決定する。

\frac{\partial f_{(A,\cdots,F)}}{\partial A} = 0 \quad, \quad\cdots\quad,\quad \frac{\partial f_{(A,\cdots,F)}}{\partial F} = 0 

コスト関数の微分

編微分自体は特に難しくはない。

\begin{align}
\frac{\partial f_{(A,\cdots,F)}}{\partial A} & = 2\sum_{n=1}^{N}(x_n^2 (Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1)) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial B} & = 2\sum_{n=1}^{N}(y_n^2 (Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1)) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial C} & = 2\sum_{n=1}^{N}(z_n^2 (Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1)) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial D} & = 2\sum_{n=1}^{N}(x_n (Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1)) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial E} & = 2\sum_{n=1}^{N}(y_n (Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1)) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial F} & = 2\sum_{n=1}^{N}(z_n (Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1)) = 0 \\
\end{align} \\

数式が多いと面倒なので、行列として書き換える。
(以降、n の記載を省略)

2\sum
\begin{pmatrix}
x^4 & x^2y^2 & x^2z^2 & x^3 & x^2y & x^2z \\
x^2y^2 & y^4 & y^2z^2 & xy^2 & y^3 & y^2z \\
x^2z^2 & y^2z^2 & z^4 & xz^2 & yz^2 & z^3 \\
x^3 & xy^2 & xz^2 & x^2 & xy & xz \\
x^2y & y^3 & yz^2 & xy & y^2 & yz \\
x^2z & y^2z & z^3 & xz & yz & z^2 \\
\end{pmatrix}
\begin{pmatrix}
A \\ B \\ C \\ D \\ E \\ F \\
\end{pmatrix}
- 2\sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}
= 0 \\

別の計算過程を見る (ゴチャゴチャしてるよ)

\begin{align}
g_{(A,\cdots,F)} & = Ax_n^2+By_n^2+Cz_n^2+Dx_n+Ey_n+Fz_n-1 \\
f_{(A,\cdots,F)} & = \sum_{n=1}^{N}(g_{(A,\cdots,F)})^2 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial A} 
& = 2\sum_{n=1}^{N}(\frac{\partial g_{(A,\cdots,F)}}{\partial A} g_{(A,\cdots,F)}) \\
& = 2\sum_{n=1}^{N}(x_n^2 g_{(A,\cdots,F)})
\end{align}

同様に、B～F の偏微分も簡単に記載できる。

\begin{align}
\frac{\partial f_{(A,\cdots,F)}}{\partial A} & = 2\sum_{n=1}^{N}(x_n^2 g_{(A,\cdots,F)}) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial B} & = 2\sum_{n=1}^{N}(y_n^2 g_{(A,\cdots,F)}) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial C} & = 2\sum_{n=1}^{N}(z_n^2 g_{(A,\cdots,F)}) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial D} & = 2\sum_{n=1}^{N}(x_n g_{(A,\cdots,F)}) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial E} & = 2\sum_{n=1}^{N}(y_n g_{(A,\cdots,F)}) = 0 \\
\frac{\partial f_{(A,\cdots,F)}}{\partial F} & = 2\sum_{n=1}^{N}(z_n g_{(A,\cdots,F)}) = 0 \\
\end{align} \\

g_{(A,\cdots,F)} = 
\begin{pmatrix}
x^2 & y^2 & z^2 & x & y & z \\
\end{pmatrix}
\begin{pmatrix}
A \\ B \\ C \\ D \\ E \\ F \\
\end{pmatrix}
- 1 \\

\begin{align}
\begin{pmatrix}
\frac{\partial f_{(A,\cdots,F)}}{\partial A} \\
\frac{\partial f_{(A,\cdots,F)}}{\partial B} \\
\frac{\partial f_{(A,\cdots,F)}}{\partial C} \\
\frac{\partial f_{(A,\cdots,F)}}{\partial D} \\
\frac{\partial f_{(A,\cdots,F)}}{\partial E} \\
\frac{\partial f_{(A,\cdots,F)}}{\partial F} \\
\end{pmatrix}
= \begin{pmatrix}
2\sum(x^2 g_{(A,\cdots,F)}) \\
2\sum(y^2 g_{(A,\cdots,F)}) \\
2\sum(z^2 g_{(A,\cdots,F)}) \\
2\sum(x g_{(A,\cdots,F)}) \\
2\sum(y g_{(A,\cdots,F)}) \\
2\sum(z g_{(A,\cdots,F)}) \\
\end{pmatrix}
& = \begin{pmatrix}
0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0
\end{pmatrix} \\

2 \sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}
g_{(A,\cdots,F)} 
& = \begin{pmatrix}
0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0
\end{pmatrix} \\

2 \sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}
\begin{pmatrix}
x^2 & y^2 & z^2 & x & y & z \\
\end{pmatrix}
\begin{pmatrix}
A \\ B \\ C \\ D \\ E \\ F \\
\end{pmatrix}
- 2 \sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}
& = \begin{pmatrix}
0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0
\end{pmatrix} \\

2 \sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}
\begin{pmatrix}
x^2 & y^2 & z^2 & x & y & z \\
\end{pmatrix}
\begin{pmatrix}
A \\ B \\ C \\ D \\ E \\ F \\
\end{pmatrix}
& = 2 \sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}

\end{align} \\

最終的に、**A～F は下記の計算で導出**できる。

\begin{pmatrix}
A \\ B \\ C \\ D \\ E \\ F \\
\end{pmatrix}
= \begin{pmatrix}
\sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}
\begin{pmatrix}
x^2 & y^2 & z^2 & x & y & z \\
\end{pmatrix}
\end{pmatrix}^{-1}
\begin{pmatrix}
\sum
\begin{pmatrix}
x^2 \\ y^2 \\ z^2 \\ x \\ y \\ z
\end{pmatrix}
\end{pmatrix} \\

中心座標の導出

フィッティングした楕円体の中心座標 $x_0, y_0, z_0$ を導出する。
一般化パラメータ A～F が求まれば、$x_0, y_0, z_0$ は簡単に求まる。

\begin{pmatrix}
x_0 \\ y_0 \\ z_0 \\
\end{pmatrix}
= \begin{pmatrix}
2A & 0 & 0 \\
0 & 2B & 0 \\
0 & 0 & 2C \\
\end{pmatrix}^{-1}
\begin{pmatrix}
-D \\ -E \\ -F \\
\end{pmatrix}

まとめ

以上で、楕円体フィッティングおよび中心座標の導出が完了。
上記の数式をPythonなりC++なり好きな言語で実装すればよい。

楕円体でなく球体ならば、一般式のパラメータが A=B=C となるので、もう少し簡単になる。

参考ページ

http://www.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/tech0091.html
https://teratail.com/questions/98155
https://www.slideshare.net/j_rocket_boy/fitting-88311197