カルマンフィルタによる状態推定

雑音の中から「真の状態」を推定する

センサの値は必ず雑音を含みます。GPS は数メートル揺れ、加速度センサはドリフトし、温度計は読むたびに小刻みに変動します。一方で対象の動き方には物理的な規則（等速運動、慣性など）があります。カルマンフィルタは、この「不完全な観測」と「不完全なモデル」を確率的に重み付けして統合し、時々刻々の状態を平均二乗誤差最小で推定する再帰アルゴリズムです。

核心は、状態を単なる1点の値ではなく 推定値 x とその不確かさを表す共分散行列 P のペアとして持ち、それを 予測 と 更新 の2ステップで回し続ける点にあります。全データを貯めずに直近の推定だけを保持して逐次更新できるため、リアルタイム制御やナビゲーションに向きます。

状態空間モデル：予測の土台

カルマンフィルタは対象を線形の状態空間モデルで表します。時刻 k の状態ベクトルを x_k、観測を z_k として、次の2式を前提にします。

状態方程式（時間発展）: x_k = F x_{k-1} + B u_k + w_k
観測方程式（測定）    : z_k = H x_k + v_k

  F : 状態遷移行列（前の状態から次の状態を予測する物理モデル）
  B : 制御入力行列,  u_k : 制御入力（例：既知の加速指令）
  H : 観測行列（状態を「観測できる量」へ写す）
  w_k : プロセス雑音（モデルの不完全さ）  ~ N(0, Q)
  v_k : 観測雑音（センサの誤差）         ~ N(0, R)

w_k と v_k はいずれも平均0のガウス雑音と仮定し、その共分散が プロセス雑音共分散 Q と 観測雑音共分散 R です。この2つがフィルタの信頼配分を決める最重要パラメータになります。

予測ステップ：モデルで先読みする

各サイクルの前半では、前回の推定を状態方程式に通して次時刻を先読みします。推定値と共分散をそれぞれ進めます（- 記号は観測前の事前推定を表す）。

事前状態推定 : x_k^- = F x_{k-1} + B u_k
事前共分散   : P_k^- = F P_{k-1} Fᵀ + Q

状態は遷移行列 F で素直に進みます。共分散の式が要点で、F P Fᵀ はモデルに従って既存の不確かさを変換し、そこへ Q を加算します。Q を足すぶん 予測後の不確かさは必ず増える——モデルは完璧ではないので、観測なしで時間を進めるほど確信は薄れる、という直観が式に表れています。

更新ステップ：観測で補正する

後半では、実際の観測 z_k を使って事前推定を補正します。まず観測残差（イノベーション）とその共分散を求めます。

イノベーション        : y_k = z_k - H x_k^-      （実測 − 予測から期待される観測）
イノベーション共分散  : S_k = H P_k^- Hᵀ + R

y_k は「予測がどれだけ外したか」、S_k はその外れの不確かさ（予測由来 H P Hᵀ ＋ 観測雑音 R）です。ここから カルマンゲイン K を計算し、状態と共分散を更新します。

カルマンゲイン : K_k = P_k^- Hᵀ S_k^{-1}
状態更新       : x_k = x_k^- + K_k y_k
共分散更新     : P_k = (I - K_k H) P_k^-

状態更新は「予測値 ＋ ゲイン × 残差」という素直な補正で、観測を丸ごと信じるのではなく残差に K を掛けたぶんだけ動かします。共分散更新では (I - K H) を掛けて P が縮む、つまり観測を取り込んだぶん不確かさが減ります。

カルマンゲインの意味：信頼配分を自動化する

K の式を1次元（スカラ）で見ると本質がはっきりします。P^- を予測分散、R を観測分散とすると次のようになります。

K = P^- / (P^- + R)      （H = 1 の1次元の場合）

K は0から1の値を取り、予測と観測のどちらをどれだけ信じるかの重みです。挙動を整理します。

重要なのは、この重みが人手のチューニングではなく P と R の共分散比から毎ステップ自動で決まることです。観測が荒れている局面では自然に予測寄りになり、確度の高い観測が来ればそれを積極的に取り込む——適応的な平滑化がゲインの数式1つに畳み込まれています。

なぜ「最適」なのか

線形の状態空間モデルで雑音がガウス分布に従うという仮定のもとでは、カルマンフィルタの更新式は 推定誤差の共分散のトレース（＝平均二乗誤差）を最小化する線形推定量として導かれます。カルマンゲインの式は、まさに「P_k の対角和を最小にする K」を微分して解いた結果です。

さらにガウス性のもとでは、この線形最適推定はベイズの意味での事後分布の平均とも一致します。予測分布（ガウス）と観測尤度（ガウス）の積がまたガウスになり、その平均・共分散が更新式に対応する——つまりカルマンフィルタは逐次ベイズ推定をガウス仮定下で閉じた形にしたものでもあります。だからこそ「最適フィルタ」と呼ばれます。

センサフュージョンへの応用

カルマンフィルタが真価を発揮するのがセンサフュージョン——特性の異なる複数センサを1つの状態推定に束ねる場面です。仕組みは単純で、観測ベクトル z を縦に積み、観測行列 H で各センサを状態へ対応づけるだけです。

例として、位置と速度を状態 x = [位置, 速度] に持ち、加速度センサ（高頻度だがドリフトで累積誤差が出る）と GPS（低頻度だが絶対位置を与える）を融合するケースを考えます。加速度は制御入力 u として予測ステップで積分し、GPS が来たときだけ観測 z = 位置 で更新します。

予測（毎ステップ, 加速度で先読み）:
  x_k^- = F x_{k-1} + B u_k        （u_k = 加速度）
  P が Q のぶん広がる

更新（GPS 到着時のみ）:
  z_k = 位置観測,  H = [1  0]
  K で位置・速度をまとめて補正し、P が縮む

ここで妙味なのは、H = [1 0] と位置しか観測しないのに、状態間の相関（P の非対角成分）を通じて 速度まで間接的に補正される点です。共分散が変数間の相関を保持しているからこそ、観測していない状態も整合的に修正されます。加速度で細かく先読みしつつ GPS で定期的に絶対位置へ引き戻すことで、両センサの弱点を互いに補完した滑らかで累積誤差の小さい推定が得られます。姿勢推定でジャイロ（角速度）と加速度計・地磁気を融合する構成も、まったく同じ枠組みです。

まとめ

カルマンフィルタは魔法ではなく、「モデルによる予測」と「観測による補正」を確率分布のまま交互に回す逐次推定器です。要点は、(1) 状態を推定値 x と共分散 P のペアで持ち、予測で P を広げ更新で縮めること、(2) カルマンゲイン K が P と R の共分散比から信頼配分を自動決定すること、(3) 線形ガウス系では平均二乗誤差最小の最適推定になり、非線形では EKF/UKF へ拡張すること、(4) 観測 z と観測行列 H を積むだけでセンサフュージョンが自然に実現し、共分散の相関を通じて未観測の状態まで補正されること。この予測・更新サイクルと共分散の伸縮を押さえれば、ナビゲーションから姿勢推定まで、状態推定の幅広い実装が同じ骨格で見通せるようになります。