- カルマンフィルターは、ノイズが多い、または不正確な測定値から より正確な状態推定値 をリアルタイムに生成し、センサーベースの予測や制御に使われるアルゴリズムである
- 単に測定値を滑らかにするだけでなく、直接観測しにくい 位置・速度のような状態変数 まで推定できる
- レーダー追跡の例では、2D位置測定値と共分散を入力として受け取り、物体の 位置と速度 を 4×1 の状態ベクトルとして推定する流れを示している
- 最初の2つの測定値で状態を初期化した後は、予測、カルマンゲイン (Kalman Gain) の計算、状態と誤差共分散の更新を繰り返す
- フィルターが保持する履歴は現在の状態推定値1つだけなので、メモリ制約のある装置 でも実装しやすい
カルマンフィルターの全体像
- カルマンフィルターは、入力と出力を持つ ブラックボックス のように見なせる
- 入力はノイズを含む、あるいはときに不正確な測定値である
- 出力はノイズが減り、より正確になった推定値である
- 推定値は、実際には測定または観測されていない システム状態パラメータ である場合がある
- 観測可能なパラメータと観測不可能なパラメータを リアルタイム に高い精度で推定するために使われる
- 高精度の推定値は、精密な予測や意思決定に活用される
- そのため、信頼できる情報が必要な ロボティクス やリアルタイムシステムでよく使われる
カルマンフィルターが解決する問題
- 不正確またはノイズを含む測定値をもとに、変数の状態、あるいは直接観測しにくい別の変数をより高い精度で推定する
- 代表的な応用は次のとおり
- 物体追跡: 測定された位置を使って、物体の位置と速度をより正確に推定する
- デジタル体重計での体重推定: 表面にかかった圧力から物体の重さを推定する
- 誘導・航法・制御: IMUセンサーで位置、速度、加速度を推定し、次の動きの制御に活用する
- カルマンフィルターの強みは、測定値の平滑化そのものよりも 測定しにくいシステムパラメータ を推定する能力にある
- リアルタイムシステムでは、精度が向上した推定値がより良い制御と機能につながる
レーダー追跡例の入力と出力
- 例では、空港周辺の飛行機や物体を追跡する レーダー追跡 の状況を扱う
- 出力されたトラック状態は、空域を監視する航空交通管制オペレーターに表示するために使われる
- レーダーは 2D デカルト座標系の x, y 位置測定値 を出力する
- 測定値は 2×1 の列ベクトル z で表される
- 測定値の 分散共分散行列 は R で表される
- 測定時刻タグは t で表される
- 添字 m は測定パラメータを、k は測定順序を表す
- カルマンフィルターは、レーダー測定値をもとに物体の 位置と速度 を推定する
- 推定値は 4×1 の列ベクトル x で表される
- 推定値の 分散共分散行列 は 4×4 行列 P で表される
- 状態推定値の時刻タグは T で示される
ステップ1: システム状態の初期化
- カルマンフィルターの 初期化方法 はアプリケーションごとに異なる
- このレーダー追跡の例では、最初の測定値でシステム状態を初期化する
- 入力測定値には位置情報しか含まれないが、出力システム状態には物体の 位置と速度 が含まれる
- 最初の測定値が入ったときに分かる情報は、その時点の位置だけである
- 最初の推定後、システム状態推定値は入力位置に設定される
- システム状態誤差共分散は、最初の測定値の位置精度に設定される
ステップ2: システム状態の再初期化
- 速度推定には2つ目の位置測定値が必要なため、システム状態推定値を 再初期化 する
- 速度は線形近似で計算される
- 速度は移動距離 / 移動にかかった時間で計算される
- 更新されたシステム状態推定値は、2つ目の測定値の位置と計算された速度で構成される
- 更新されたシステム状態誤差共分散には、2つ目の測定値の位置精度と近似した速度精度が入る
- 速度精度の近似値は、フィルターにデータを通した後で チューニング して調整できる
- 例では、速度の分散項を 10⁴ に設定している
- この値は、速度状態値に対する大きな不確実性を表す
- 例での速度単位は m/s である
初期化後の反復処理
- 最初の2ステップは、いくつかの測定値でシステム推定値を初期化し、再初期化する過程である
- アプリケーションごとに初期化方法は異なる場合がある
- 目標は、その後の測定値をカルマンフィルター方程式で更新できる システム状態推定値 を整えることである
- 以降のステップでは、新しい測定値をフィルターに反映し、状態推定値を更新する
ステップ3: システム状態推定値の予測
- 3つ目の測定値が入ると、システム状態推定値を前方に伝播させて測定値と 時間整列 する
- この整列は、測定値と状態推定値を結合するために必要である
- 予測には システムモデル が使われる
- 例では、一定速度の線形運動モデルで時間間隔中の物体の位置変化を近似する
- 状態遷移行列は、この運動方程式を表現する
- この行列は、状態推定値と状態誤差共分散行列を適切に伝播させるために使われる
- 状態推定値を将来時点へ伝播させると、その時点での状態の不確実性が生じるため、誤差共分散は大きくなる
Q行列とH行列
- Q行列 はシステムモデルのプロセスノイズを表す
- システムモデルは近似値である
- システム状態の全期間にわたり、モデルの精度は変動する
- Q行列はこの不確実性を表し、既存の状態ノイズに加えられる
- 例では、実際の加速と減速がこの誤差に寄与する
- H行列 は状態-測定行列で、システム状態推定値を状態空間から測定空間へ変換する
- 一部のアプリケーションでは、0 と 1 で構成された行列である
- 拡張カルマンフィルター (Extended Kalman Filter) を使うアプリケーションでは、微分方程式で埋められる
- 関連説明は Extended Kalman Filters の記事へ続く
- この例の H行列 は、状態推定値と誤差共分散を位置と速度の全体ではなく 位置値だけ 比較できるように縮約する単純な行列である
ステップ4: カルマンゲインの計算
- カルマンフィルターは、新しい測定値ごとに カルマンゲイン を計算する
- カルマンゲインは、入力測定値がシステム状態推定値にどれだけ影響するかを決定する
- 測定値のノイズが非常に大きい場合、カルマンゲインは新しいが不正確な情報よりも現在の状態推定値をより信頼するようになる
- 現在の推定値と新しい測定情報を適切に重み付けして 最適推定値 を作る方法が、カルマンフィルターアルゴリズムの核心である
ステップ5: 状態と誤差共分散の更新
- カルマンフィルターは、カルマンゲインを使って入力測定値時点のシステム状態と誤差共分散行列を推定する
- カルマンゲインは、2つの計算で測定値に適切な重みを与えるために使われる
- 新しい システム状態推定値 の計算
- システム状態 誤差共分散 の計算
- 計算された状態推定値は、カルマンフィルターが保持する唯一の状態履歴である
- この特性により、カルマンフィルターは 厳しいメモリ制約のある装置 でも実装可能である
まとめ
- カルマンフィルターは、最適な状態推定のための 汎用プロセス である
- 正確な推定が必要なさまざまなアプリケーションで使われる
- 小さなステップに分けると、カルマンフィルターの動作をより負担なく理解しやすい構造として捉えられる
1件のコメント
Hacker Newsのコメント
Kalmanフィルタの話で、この優れた学習資料へのリンクは外せない。Jupyter Notebook集として書かれた本: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
この本では、アルファ・ベータフィルタを本格的なKalmanフィルタの弟分のように扱っている。最近の仕事で似たようなものが必要になってかなり読んでいたら、アルファ・ベータフィルタやKalman系は近い未来の予測に重点が強く、実際に必要だったのは過去データの平滑化だったと気づいた。
そこで二重指数平滑を読み始めたところ、ユースケースにぴったり合いそうに見え、掘り下げるほど、結局は変数名が違うだけのアルファ・ベータフィルタだと分かった。
この数学分野全体は、いくつかの共通した基礎理論の上に成り立っているのに、異なる分野が別々のアプローチで同じシステムに到達するため、名前や表記が異なり、共通性が見えにくくなっているように感じる。冪級数、自然定数、勾配降下法、フィルタ、フィードバックシステム、一般システム理論のようなものの中心には、比較的小さな直感の核があり、それをつかめば多くの数学が見事につながるのではないかと思う。
紙を節約したり、書きやすくしたりするためではない。英語キーボードでLaTeXに上付き・下付き付きのギリシャ文字を書くのは、単語を書くより簡単ではない。読者に、別の方法では伝えにくい特定の情報を伝えようとしているのだ。
数学記法は文字のように見えるが、本質的にはかなり図的な表記なので、長い単語はその図を隠してしまう。
制御や信号/システムの講義をきちんと受けると、そうした直感が身につき、特定の表記慣習にこだわらずに数学と用語を学べる。本当の直感は「すべてはフィルタである」で、残りはそのアイデアを解析し合成する作業だ。
AI/MLが大きくなるにつれて、古典制御理論のアイデアも強化学習と融合しつつある。
たいていQとRは「見た目がよくなるまで」手で調整した後、再び変えないからだ。しかも、QとRという複数の値を調整する代わりに、ゲイン一つだけを手で合わせればよい。
測定値のノイズが常に同じなら、時間がたつにつれて定数に収束し、測定精度をよく把握できる一方で大きく変動する場合にこそ特に有用になる。
最近Kalmanフィルタの実装を担当したが、開発者である自分にも理解できる言葉で説明した良い資料を見つけるのが本当に難しかった。1か月ほど学んだあと記事を2本書いたので、誰かの役に立つかもしれない: https://www.splinter.com.au/2023/12/14/the-kalman-filter-for..., https://www.splinter.com.au/2023/12/15/the-kalman-filter-wit...
開発者の立場からすると皮肉なことに、自分で実装して初めて数学が理解できた。すでに知っていることの上に積み上げながら学ぶやり方のようだが、こういうものに付く用語があるのか気になる。
数学は説明的な変数名を使えば、ずっと学びやすくなるといつも思っていた。少なくともWebのようなインタラクティブな媒体なら、最低限ツールチップくらいは入れられる。数学を勉強するとき、時間の90%を記号を調べることに使っている
それにここで下付き文字が「測定の順序を表す」と言うとき、どの順序を指しているのか混乱する。たぶんインデックスという意味だと思う。Kalmanフィルタを触ってからずいぶん経つけれど
数学が純粋になるほど、ほとんどの変数のスコープは短くなる。たいてい変数は使う直前に定義され、スコープもその証明や導出の中を超えない
ただし、この記事の選択の中には単に変なものもある。たとえば P を変数とインデックスに同時に使っていたり、精度行列が共分散行列の正確な逆行列なのに P を共分散行列に使っていたりする点だ
研究論文でいちばん腹立たしい部分が、まさにこの曖昧さだ。必ず明確にすべきだ。相対的な表現ではなく、絶対的な説明を使うべきだ。「右を見よ」と言ってはいけない。私は左を見るかもしれない
「プリズムを回転させた後、光円錐が...」と書くと、その回転がどの方向なのかが抜けている。左なのか右なのか、水平なのか上下なのか、速いのか遅いのか。書き手の頭の中にはこうした重要な要素がすべて描かれているが、読者はその心を読めない
下付き文字「p」はおそらく prediction、つまり予測を意味していると思う。時刻3の x_p は時刻4で期待する状態だ。しかし実際に時刻4が来ると、新しい測定値を反映して x_4 を計算する
はっきり言えば、この x_4 は時刻3で計算した x_p とは違う。予測は常に少しずつ外れるからだ
問題は、始めたばかりの人がまだ概念や各問題領域の標準表記法を身につけていないことで、そのため新しいトピックに乗る苦痛が残る
Kalmanフィルタは、数学で単純化したバージョンがあまりにも単純化されすぎて、元の姿がほとんど見えなくなる特異な例のひとつかもしれない
実際にやっていることは、測定値を1つ得て、あり得る未来の状態をシミュレーションし、その情報を次の測定値と結合し、これを繰り返すことだ
たとえばテニスボールの写真を何枚も撮り、最初の写真から位置と速度を推定したうえで、どこへ行くかをシミュレーションし、次の写真と比較してどの推定が真実により近いかを見るようなものだ。もっと古い方法としては、太陽の高度を測って地図上に可能な位置の線を作り、もともと自分がいると思っていた地点と比較するやり方も思い浮かべられる
もちろん正確な計算は現実的にはほぼ不可能だ。そこで標本抽出で単純化する。それでも難しいので、分布がある程度ガウス分布に近いと仮定する。そこからさらに単純化して、システムの変化が線形変換だと仮定すると、ここで扱っているKalmanフィルタになる
線形代数だけを見て、実際に何が起きているのか理解できるなら驚きだと思う
この説明こそ、初めて実際に何が起きているのかについての直感を与えてくれた。コンピュータ寄りの人間である自分には、名前自体もかなり誤解を招く。ストリーム処理やSQLで言うフィルタではないからだ
Kalmanフィルタは、測定値を生み出した隠れた入力を推定しようとする。そのために、
xについて [実際の測定値 - 期待測定値(x)]^2/s^2 を最小化する問題を作る。ここでsはノイズのシグマだこれは、
xについて実際の測定値を見る尤度を最大化する状態推定問題から出てくる。尤度関数で重要な項は-([x-expected(x)]/s)^2だけだからだ。正規分布、あるいは実質的に任意の指数分布の指数部を見ればよい実際の測定値は定数なので、期待測定値関数が線形なら凸最適化としてすぐ解ける。微分して0に等しいと置いて解けば、Kalmanフィルタの更新ステップが出てくる関数が非線形なら、方程式を線形化して最小化した後、その「偽の線形化」の解を返すNewton-Raphsonの1ステップを実行しているようなものだ。学部レベルの微積分と線形代数なのに、誰もそう言ってくれなかっただけだ
しかしこれは完全に間違ってもいる。1960年代に出てきた、この種の再帰的な単一ステップ線形化で尤度関数を最大化しようとするハックだ。Cramér-Rao下限を誤読したせいで、何世代ものエンジニアがこれを最適だと信じてきたが、実際にはそうではない
今は計算資源が1万倍は多く、非線形方程式を複数のNewton-Raphsonステップとして立てて解いたり、長い測定履歴を保持してその束全体を複数のNewton-Raphsonステップで解いたり、多峰性の測定関数を受け入れるためにガウス混合表現を使ったりすれば、より良い性能が出る
こうしたものは1980年代から現在までの状態推定研究でよく扱われてきたが、教科書は今なお1972年に石に刻まれたかのようだ
Cramér-Rao下限は、すべての測定尤度関数が真の状態で線形化される場合にのみ定義されるが、これはすべての測定値を保持する束において漸近的にしか可能でなく、無限の過去時点では不可能で、再帰フィルタでも不可能だ
以前の説明では、この記事がよかった: https://www.bzarg.com/p/how-a-kalman-filter-works-in-picture..., https://news.ycombinator.com/item?id=13449229
本当にKalmanフィルタが必要なら、この記事やWikipediaの記事、あるいは実装ソース（https://github.com/LdDl/kalman-rs/blob/master/src/kalman/kal...）を読めば理解できるとは思う
ただ、経験上、世界中のほぼすべての人は、例で学ぶのがいちばん得意な視覚的学習者だ。だから、ページ中ほどのチュートリアルが数式に具体的な数字を入れていない点には驚く。見落としているのかもしれないが、図も「Kalmanフィルタとは何か」というテキストが1ページ出てきた後でようやく始まり、その図でさえさらに多くの数式になっている
こういうことがあまりにもよくあるので驚くほどだ。もっと軽い形では、コード例を含まないソフトウェアライブラリにも同じ問題が現れる
自分にしっくりきたのは、不確実な測定分布が2つ、つまり分散の大きい分布を組み合わせると、より確実な測定、つまりより狭い分布になるという点だ
このより確実な測定値を次の測定値とまた結合し、それを繰り返すと、そのままKalmanフィルタになる
KFを説明する動画群の中では、これは本当によかった。元記事の資料より理解しやすかった: https://www.youtube.com/watch?v=CaCcOwJPytQ
亡くなった父は、このフィルタが発明されたばかりの頃からキャリアを通じてよく使っていた。レーダーとミサイル誘導システムを扱っていた
目を閉じて少し歩き回ってみて。自分がどこにいるか想像してから目を開けると、実際の位置は思っていた位置と違っているだろうか？
その最後のステップ、つまり観測を使って状態変数に対する信念を更新することが、Kalmanフィルタのすることだ