すほーいログ

140字以上のものを残しておくために開設しました

くり込み法による幾何学推定と磁気センサのSoft-Iron Offset補正 (2軸)

Matlab ロボット

磁気センサはHard-Iron offsetとSoft-Iron offsetの2種類のオフセットが載った状態で出力されます．

Hard-Iron offsetは補正しないと使い物にならないので補正の話は山のように出てきます．

一方でSoft-Iron offsetの補正は細かいアルゴリズムがなかなか話に出てきません．出てこないので自前でアルゴリズムを作ってみました．ついでにせっかくなのでブログのネタにしました．

中身のレベルとしては基本的な補正のみなので，補正したいだけの場合は既存のライブラリ探して使ったほうがたぶん性能は良いです．ここに書いてるのは勉強用ってことで．

f:id:Fragmented:20190820170557p:plain
こんな感じで補正できます

目次

やったこと

  • 磁気センサのSoft-Iron offset補正
    • 2軸・平面回転での動作を前提
    • 初期化プロセスで収集した十分な数のサンプリング点を基にオフセット補正パラメータを計算
    • 動的なキャリブレーションはとりあえず考えない

キャリブレーションの流れ

  1. センサを平面回転させて十分な量のサンプリング点を収集
  2. サンプリング点に対して楕円でフィッティング（幾何学的推定）
  3. フィッティング結果から楕円パラメータを求める
  4. 楕円パラメータを基にオフセット補正パラメータに変換
  5. 以降はサンプリング毎に補正が可能

磁気センサそのもののお話

大体↓にまとまってます．神ですね． myenigma.hatenablog.com

磁気センサ

  • センサ周囲の磁気強度を測れる
  • だいたい3軸積んでる
  • 一般的には地磁気を読んで方位角を出力する
  • 2軸センサとして使用も可能
    • 2軸の場合は平面移動で使用する
    • 平面回転させて出力をプロットすると理想状態では真円となる
  • 3軸使うと3次元移動する物体の姿勢推定に使える
    • ぶんぶん振り回して出力をプロットすると理想状態では真球となる
    • 加速度センサでロール・ピッチを算出してヨー角を地磁気センサに頼る話は非常に多くの文献およびブログが存在
    • 3軸地磁気センサで方位角を算出する際はロール・ピッチ情報必須なので必然的に加速度センサと一緒にパッケージングして販売されることが多い（あとジャイロも積んで合計9軸ってのが常）

磁気センサのオフセット

  • 磁気センサのオフセットてなんやねん．という話
  • 参考文献張っておくけど一応説明します

www.nxp.com

ez.analog.com

Hard-Iron offset

  • センサ周囲に存在する磁気物体によるオフセット
  • あとセンサ内部のバイアスも含む
  • プロットした際に円（球）の中心がずれる原因
  • サンプリング点持ってきて平均したり(最大値-最小値)/2とかで求められる
    • 今回紹介するアルゴリズムはそこまで乱暴なことはしないので安心してくれ（？）

Soft-Iron offset

  • センサ周囲に透磁率の高い物質が存在することによるオフセット
  • あとそもそもセンサそのものの各軸の感度が異なることによっても発生
  • プロットした際に円（球）が歪んで楕円（楕円体）となる原因

幾何学的推定

  • 直線とか楕円とか，ある幾何学的な拘束条件に基づいてそのパラメータを推定すること
  • 有名どころで行くと最小二乗法
    • 実際は他にもいろいろ手法がある
  • 日本語資料だと↓にまとまっている

幾何学推定のための最適化手法：最小化を超えて http://www.iim.cs.tut.ac.jp/~kanatani/papers/hypertutor.pdf

幾何学的推定の準備

一般的な楕円方程式から2次形式へ変換

幾何学的推定を適用するには一般的な円/楕円の方程式の形から2次形式へ変換してやる必要があるのでその準備

楕円の2次形式標準系は次の通り

 
\begin{eqnarray}
Ax^2 + 2Bxy + Cy^2 + 2Dx + 2Ey + F = 0
\label{eq-1}
\end{eqnarray}

参考文献に基づいた書き方だと

 
\begin{align}
&(\boldsymbol{ \xi }(\boldsymbol{ x }), \boldsymbol{ \theta }) = 0 \notag \\
&\boldsymbol{ \xi }(\boldsymbol{x} ) \equiv \boldsymbol{ \xi }(x, y) \equiv (x^2, 2xy, y^2, 2x, 2y, 1)^{ \mathrm{ T } } \label{eq-2} \\
&\boldsymbol{\theta} \equiv (A, B, C, D, E, F)^{ \mathrm{ T } } \notag
\end{align}

