地磁気センサの逐次較正 - 宇宙電波実験室

勝手に較正してくれたなら

地磁気センサを手に持ってぐるぐるぐるぐる…データを取り直してぐるぐるぐるぐーるぐる…。本記事の読者の中にはこのような経験がある方も多いでしょう。

静置試験で簡単にバイアスを求められる加速度・角速度センサと異なり、地磁気センサの場合にはバイアスを求めるだけでも面倒な作業が必要となります。

具体的にどうするかというと、まずセンサを同じ場所でくるくると回すように動かします。このとき、局所的には地磁気は一定とみなせるので、センサで計測した地磁気ベクトルの軌跡は球体を描きます。センサから見ると地磁気ベクトルはこの球の中心を始点として動いているわけですから、球の中心座標を求めることで各軸のバイアスが求まるわけです。

これを一度やるだけならまだしも、地磁気は周囲の環境によって乱されやすく場所によっても変化してしまうため定期的に較正する必要があります。

というわけで、本記事ではそんな問題をいい感じに解決すべく、文献[1]で比較を行っている3つの逐次較正アルゴリズムを実装してみます。また、いくつかの実験データを用いて逐次較正を行い、実装したアルゴリズムが安定して動作するか確認します。

いざ実装へ

3つの逐次較正アルゴリズムについて

文献[1]で比較しているアルゴリズムは、Suquencial centeredアルゴリズムとEKF（Extend Kalman Filter）およびUKF（Unscented Kalman Filter; 文献中ではUF表記）の3つです。それぞれ、3軸の地磁気センサを対象としてバイアス・スケールファクタ・非直交性補正係数を推定しています。

各アルゴリズムについて、Sequencial centeredアルゴリズムは最小二乗法ベースの手法であり、文献[2]で提案されている地磁気較正アルゴリズムの逐次計算版のようです。逐次更新式の詳しい導出は文献[3]に記載されています。また、EKFおよびUKFはSequential centeredアルゴリズムの考え方をEKFとUKFのアルゴリズムに当てはめたものであり、文献[1]中では総合的に見てUKFが最も正確かつロバストという結論になっています。

実装例

文献[1]に記載されている式をプログラムに落とし込んだだけなので、各アルゴリズムの詳細は文献[1]をご覧ください。

前述したように文献[1]のアルゴリズムでは非直交性補正係数の推定を行っています。しかし、これは製造時に較正を行っているセンサであれば十分小さい値であり使用環境によって変化するもではない[4]ので、本記事では計算量を減らすために推定対象から外して実装しました。

本記事中で使用しているプログラムは以下のリンクより閲覧できます。

https://github.com/HamaguRe/space-denpa_sequential_mag_calibration

また、非直交性補正係数の推定を含めた実装も一応あって、以下のリンクより閲覧できます（記事中では使用していません）。

https://github.com/HamaguRe/space-denpa_sequential_mag_calibration_full

補足

なぜか文献[1]中には直接書かれていませんが、サンプル時刻$( k $)における較正された地磁気計測値$( \boldsymbol{C}_k $)は

$[ \begin{align*} \boldsymbol{C}_k &= (I + D) \boldsymbol{B}_k - \boldsymbol{b} \\ &= (\boldsymbol{B}_k - \boldsymbol{b}) + D \boldsymbol{B}_k, \quad k=1,2,\dots,N \end{align*} $]

として求まります。ここで、$( \boldsymbol{B}_k $)は生の地磁気計測値を表し、$( \boldsymbol{b} $)が各軸のバイアスをまとめたベクトルを表します。 $( D $)は対角成分に各軸のスケールファクタが入る対称行列であり、非直交性補正を行う場合には非対角成分に非直交性補正係数が入ります。

上式からわかるようにセンサにスケールファクタ誤差が存在しない場合には$( D $)の対角成分は$( 0 $)となります。スケールファクタなんだから誤差が無ければ$( 1 $)になるだろと思ってしまいがちですが、観測モデルの定義より$( 0 $)になりますのでご注意ください。

また、非直交性補正をなくした関係で一部の行列とベクトルが変わっています。といっても全く別物になってしまうわけではなくて、ベクトルは先頭3要素、行列は左上の3×3行列だけ取り出せばokです。

あとは各行列のサイズを合わせれば実装できます。

