- 現実の位置・速度測定は常に揺らぐため、カルマンフィルタは複数の不完全な情報源を組み合わせて、より信頼できる状態推定値を作る仕組みとして理解できる
- 船の位置の例では、エンジン速度 10m/s というモデルは風や波のために外れることがあり、GPS のようなセンサーもノイズや障害のため常に正確とは限らない
- サンプルコードでは 1000 人の乗客がそれぞれ速度ベースの推定とセンサー測定を作り、2 つの値を加重平均で組み合わせることでカルマンフィルタの直感を示す
- 情報源の信頼度は測定値の分散で計算し、ばらつきの大きい側はあまり信用せず、一貫した側をより強く反映するために
1/variance を使う
- センサーが正常なときはセンサー値をより重視し、
t=3・t=6 のように揺らぐ区間では自動的に影響が小さくなり、より安定した推定値を維持する
カルマンフィルタが必要な状況
- カルマンフィルタは、複数のノイズを含む情報源を 1 つのより正確な統計値に圧縮する漏斗のようなものと見なせる
- 数学的には線形代数、確率論、微積分が関係するが、ここでは完全な理論よりも直感に焦点を当てる
- 船が港の
x=0 から出発し、1 次元に移動し、エンジンが 10m/s の一定速度を与えると仮定する
- 理想的な世界なら 2 秒後の位置は
2 * 10 = 20m だが、現実の世界ではエンジン・風・波のため、速度や位置は正確に保たれない
- したがって、位置の公式だけでは実際の船の位置を確信するのは難しい
センサーも完全ではない
- GPS のようなセンサーがあれば、特定時点の位置を直接測定できるが、センサーの測定値も常に正確とは限らない
- GPS は 3 秒時点に 29.998m、30.002m のような実際の位置に近い値を返すこともあれば、ごくまれに 100m のように大きく外れた値を返すこともある
- 衛星カバレッジのない区域では、GPS センサーは事実上機能しない場合がある
- センサーが決してオフラインにならず、任意の精度で望む値を測定できるなら、カルマンフィルタは不要である
- カルマンフィルタは、速度ベースの位置推定、GPS 推定、レーダー、ソナーなど複数の情報源を組み合わせ、位置をより正確に推定できる
コードで見る位置推定
- 例では船に 1000 人の乗客がおり、各乗客が自分の GPS デバイスを持っている状況を仮定する
- 各乗客はまず、前回の位置に速度と外部要因の変動を加えて新しい位置を推定する
from random import gauss
def new_position(last):
velocity = 10
wind = gauss(0, 2)
wave = gauss(0, 0.1)
return last + velocity + wind + wave
gauss は正または負のランダム値を作り、2 番目の引数は変動の大きさを表す- 風と波の効果を直接測定できないため、サンプルでは平均 0、標準偏差
2 と 0.1 を持つ乱数でノイズをモデル化する
- 2 段階目では、センサーが実際の位置にセンサーノイズを加えた測定値を返す
def sensor(t):
if t == 3:
# oops, passing through a thunderstorm. GPS fluctuating!
sensor_noise = gauss(5, 10)
elif t == 6:
# uh-oh, satellite unavailable!
sensor_noise = gauss(-5, 10)
else:
sensor_noise = gauss(0, 1)
return true_position[t] + sensor_noise
t=3 では雷雨のため GPS が揺らぎ、t=6 では衛星を利用できない状況をモデル化している- 同じ時点でも、乗客ごとにセンサー測定値は異なる
実際の移動経路と単純平均
true_position = [0, 9, 19.2, 28, 38.1, 48.5, 57.2, 66.2, 77.5, 85, 95.2]
- 船は港の
x=0 から出発し、1 秒後に 9m、2 秒後に 19.2m、その後はリストの値のように移動する
- 乗客の目標は、ノイズがあり信頼できない測定値から各秒の位置をできるだけ正確に予測することだ
t=1 で、ある乗客の速度ベース推定が 9.37、センサー測定が 8.98 なら、単純平均は 9.17 となる- 実際の位置が 9m のとき、この単純平均は速度推定より誤差が小さいが、この例のセンサー測定よりは悪い
加重平均と信頼度
def combine(A, B, trustA, trustB):
total_trust = trustA + trustB
return (A * trustA + B * trustB) / total_trust
combine(9.37, 8.98, 10, 1) は速度推定をより信頼するので、結果は 9.37 に近い 9.33 になるcombine(9.37, 8.98, 1, 10) はセンサー測定をより信頼するので、結果は 8.98 に近い 9.01 になる- この信頼度ベースの加重平均がカルマンフィルタの中核となる直感であり、データ結合の力の中心である
- どの情報源をより信頼するかは分散で決まる
- 結論が大きく揺らぐ情報源はあまり信頼しない
- 結論が一貫している情報源はより強く信頼する
- 10 のラジオ局のうち 4 局が雨、6 局が晴れと言い、10 のウェブサイトのうち 9 サイトが雨だと言うなら、ウェブサイト側は分散が低く、より信頼できる
更新ステップ
- 全体の更新では、乗客ごとの速度ベース推定とセンサー測定を作ったあと、2 つの測定集合の分散から信頼度を計算する
from statistics import variance
def update(t, last):
velocity_updates = []
sensor_updates = []
for p in range(1000):
velocity_updates.append(new_position(last[p]))
sensor_updates.append(sensor(t))
fluctuation_velocity = variance(velocity_updates)
fluctuation_sensor = variance(sensor_updates)
trust_velocity = 1 / fluctuation_velocity
trust_sensor = 1 / fluctuation_sensor
combined = []
for p in range(1000):
combined.append(combine(
A=velocity_updates[p],
B=sensor_updates[p],
trustA=trust_velocity,
trustB=trust_sensor
))
return sensor_updates, velocity_updates, combined
- 分散が大きいほど信頼度は低くなるため、
1/variance を使う
- 各乗客は自分の位置を個別に更新する
- すべての乗客の位置更新が終わったあと、船の実際の位置推定は乗客たちの位置の平均から推測できる
結果の読み方
update_plot 関数は、プロットを作るために各時点の実際の位置、センサー推定、速度推定、結合推定を保存する- メインループは、現在乗客たちが持つ最良の推定値を使って、各時点の位置推定を継続的に更新する
- プロットで線の周囲にあるenvelopeは不確実性を意味し、幅が広いほどその値に対する不確実性が大きい
- センサーが正常に動作する
t=0.75 から t=1 の区間では、結合位置推定は速度推定のみを使うより良いが、センサー測定のみを使う場合より悪いことがある
- センサーが故障する
t=2 から t=4 の区間では、結合推定は故障したセンサー測定のみを使う場合より良い結果を示す
- センサーが回復する
t=4 から t=5 の区間では、カルマンフィルタは再びセンサーをより重視し始める
付録: gauss と分散
normal distribution function である gauss(0, 0.1) と gauss(0, 2) は、主に 0 付近のランダムな値を生成する- 2 番目の引数である標準偏差は、測定値がどれだけ揺らぐかを制御する
gauss(0, 0.1) は 0.06, -0.07, 0.02 のように、0 に近い小さな値を返す可能性が高いgauss(0, 2) は 1.05, -1.06, 1.29, -1.72 のように、より広く散らばった値を返す可能性が高い
- サンプルコードでは、風はより大きく変動し、波はより小さく変動すると仮定している
- 分散は一貫性の尺度であり、一貫性が高ければ分散は低く、一貫性が低ければ分散は高い
- 標準偏差が 2 の分布の分散は 4 であり、標準偏差が 0.1 の分布の分散は 0.01 である
1件のコメント
Hacker News の意見
記事は楽しく読めたが、実装が間違っている。最大の誤りは、不確実性を時間軸方向に伝播させていないため、誤差を過小評価している点
グラフにも表れていて、誤差範囲はほとんどの時点で実際の状態を含むべきだが、結果ではそうなっていない
乗客たちが推定値にノイズを加えるのではなく、乗客の立場から見て時点 k の状態が与えられたとき、時点 k+1 の状態の期待値は単に
position_k+1 = position_k + velocity * Delta_tとなる。実際のダイナミクスにはノイズがあり、フィルタではそれを推定共分散に加える形で反映するコードがすぐに破綻しない理由は、1000人の乗客でダイナミクスのサンプルを多数取り、その結果の分散を数値的に計算しているからだが、実務で通常行う方法とはかなり異なる
また、GPS が天候の影響を受けるというのはよくある誤解で、実際にはそうではない。そして記事で使われている一貫性の定義も標準的ではない。推定理論で推定器が一貫的であるとは、データが増えるほど推定値が真値に収束するという意味
一般読者向けにわかりやすく説明している点は良いが、いくつかの誤解が問題を生んでいるように見える。推定理論の大学院生なので、さらに話せば手助けできると思う
大学の信号処理の講義で、なぜ Kalman Filter をこのように単純に教えないのか不思議になるほどだった。数学の概念は数学的に教えるのが正しいが、背景知識が足りない人には情報の欠落が生じる
以前、離散コサイン変換とウェーブレット変換を画像中心に教えたことがあるが、厳密さより先に直感を与えるやり方は、逆の順序より常にうまくいった
教授たちが直感を先に与えない理由はいくつか考えられる。教授本人が直感よりも数値や方程式の操作に深い専門性を持っている場合、教育能力が評価されないため直感的に解きほぐす時間を研究費申請書や博士課程学生の管理に使う場合、そして数学を理解してしまうと直感的な説明のほうがむしろ「遠回り」に感じられ、脳が戻るのを拒む場合
特に三つ目が最大の理由だと思う。これは数学教育を超えて、専門性と教育能力の違いでもある。ゴルフのドライブを学ぶなら、最も遠くへ飛ばす人よりも、100ヤードから始めて安定して300ヤード打てるようになった人に教わるほうがよいかもしれない
Kalman Filter を実装しようとする人への一般的な注意として、数値的不安定性への対処方法については https://www.stat.berkeley.edu/~brill/Stat248/kalmanfiltering... の冒頭数ページを読むとよい
実務ではそうしても目立つ数値的アーティファクトを見たことはあまりないが、かなり魅力的な解決策だ
この記事を補完するものとして、Kalman Filter 系をより徹底的かつ数学的に入門したいなら、この本を強く薦める: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
業務で Kalman Filter を実装する必要があったソフトウェアエンジニアが書いた本なので、概念を動機づけて伝える方法がこの読者層に合っているかもしれない。対話型の Jupyter Notebook として書かれており、リポジトリをクローンして自分で実行しながら追うことができる
単純なフィルタから始め、ベイズの規則を入れ、確率分布へ拡張していく形で段階的に改善していくため、Kalman Filter へのなだらかな導入路を提供している
抜けている側面が一つある。予測値と測定値の加重平均を取るとき、Kalman Filter の重みは時間とともに変わり得る。そうでなければ別の名前で呼ばれると思う
ゆっくり変化する値を単一センサーで測定する例がよい。たとえば燃料計では、秒単位では変化しないと推定するのがよいが、測定値にはタンク内の燃料の揺れのようなノイズがあり得る
この場合、Kalman Filter は指数的に減衰するゲインを持つ一次ローパスフィルタのように見える。カットオフ周波数が変わるため、開始時の水準は数秒以内に素早く見つけ、その後は 0.01Hz のような非常に低いカットオフ周波数でノイズを無視できる
重要なツールについての良い記事
私の理解では、線形 Kalman Filter は線形問題に対する最適解で、比較的理解しやすく実装もしやすい。ただ、私が見た応用のほとんどは非線形だった
Extended Kalman Filter と Unscented Kalman Filter は理解も実装もはるかに難しく、資料やライブラリも少なく、あまり役に立たなかった
例えば小型 UAV 向けの AHRS/GNSS CAN デバイスを扱うとき、PX4 や Ardupilot で見た Extended Kalman Filter は非常に複雑で、パラメータも多かった。そこで、クォータニオンの基本原理から出発し、ジャイロの解を加速度計の「上」方向と磁力計の傾斜ベクトルの方向へ少しずつ補正するやり方のほうが単純だった
加速度の大きさが 1G から大きく外れていたり、磁場ベクトルがその地域の地磁気強度から大きく外れていたりする場合は、そのセンサー更新の重みを下げるかスキップし、ジャイロをそのまま進ませる。おそらく EKF が正解である可能性は高いが、理解しやすく、作りやすく、調整・診断しやすい形にするのは諦めた
ただしクアッドローターでは、回転が大きな難点になる。Kalman Filter の線形モデルはすべてがユークリッド空間にあると仮定するが、回転は多様体上にある。クォータニオンの場合、この多様体は単位クォータニオンの集合になる
クォータニオンを推定しようとして EKF を素朴に適用すると、もはや単位クォータニオンではなくなり、推定値が壊れてしまう。この多様体制約を扱うよく知られた方法はいくつかあるが、コードに落とし込んだ方程式の中では最も見苦しい部類だった
単純な例として、物理法則のため常に単位円上になければならない状態
(x, y)を考えられる。実際のダイナミクスf(x, y)は円上の新しい点を返すが、それを線形化した近似ダイナミクスは単位円上にとどまる保証がなく、非物理的な状態や EKF の状態推定につながり得る検討に値する選択肢として ForneyLab.jl、Infer.net、Gen.jl、Pyro がある
ただし Kalman Filter の使い方は多く、どこから始めるかによっては、非線形変換を正しく扱うことが極端に面倒になり得る
近視と乱視があるのだが、片目を閉じて壁時計のようなものを見ると、それぞれの目が互いに異なる歪んだ画像を作っていることが分かる。両目は少しずつ違う
ところが両目で時計を見ると、画像はずっと鮮明で、どちらか片方の目だけで見るより良い。記事に出てくる、船の乗客 1000 人がそれぞれ自分の GPS 座標を報告するシナリオで、この現象を思い出した
脳も Kalman Filter のような賢いアルゴリズムを広く使っていそうだ
Kalman Filter は、ノイズの混じった観測値から単純平均より良い値の推定をしてくれるもの、と理解すれば合っている？
例えば何かを 3 回測って 7、8、9 が出たら、実際の値は 8 だと推測するだろうけど、Kalman Filter は別の推定値を出すのか？
Kalman Filter は伝統的に、時間とともに動くものを推定するのに使われる。映像内の人物や、ある種のランダムウォークを考えればよい
連続する 2 つの時点や測定値の間に、速度や現在の方向のような関係があると仮定すると、運動モデルの情報とノイズのある測定モデルの情報を混ぜて、位置や値、あるいは移動履歴全体をよりよく推定できる
運動モデルが意味のある形で間違っていれば、推定は改善されない。その後に拡張された多くの手法は、ロボティクスにおける車輪の滑りのような、より精巧な運動モデルを含めることに焦点を当てている
例えば定数を測定するとして、初期不確実性を持つ定数、たとえば標準偏差を持つ Gaussian 分布と、同じく Gaussian ノイズと標準偏差を持つ測定値を基本モデルにできる。推定しようとする定数周辺の初期不確実性と測定値の不確実性を調整できる
この例では Kalman Filter は平均のようには振る舞わない。測定値が良い、つまり不確実性が低ければ素早く収束し、測定値が悪ければ推定値が揺れ、収束により長くかかる
そして Kalman Filter が動くものにだけ使われるというのは事実ではない。定数推定にも常に使われるが、動く対象でより有名なだけ
Kalman Filter は、何らかの線形ダイナミクスのために標本が相関している場合を扱う。測定値が関心対象そのものである必要もなく、その対象の線形関数に Gaussian ノイズが加わった形でもよい
したがって、最初の測定で 7 を見たという知識は、2 回目の測定で 8 を見る確率を変える。上のように標本平均だけを取ると、一般には実際の平均値に収束しない
例で何を出すかは、正確なモデルに依存する。想像できる最も単純なシナリオでさえ、Bayes の場合のように、どの期待値から始めるかを指定する必要がある
その表現を線形 Gaussian モデルとしてエンコードすると、Kalman Filter になる
カルマンフィルターを理解するうえで、もう一つの気づきを与えてくれた短い記事は John D. Cook のノートだった: https://www.johndcook.com/blog/applied-kalman-filtering/
従来の微積分・微分方程式ベースのシステムモデリングは、データに不確実性はなく、すべてがシステムモデルに含まれると考える一方で、データ駆動の推定器は「すべてはデータの中にある」と見なし、データを生成する物理過程のモデルを完全に無視する
カルマンフィルターの美しさは、この2つのアプローチを組み合わせる点にある
可視化に優れた別の記事もある: https://www.bzarg.com/p/how-a-kalman-filter-works-in-picture...
HN にはすでに3回投稿されていた記事