上式における \boldsymbol{\theta} \equiv (A, B, C, D, E, F)^{ \mathrm{ T } } を求めることが楕円あてはめの幾何学推定となります．

以下に一般的な楕円の方程式からこの2次形式を得る過程を書いておきます．

楕円方程式

皆さんご存じ．x軸長さa，y軸長さbとして

 \begin{eqnarray}
\frac {x^2}{a^2} + \frac{y^2}{b^2} = 1
\end{eqnarray}

2次形式標準系は次の通り

 \begin{eqnarray}
\boldsymbol{\theta} \equiv  (\frac 1{a^2}, 0, \frac 1{b^2}, 0, 0, -1)^{ \mathrm{ T } } \label{eq-eli}
\end{eqnarray}

傾いた楕円の方程式

高校数学の範囲らしいです．

傾き \phiとして


\displaystyle
\frac{(x\cos \phi + y \sin \phi) ^2}{a^2}
+ 
\frac{(-x\sin \phi + y \cos \phi) ^2}{b^2}
= 1

2次形式にすると


\left( \frac {\cos ^2 \phi}{a^2} + \frac {\sin ^2 \phi }{b^2} \right) x^2
+ \left( \frac1{a^2}-\frac 1{b^2} \right) \cos \phi \sin \phi \ xy
+ \left( \frac {\sin ^2 \phi}{a^2} + \frac {\cos ^2 \phi }{b^2} \right) y^2
-1=0

2次形式標準系では

 \begin{eqnarray}
\boldsymbol{\theta} \equiv 
 \left( \frac {\cos ^2 \phi}{a^2} + \frac {\sin ^2 \phi }{b^2},
 \left( \frac1{a^2}-\frac 1{b^2} \right) \cos \phi \sin \phi,
\frac {\sin ^2 \phi}{a^2} + \frac {\cos ^2 \phi }{b^2}
, 0, 0, -1 \right) ^{ \mathrm{ T } }
\label{eq-eli-tan}
\end{eqnarray}

係数がちょっと長いので次のようにおくと

 \begin{align}
A&=\frac {\cos ^2 \phi}{a^2} + \frac {\sin ^2 \phi }{b^2} \label{eq-A} \\
B&= \left( \frac1{a^2}-\frac 1{b^2} \right) \cos \phi \sin \phi \label{eq-B} \\
C&=\frac {\sin ^2 \phi}{a^2} + \frac {\cos ^2 \phi }{b^2} \label{eq-C}
\end{align}
 \begin{eqnarray}
A x^2 + 2B xy + C y^2 = 1 \label{eq-eli-tan2}
\end{eqnarray}

と書き換えができます

中心が原点にない傾いた楕円の方程式

楕円中心 (x_o, y_o)として2次形式で表した傾いた楕円の方程式に代入


A (x - x_o)^2 + 2B (x - x_o)(y - y_o) + C (y - y_o)^2 = 1

展開して


A x^2
+ 2B xy
+ C y^2
+ 2(-Ax_o - Bx_o)x
+ 2(-B x_o - C y_o)y
+ (A x_o^2 + 2Bx_o y_o + C y_o^2 - 1)
 = 0

第4項～6項を

 
\begin{align}
D&=(-Ax_o - By_o) \label{eq-D} \\
E&=(-B x_o - C y_o) \label{eq-E} \\
F&=(A x_o^2 + 2Bx_o y_o + C y_o^2 - 1) \label{eq-F} 
\end{align}

とおいて

 
A x^2 + 2B xy + C y^2 + 2Dx + 2Ey + F = 0

冒頭\eqref{eq-1}の2次形式が得られました．

ノイズのモデル化（共分散行列の算出）

  • 最小二乗法はノイズモデルに基づかないため不要
  • 最尤推定ほかサンプソン誤差の最小化に基づいた幾何学推定アルゴリズムを用いる場合は必要

計算してみる

共分散行列は次式で計算できます

 \begin{align}
V_0[\boldsymbol{\xi}_{\alpha}]
= \left. \frac{\partial \boldsymbol{\xi}(\boldsymbol{x})}{\partial \boldsymbol{x}}\right|_{\boldsymbol{x}=\boldsymbol{x}_{\alpha}}
V_0[\boldsymbol{x}_{\alpha}]
\left. \frac{\partial \boldsymbol{\xi}(\boldsymbol{x})}{\partial \boldsymbol{x}}\right|_{\boldsymbol{x}=\boldsymbol{x}_{\alpha}}^{ \mathrm{ T } }
\end{align}