実行結果

各アルゴリズムのパラメータ設定

今回実装したような推定アルゴリズムで（ロバスト性とか精度という意味で）良い推定結果を出すためには、ハイパーパラメータを適切に決定する必要があります。制御屋さんは設計すると言ったりしますね。

この"適切な"ハイパーパラメータの決め方ですが、ぶっちゃけ設計者の裁量で適当に決めている場合が多い気がします。ノイズの仮定も実際には少し違ったりしますし、もしノイズが仮定通りだったとしてもロバスト性を高めるためにあえてノイズ分散を大きめに見積もったりします。

遺伝的アルゴリズムで最適なパラメータを探すみたいなやり方もありますが、流石にそれは面倒なので今回は私の裁量でいい感じに決めることにしました。逐次較正という特性上、途中で発散されるとマズいのである程度ロバストな設計を目指します。

今回実装した3種類のアルゴリズムで共通しているハイパーパラメータは、観測ノイズの共分散（NOISE_VAR）と地磁気ベクトルのノルム（FIELD_NORM）および誤差共分散行列の初期値（p_init）の3つです。 UKFの場合にはここにシグマポイントの拡げ具合を調整するパラメータ（ALPHA）が加わります。事前に計測したデータでシミュレーションしつつ、それぞれ以下のように決定しました。

NOISE_VAR = $( 0.04 $)
FIELD_NORM = $( 0.47 \mathrm{[G]} $)
p_init = $( \mathrm{diag}(0.1, 0.1, 0.1, 0.001, 0.001, 0.001) $)
ALPHA = $( 0.1 $)

ただし、地磁気ベクトルのノルム（FIELD_NORM）に関しては、国土地理院が公開している2020年1月1日時点のデータ[5]より関東付近の全磁力を調べて上記の値としました。スケールファクタを推定しているのでこの値は大体で良いです。

誤差共分散行列の初期値（p_init）がハイパーパラメータかというと微妙な気もしますが、極端にずれた値を指定すると発散してしまいますし、特にUKFの場合には誤差共分散行列がシグマポイントの配置に影響するためハイパーパラメータとして扱っています。

較正用に計測したデータ

それでは実際の地磁気データを使ってちゃんと逐次較正できるか確認していきましょう。

まずは通常地磁気センサの較正を行うときに使用するような、計測値が球面上に満遍なく分布したデータを使用してシミュレーションを行います。

以前LSM9DS1（STMicroelectronics製のMEMS 9軸センサ）を較正するときに取得したデータが残っていたので、今回はそれを使用します。以下の図は各軸の地磁気計測値をプロットしたグラフです。

計測値の2Dグラフ

センサをくるくる回しながら計測したので常に値が変動していることがわかります。

また、以下の図は各サンプル時刻における地磁気計測値を点として3Dプロットしたものです。

計測値の3Dプロット

地磁気ベクトルのノルムが一定なので計測値が球面状に分布していることが確認できます。

上図では分かりづらいですが、中心座標が大体

$[ [X_0, Y_0, Z_0] = [-0.247, 0.269, -0.680] $]

で半径が約$( 0.411 $)の球となっています（単位はGauss）。ただし、球の中心座標と半径は文献[6]を参考にして最小二乗法で計算しました。

このような計測データに対して逐次較正を行った結果、推定したパラメータの時間推移は以下のようになりました。

逐次較正結果(2D)

図の上段と下段はそれぞれバイアスとスケールファクタの時間推移を表しており、左の列からSequential centeredアルゴリズム・EKF・UKFの推定結果となっています。バイアスは大体同じ値に収束していますが、スケールファクタはアルゴリズムによって結構差がありますね。このデータの場合、正しく推定できていればスケールファクタは正の値を取るはずです。

また、生データ（較正前のデータ）と逐次較正したデータを3Dプロットすると以下のようになりました。

逐次較正結果(3D)

上図の青色の点が生データ、オレンジ色の点が逐次較正したデータを表し、赤色の点が座標系の原点を表しています。つまり、正しく較正できていればオレンジ色の点は赤色の点を中心とした球を描きます。これは画像だとわかりにくいので、先に載せたプログラムを実行して確認してみてください。

