1. はじめに

　カルマンフィルタは、制御工学やロボティクス、さらには金融分野まで幅広く活用されている強力な推定アルゴリズムです。このアルゴリズムは、アポロ計画で宇宙船の軌道計算に使われたことで有名ですね。
　当時の宇宙船に搭載されていたコンピュータはファミコンと同程度のスペック(RAMはファミコンの倍)でしたが、そのようなコンピュータでも精度の高い軌道計算を実現できるくらいシンプルで効果的なアルゴリズムなのです。
　しかし、初学者にとっては何をやっているのか掴みにくい難解な理論や数式が出てきます（私も今回学び直してやっと理解できました...）今回はカルマンフィルタについて、そもそも何をやっているのか数式的な説明は極力省いて、わかりやすくまとめてみようと思います。

2. そもそもなぜカルマンフィルタが必要なのか

　現実世界で何かを計測しようとする時、必ずつきまとうのが「ノイズ」です。センサーの精度限界、外乱の影響など、様々な要因によって、私たちが得られる観測データは真の値からズレてしまうことがほとんどです。
　例えば、ドローンの位置をGPSで計測する場合、電波状況によって誤差が生じることがあります。また、工場の生産ラインで製品の温度を測る場合でも、周囲の環境変化によって測定値が変動することが考えられます。
　このようなノイズを含む観測データだけを頼りに判断していては、誤った結論を導き出してしまう可能性があります。そこで登場するのが、カルマンフィルタです。

3. カルマンフィルタの基本的な考え方

　カルマンフィルタは、「予測ステップ」と「更新ステップ」を繰り返すことで、ノイズの影響を低減し、より正確な状態（例えば、物体の位置や速度、システムの内部状態など）を推定します。

3.1 予測ステップ

　まず最初に、システムの過去の状態に基づいて、次の時刻の状態を予測します。この予測には、システムの挙動に関する知識（運動方程式や状態遷移モデル）が用いられます。例えば、自動車の速度と加速度が分かっていれば、次の瞬間の位置をある程度予測できますよね。
　ただし、この予測にも不確実性が伴います。なぜなら、知り得ない(モデル化されていない)外乱や、システム自体のわずかな変動などが存在する可能性があるからです。カルマンフィルタでは、この予測の不確実性を「予測誤差共分散」という形で定量的に扱います。

3.2 更新ステップ

　次に、実際にセンサーなどを用いて観測を行います。しかし、前述の通り、この観測データにもノイズが含まれています。
そこで、カルマンフィルタは、予測された状態と実際の観測データを、それぞれの信頼性（不確実性）に基づいて最適な形で組み合わせます。予測の不確実性が小さければ予測を重視し、観測の不確実性が小さければ観測を重視する、というように、賢く情報を統合するのです。
　この組み合わせによって、より真の値に近い、更新された（フィルタリングされた）状態と、その誤差共分散が得られます。

3.3 予測と更新の繰り返し

　そして、この更新された状態が、次の時刻の予測ステップにおける「過去の状態」として用いられ、予測と更新のサイクルが繰り返されます。
　このように、カルマンフィルタは、過去の情報と現在の観測を逐次的に取り込みながら、時間とともに推定精度を向上させていくのが特徴です。

4. カルマンフィルタの数式

　数式を省いて説明しようと思いましたが、数式を全く説明しないと逆に分かりづらい気がしたので、直感的な理解を優先しつつ、ざっくりと式の意味をまとめてみました。

4.1. 予測ステップ

まず状態をシステムの数理モデルから予測します。

x̂(k|k-1) = F(k-1)x̂(k-1|k-1) + B(k-1)u(k-1)

ここで、
・x̂: 状態ベクトルの予測値。推定したいシステムの状態（例：位置、速度）。
・u: 制御入力ベクトル。システムに既知の入力（例：モーターへの電圧）。
・F: 状態遷移行列。現在の状態から次の状態への遷移を表す行列。
・B: 入力行列。制御入力が状態に与える影響を表す行列。
なお、kは現在時刻を表し、k-1は前回時刻を表します。

次に予測誤差共分散を算出します。

P(k|k-1) = F(k-1)P(k-1|k-1)F(k-1)^T + Q(k-1)

ここで、
・Q: プロセスノイズ共分散行列。システムモデルの不確実性を表す行列。
・P: 予測誤差共分散行列。予測された状態の不確実性を表す行列。

4.2. 更新ステップ

まず、カルマンゲインを算出します。カルマンゲインとは予測と観測のどちらをどれだけ重視するかを決めるゲインです。

K(k) = P(k|k-1)H(k)^T (H(k)P(k|k-1)H(k)^T + R(k))^-1

ここで
・K: カルマンゲイン。
・H: 観測行列。状態から観測値への変換を表す行列。
・R: 観測ノイズ共分散行列。観測データのノイズの大きさを表す行列。

次に推定した状態を更新します。

x̂(k|k) = x̂(k|k-1) + K(k)(z(k) - H(k)x̂(k|k-1))

ここで
・z: 観測ベクトル。センサーから得られた観測データ。

最後に、予測誤差共分散を更新します。

P(k|k) = (I - K(k)H(k))P(k|k-1)

このようにして、予測の不確実性と観測の不確実性を考慮しながら、最適な推定値を導き出しています。

5. カルマンフィルタの特徴

上記式の通り、カルマンフィルタは逐次的な処理が可能であり、新しい観測データが入るたびに推定を更新できるため、リアルタイム処理に適しています。また、統計的な手法に基づいてノイズの影響を効果的に低減できます。さらに、状態の推定値だけでなく、その不確実性（誤差共分散）も同時に評価できます。

6. カルマンフィルタの注意点

非常に強力なアルゴリズムであるカルマンフィルタですが、利用する際にはいくつかの注意点があります。

・システムモデルは正確であるか？

カルマンフィルタの性能は、システムの挙動を記述するモデルの正確性に大きく依存します。不正確なモデルを使用すると、推定精度が低下する可能性があります。

・ノイズ特性をある程度十分に把握しているか？

プロセスノイズと観測ノイズの特性（共分散行列 Q と R）を適切に設定する必要があります。これらの設定が不適切だと、フィルタの性能が十分に発揮されません。

・モデルは線形か？

標準的なカルマンフィルタは、システムモデルと観測モデルが線形であることを前提としています。非線形なシステムに対しては、拡張カルマンフィルタ（EKF）やアンセンテッドカルマンフィルタ（UKF）などのより高度な手法を検討する必要があります。

7.まとめ

　今回は、カルマンフィルタの基本的な考え方についてその概要をまとめました。カルマンフィルタは、ノイズを含む不確実な情報から、最も可能性の高い真の状態を推定するための強力アルゴリズムです。その背後にある数学的な理論は少し複雑ですが、「予測」と「更新」という基本的な考え方を理解することで、そのシンプルさと実用性を実感できると思います。次回記事では、カルマンフィルタの数式について詳細をまとめています。最後までお読みいただきありがとうございました。