ここで V_0 [ \boldsymbol{x}_{\alpha} ]は正規化共分散と呼ぶもので， \boldsymbol{x}に関するノイズの方向依存性を表す行列です．一様等方性（独立したノイズ）であれば単位行列 \boldsymbol{I}_n です．

添え字 \alphaはサンプリング点を意味します． 得られたn 個のデータのうち各点 \boldsymbol{x}_{\alpha} = (x_{\alpha}, y_{\alpha})に対応する列ベクトル\boldsymbol{\xi}(\boldsymbol{x}_{\alpha}) \equiv (x_{\alpha}^2, 2x_{\alpha}y_{\alpha}, y_{\alpha}^2, 2x_{\alpha}, 2y_{\alpha}, 1)^{ \mathrm{ T } }です．

ヤコビ行列を計算して

 \begin{eqnarray}
V_0[\boldsymbol{\xi}]=
\begin{pmatrix}
4x^2 & 4xy       & 0    & 4x & 0  & 0 \\
4xy  & 4x^2+4y^2 & 4xy  & 4y & 4x & 0 \\
0    & 4xy       & 4y^2 & 0  & 4y & 0 \\
4x   & 4y        & 0    & 4  & 0  & 0 \\
0    & 4x        & 4y   & 0  & 4  & 0 \\
0    & 0         & 0    & 0  & 0  & 0 
\end{pmatrix}
\end{eqnarray}

幾何学推定のアルゴリズム

参考文献では

  • 最小化に基づく方法
    • 最小二乗法 (Least squares)
    • 最尤推定 (Maximum likelihood estimation)
    • FNS法 (Fundamental numerical sceme)
    • 超精度補正
  • 最小化に基づかない方法
    • 重み反復法 (Iteratively reweighted least squares)
    • くりこみ法 (Renormalization)
    • 超精度くりこみ法 (Hyper-Renormalization)

というように分類・紹介されています． 最小二乗法・重み反復法・くり込み法はコーディング（Matlab）したのでそれらについてアルゴリズムを抜粋します*1

最小二乗法


\boldsymbol{M} = \displaystyle \sum_{ \alpha = 1 }^{ n  } \boldsymbol{\xi}_{\alpha} \boldsymbol{\xi}_{\alpha}^{\mathrm{T}}

\boldsymbol{M}の最小固有値に対する単位固有ベクトルを\boldsymbol{\theta}として幾何学推定解とする

重み反復法

  1. サンプリング点に対する重み W_{\alpha} = 1， \alpha=1 \ldots n ， \boldsymbol{\theta}_0 = \boldsymbol{0}とおく
  2. 次の行列\boldsymbol{M}を計算する 
    
\boldsymbol{M} = \displaystyle \sum_{ \alpha = 1 }^{ n  } \boldsymbol{W}_{\alpha} \boldsymbol{\xi}_{\alpha} \boldsymbol{\xi}_{\alpha}^{\mathrm{T}}
  3. 固有値問題 
    
\boldsymbol{M} \boldsymbol{\theta}=\boldsymbol{\lambda} \boldsymbol{\theta}
    を解いて最小固有値\boldsymbol{\lambda}に対する単位固有ベクトル\boldsymbol{\theta}を計算する
  4. 符号を除いて\boldsymbol{\theta} \approx \boldsymbol{\theta}_{0}なら\boldsymbol{\theta}を解として終了 そうでなければ次のように重みを更新してステップ2に戻る 
    
W_{\alpha} \leftarrow \displaystyle \frac{1}{(\boldsymbol{\theta},V_0[ \boldsymbol{ \xi } _{\alpha} \boldsymbol{ \theta } ])}, \ \ \  \boldsymbol{\theta}_{0} \leftarrow \boldsymbol{\theta}

くり込み法

  1. サンプリング点に対する重み W_{\alpha} = 1， \alpha=1 \ldots n ， \boldsymbol{\theta}_0 = \boldsymbol{0}とおく
  2. 次の行列\boldsymbol{M}，\boldsymbol{N}を計算する 
     \begin{align*}