さて、上記の結果の妥当性についてですが、今回の場合バイアスはともかくスケールファクタの真値がわかりません。そこで、生データが描く球と、逐次計算の最終ステップにおけるパラメータで較正したデータが描く球の中心座標及び半径を比較して推定結果の妥当性を確認します。評価方法としてどうなのかという気がしないでもないですが、本記事の目的はあくまで動作確認なのでご了承ください。この辺りの詳細な結果は原著論文[1]に纏められています。

逐次計算の最終ステップの推定パラメータを用いて較正した地磁気データが描く球の半径および中心座標は以下の通りです（最小二乗法[6]で計算）。

	Centered	EKF	UKF
半径	0.407	0.461	0.452
中心座標[x, y, z]	[3.07e-4, 4.02e-3, -5.17e-3]	[-4.32e-3, -1.59e-2, -3.69e-2]	[1.55e-2, 1.05e-3, -1.25e-2]

パラメータを正しく推定できていれば半径は0.47、中心座標は全て0となります。球の半径に注目するとCenteredはEKF・UKFに比べて悪い結果となっていますが、中心座標はEKF・UKFよりも一桁良い精度が出ています。

この精度で十分かどうかは使用用途によるのでなんとも言えませんが、どのアルゴリズムも逐次計算の割にはそこそこ良い結果を出しているのではないかと思います。

少し厳しい条件のデータ

先程使用したデータは計測値が球面上に満遍なく分布しており、逐次較正アルゴリズムにとってはかなり易しい条件でした。

しかし、実際にはそんなに都合よくセンサが動くとは限りません。そこで、もう少し厳しい条件のデータで試してみます（データ自体は手でスマホを動かして計測しました）。

以下の図は各軸の地磁気計測値をプロットしたグラフです。

計測値の2Dグラフ

各サンプル時刻における地磁気計測値を3Dプロットすると以下のようになります。

計測値の3Dプロット

先程のデータと異なり、今回のデータは計測値が球面上の一部にしか分布せず、時系列で見るとほとんど値が変化しない期間があることがわかります。

これまた図からはわかりにくいですが、このデータは中心座標が大体

$[ [X_0, Y_0, Z_0] = [2.32 \times 10^{-4}, 4.92 \times 10^{-5}, 1.66 \times 10^{-3}] $]

で半径が約0.487の球の一部を成しています（単位はGauss）。先ほどと同様に、中心座標と半径は文献[6]を参考にして最小二乗法で計算しました。

このような計測データに対して逐次較正を行った結果、推定したパラメータの時間推移は以下のようになりました。

逐次較正結果(2D)

また、生データ（較正前のデータ）と逐次較正したデータを3Dプロットすると以下のようになりました。

逐次較正結果(3D)

上図の青色の点が生データ、オレンジ色の点が逐次較正したデータを表し、赤色の点が座標系の原点を表しています。 UKFは較正したデータがおかしくなってしまっていますね。バイアス・スケールファクタの推定値も他の2つのアルゴリズムと大きく異なっています。

続いて、逐次計算の最終ステップにおけるパラメータで較正したデータが描く球の中心座標及び半径を計算して推定結果の妥当性を確認します。先程と同様に最小二乗法[6]で計算すると、以下のようになりました。

	Centered	EKF	UKF
半径	0.486	0.476	0.455
中心座標[x, y, z]	[2.32e-4, 4.92e-5, 1.66e-3]	[9.32e-4, -3.96e-4, 6.65e-3]	[1.80e-2, 2.31e-1, 1.34e-1]

このデータの場合も中心座標だけで見るとCenteredが最も良い結果となっています。とはいえ、半径を加味して考えるとEKFもそこそこバランス良い結果です。一方で、UKFは中心座標のズレが無視できないほど大きく残っています。変な値に収束してしまっているのでしょうか…。

Sequential Centeredアルゴリズムの問題点（？）

前述のように、少なくとも中心座標で評価した場合に関してはSequencial centeredアルゴリズムが最も良い結果を出しました。これだけなら「とりあえずSequencial centeredアルゴリズム使っとけば良いんじゃない」と言えるのですが、色々と試しているうちに、長時間動かした場合に較正データがおかしくなってしまうことに気づきました。

例えば、『較正用に計測したデータ』に対して500回繰り返し逐次計算を行った結果は以下のようになります（全てプロットすると見えなくなるので、下図では最後の1回分の結果のみプロットしています）。