\boldsymbol{M} &= \sum_{ \alpha = 1 }^{ n  } \boldsymbol{W}_{\alpha} \boldsymbol{\xi}_{\alpha} \boldsymbol{\xi}_{\alpha}^{\mathrm{T}} \\
\boldsymbol{N} &= \sum_{ \alpha = 1 }^{ n  } \boldsymbol{W}_{\alpha} V_0[\boldsymbol{\xi}_{\alpha}]
\end{align*}
  3. 固有値問題 
    
\boldsymbol{M} \boldsymbol{\theta}=\boldsymbol{\lambda} \boldsymbol{N} \boldsymbol{\theta}
    を解いて絶対値最小の固有値\boldsymbol{\lambda}に対する単位固有ベクトル\boldsymbol{\theta}を計算する
  4. 符号を除いて\boldsymbol{\theta} \approx \boldsymbol{\theta}_{0}なら\boldsymbol{\theta}を解として終了 そうでなければ次のように重みを更新してステップ2に戻る 
    
W_{\alpha} \leftarrow \displaystyle \frac{1}{(\boldsymbol{\theta},V_0[ \boldsymbol{ \xi } _{\alpha} \boldsymbol{ \theta } ])}, \ \ \  \boldsymbol{\theta}_{0} \leftarrow \boldsymbol{\theta}

磁気センサのオフセット補正

幾何学推定結果から楕円パラメータへの変換

幾何学推定結果 \boldsymbol{\theta} \equiv (A, B, C, D, E, F)^{ \mathrm{ T } } から楕円パラメータ（長軸長さa，短軸長さb，x軸と長軸のなす角\phi，中心(x_o, y_o)）へ変換します．

\eqref{eq-A} \eqref{eq-B} \eqref{eq-C} \eqref{eq-D} \eqref{eq-E} \eqref{eq-F}式をa, b, \phi, x_o, y_oについて解く感じですね．

中心位置

\eqref{eq-D} \eqref{eq-E}を連立して(x_o, y_o)について解きます．

\begin{eqnarray}
\left\{    \begin{array}{l}
-Ax_o-Bx_o = D \\
-Bx_o-Cy_o = E  \end{array}  \right.
\end{eqnarray}

逆行列を使ってパパッとできます．

\begin{eqnarray}
\left(  \begin{array}{c}  x_o \\ y_o  \end{array}\right)
=
\begin{pmatrix} -A & -B \\ -B & -C \end{pmatrix} ^ {-1}
\left(  \begin{array}{c}  D \\ E  \end{array}\right)
\end{eqnarray}

傾き

\eqref{eq-eli}，\eqref{eq-eli-tan}を見ると，傾いた楕円はxy項があることがわかります．

傾いた楕円の2次形式について対角化する過程で傾きを得ることが出来ます．

http://www.math.s.chiba-u.ac.jp/~yasuda/Chiba/Lec/senkei23U8.pdf より

 \begin{eqnarray} 
\large{ \phi } = 
\begin{cases}
\displaystyle \frac{1}{2} \tan ^{-1} \frac{2B}{A-C} & (\mathrm{if} A \neq C) \\
\displaystyle \frac{\pi}{4} & (\mathrm{if} A = C)  \end{cases} \label{eq-phase}
\end{eqnarray}

ここで得られる傾きはx軸と長軸のなす角ではなく，x軸と楕円の長軸・短軸のうち近傍軸のなす角度なので，長軸・短軸の長さを求めた後に手を加える必要があります．

定数倍不定性の除去

\eqref{eq-1}式のような陰関数形式には定数倍の不定性があります．

幾何学推定では |(A, B, C, D, E, F)^{ \mathrm{ T } } | = 1とすることで不定性を除去しており，\eqref{eq-A} ~ \eqref{eq-F}式で求まる係数とは異なるので，これらを相互に変換する定数倍係数を求めます．

楕円パラメータを求めるには必要な過程ですが，オフセット補正行列を求めるだけなら不要です（定数倍不定性が消えるので）．

幾何学推定結果を (A', B', C', D', E', F')^{ \mathrm{ T } }，定数倍係数nとすると，

 \begin{eqnarray}
n(A'x^2 + 2B'xy + C'y^2 + 2D'x + 2E'y + F')=Ax^2 + 2Bxy + Cy^2 + 2Dx + 2Ey + F = 0 \label{ff-1}
\end{eqnarray}

という関係が得られます．

\eqref{ff-1}式が中心 (x_o, y_o)の図形であることから， x \leftarrow x + x_o, y \leftarrow y + y_oとすることで中心を原点に移動させます．

 \begin{eqnarray}
&n(A'(x+x_o)^2 + 2B'(x+x_o)(y+y_o) + C'(y+y_o)^2 + 2D'(x+x_o) + 2E'(y+y_o) + F')=0 \notag \\
&\Rightarrow n(A'x^2 + 2B'xy + C'y^2) + n(2A'x_o + 2B'y_o + 2D')x + n(2B'x_o + 2C'y_o + 2E')y + n(A'x_o^2 + 2B'x_oy_o + C'y_o^2 + 2D'x_o + 2E'y_o + F') = 0 \label{ff-2}
\end{eqnarray}

ここで，\eqref{ff-2}は中心原点の傾いた楕円であること，つまり\eqref{eq-eli-tan2}式の形になることから，

 
n(A'x_o^2 + 2B'x_oy_o + C'y_o^2 + 2D'x_o + 2E'y_o + F') = -1
,

よって定数倍係数は

 \begin{eqnarray}
n =\displaystyle \frac{-1}{A'x_o^2 + 2B'x_oy_o + C'y_o^2 + 2D'x_o + 2E'y_o + F'} 
\end{eqnarray}

となり，

 \begin{eqnarray}
\begin{matrix}
A &\leftarrow nA' \\
B &\leftarrow nB' \\
C &\leftarrow nC' \\
D &\leftarrow nF' \\
E &\leftarrow nE' \\
F &\leftarrow nF'
\end{matrix} 
\end{eqnarray}

とすることで楕円パラメータを求めるための定数倍不定性が除去できました

長軸・短軸

対角化から計算できなくはないんですが傾きとの整合性が取りづらいので\eqref{eq-A} \eqref{eq-C}をa，bについて解きます．

 \begin{eqnarray} 
\large{a} &= \sqrt{ \left| \frac{ \cos 2\phi }{ C(\cos ^2 \phi -1) + A \cos ^2 \phi } \right|  } \\
\large{b} &= \sqrt{ \left| \frac{ \cos 2\phi }{ A(\cos ^2 \phi -1) + C \cos ^2 \phi } \right|  }
\end{eqnarray}

傾きの修正

傾きを求める項でも述べましたが，\eqref{eq-phase}式はx軸と楕円の長軸・短軸のうち近傍軸のなす角度なので手を加える必要があります．

Matlabだとこんな感じです．MathJaxとはてなのソースコードシンタックスハイライトが同居できなかったんでハイライトはなしです．

if a < b
    swap = a;
    a = b;
    b = swap;
    if phi> 0
        phi = phi - 0.5*pi;
    else
        phi = phi + 0.5*pi;
    end
end

オフセット補正行列

幾何学推定したり，楕円パラメータ求めたりと長かったですね．ここから本題です．

ちなみに本題は一瞬で終わります．

Hard-Iron offset補正ベクトル

楕円中心を原点に持ってくるだけです．

楕円中心 (x_o, y_o)としてHard-Iron offset補正ベクトルHは

 \begin{eqnarray} 
H = \left(  \begin{array}{c}
-x_o \\
-y_o
  \end{array} \right)
\end{eqnarray}

Soft-Iron offset補正行列

回転した楕円について，回転行列で回転→短軸長さを長軸長さに合わせる→逆回転 させるような行列を作ってやればよいです．

長軸と短軸の比sとして（ここで比を取ることで定数倍係数が消えるので，補正するだけなら定数倍不定性を考える必要はないんですよね）

 \begin{eqnarray} 
s = \frac{a}{b}
\end{eqnarray}

Soft-Iron offset補正行列は

 \begin{eqnarray} 
S = \begin{pmatrix}
\cos \phi & -\sin \phi \\
\sin \phi & \cos \phi
  \end{pmatrix}
\begin{pmatrix}
\cos \phi & \sin \phi \\
-s\sin \phi & s\cos \phi
  \end{pmatrix}
\end{eqnarray}

補正

磁気センサデータ(m_x, m_y)^\mathrm{T}，補正済み磁気センサデータ(m'_x, m'_y)^\mathrm{T}として次式で補正が出来ます．

 \begin{eqnarray} 
\left(  \begin{array}{c}
m'_x \\
m'_y
  \end{array} \right)
 = S \left( 
\left(  \begin{array}{c}
m_x \\
m_y
  \end{array} \right)
+ H
\right)
\end{eqnarray}

f:id:Fragmented:20190823193128p:plain

Matlab Code

CurveFittingSample.mを実行したら乱数で作った楕円が補正される様が見れます．

github.com

*1:文献中のNは\boldsymbol{N}と混同を避けるためnに変更しています