500回繰り返した場合の較正データ

明らかにSequencial centeredアルゴリズムで較正したデータがおかしいことがわかります。これを数値で見ると、以下のようになります。

	Centered	EKF	UKF
半径	0.2291	0.4696	0.4695
中心座標[x, y, z]	[3.35e-5, -1.30e-4, 4.15e-5]	[6.37e-4, -5.11e-4, -6.65e-4]	[6.79e-4, -4.92e-4, -6.00e-4]

同じデータに対して繰り返し計算を行った結果、EKFとUKFではより高精度な結果が得られているのに対して、Sequencial centered アルゴリズムでは半径が小さくなってしまっています。アルゴリズムとしては徐々に中心座標が0かつ半径0.47(FIELD_NORM)に収束していくのが自然だと思うのですが…。

この現象について言及している文献はないかと思い文献[1]を引用している文献をいくつか読んでみたのですが、私が調べた限りではそれらしいことが書いてある文献は見つかりませんでした。

因みに他のデータセットに対して同じことをやっても同様の現象が起こり、なんとなくですがNOISE_VARを小さくするほど最終的な半径が小さくなっていく感じがします。実装が間違ってるんですかねぇ。でも何回も確認したし同じ理解度で実装したEKF・UKFはちゃんと収束しているし…。

しかしまあ、原因がわからないのでこの結果だけでSequencial Cenetered アルゴリズムが悪いとは言えません。本記事で使用した実装ではこのような現象が起きたという事実だけ記載しておきます。

所感

本記事では文献[1]中で比較を行っている3つのアルゴリズムを実装して動作確認を行いました。記事中に掲載した内容以外にも色々と弄ってみましたが、個人的にはEKFが一番扱いやすい印象を抱きました。精度も良く安定しています。

UKFは文献[1]中で最もロバストかつ高精度という結論になっていますが、Sequencial centered アルゴリズムや EKFに比べるとハイパーパラメータの決め方がかなりシビアで、特に誤差共分散行列の初期値（p_init）は何回も調整して適切な値を探る必要がありました。

そのため、UKFはロバスト性どうこう以前にそもそもちゃんと動かない可能性があり、私自身ここで苦労した（実装ミスだと思っていて原因究明に数週間かかった）のであまり良い印象がありません…。まあ私が苦労したからというのは個人的な理由過ぎるので聞き流してもらって構いませんが、今回の結果を見る限りはEKFで十分というか、EKFのほうが良いのではないかと思います。計算量も少ないですし。

因みに、今回実装したアルゴリズムでは、通常のEKFで逆行列計算が必要となる部分がスカラーになるので逆行列計算は不要です。これはマイコン上で動かしたい人にとってはかなり嬉しいポイントですね。

というわけで、実装して動かしてみたという記事でした。これで地磁気センサの較正作業から開放されるぞ〜！記事の最初の方に実装例を載せてあるのでぜひ使ってみてください（そんでもってSequencial centered アルゴリズムの実装ミスを見つけたら教えてください）。

参考文献

John L. Crassdis, Kok-Lam Lai, Richard R. Harman, “Real-Time Attitude-Independent Three-Axis Magnetometer Calibration”, Journal of Guidance, Control, and Dynamics, Vol.28, No.1, 2005.
Robert Alonso, Malcolm D.Shuster, “Complete Linear Attitude-Independent Magnetometer Calibration”, The Journal of the Astronautical Sciences, Vol.50, No.4, 2002.
John L. Crassidis, F. Landis Markley, E. Glenn Lightsey, “Global Positioning System Integer Ambiguity Resolution Without Attitude Knowledge”, Journal of Guidance, Control, and Dynamics, Vol.22, No.2,1999.
Hüseyin Yiğitler, M. Kemal Leblebicioğlu, “Online Calibration of Strapdown Magnetometers”, IFAC Proceedings Volumes, Vol.42, Issue.16, pp.687-692, 2009.
国土地理院ウェブサイト, 磁気図（https://www.gsi.go.jp/buturisokuchi/menu03_magnetic_chart.html）, 2024-01-15閲覧.
j_rocket_boy, 球面フィッティングの導出と実装（https://www.slideshare.net/j_rocket_boy/fitting-88311197）, 2024-01-15閲覧.