非線形状態推定（EKF/UKF）

現実の運動も観測も線形ではない

カルマンフィルタは「状態の遷移も観測も線形関数で、雑音がガウス分布」という前提のもとで、事後分布を最小分散の意味で最適に推定します。ところがロボットの実際の運動は線形ではありません。2輪差動駆動の姿勢更新には向きの三角関数が入り、観測モデルもレンジ・ベアリングセンサなら距離が座標差の平方根、方位が逆正接（arctan）になります。こうした非線形関数にガウス分布を通すと、出力はもはやガウス分布ではなくなり、平均・共分散だけで正しく表せません。

横にスクロール

それでも実務では、多くの場合分布が単峰で緩やかな非線形性に収まるため、「非線形関数の近くではガウス分布で近似し続ける」という割り切りが有効です。この近似の仕方が2通りあり、それが拡張カルマンフィルタ（EKF）と無香料カルマンフィルタ（UKF, Unscented Kalman Filter）です。EKFは関数を線形化し、UKFは分布を代表点で標本化します。多峰性まで扱う必要がある場合はパーティクルフィルタへ進みますが、単峰で足りるならこの2つが計算効率で圧倒します。

EKF：ヤコビ行列による1次線形化

EKFの発想は、非線形関数を現在の推定値の周りで1次のテイラー展開に置き換える、というものです。運動モデル f と観測モデル h を、現在の平均のまわりで偏微分したヤコビ行列が線形化の係数になります。

非線形モデル:
  x_t = f(x_{t-1}, u_t) + w_t      w_t ~ N(0, Q)   プロセス雑音
  z_t = h(x_t)         + v_t      v_t ~ N(0, R)   観測雑音

ヤコビ行列（各要素は偏微分）:
  F = ∂f/∂x |（x_{t-1}=推定平均）    運動モデルの線形化
  H = ∂h/∂x |（x_t=予測平均）        観測モデルの線形化

予測ステップ:
  x_pred = f(x_est, u_t)                非線形関数はそのまま適用
  P_pred = F P_est Fの転置 + Q          共分散はヤコビで線形伝播

更新ステップ:
  K = P_pred Hの転置 (H P_pred Hの転置 + R)^-1   カルマンゲイン
  x_est = x_pred + K (z_t - h(x_pred))            観測残差で補正
  P_est = (I - K H) P_pred

ポイントは、平均の伝播には非線形関数 f・h をそのまま使い、共分散の伝播にだけ線形化したヤコビ行列 F・H を使うという非対称な構造です。平均は非線形の中心を素直にたどり、不確かさ（共分散）だけを局所的な傾きで近似して運びます。これがEKFが「そこそこ効く」理由であり、同時に限界の源でもあります。

UKF：シグマ点で分布そのものを通す

UKFは「関数を近似するより、分布を近似する方が正確だ」という異なる立場を取ります。ヤコビ行列を一切計算せず、代わりに現在の平均・共分散を代表する少数のシグマ点を決定論的に選び、その各点を非線形関数へそのまま通してから、通過後の点群の重み付き平均・共分散を再計算します。これを無香料変換（Unscented Transform, UT）と呼びます。

シグマ点の生成（状態次元 n に対し 2n+1 点）:
  χ_0 = x_est                                       中心点
  χ_i     = x_est + (√((n+λ) P))_i    i=1..n        列ベクトル方向へ
  χ_{i+n} = x_est - (√((n+λ) P))_i    i=1..n        反対方向へ

  √P は共分散の行列平方根（コレスキー分解など）
  λ = α² (n+κ) - n : 中心からの散らばりを決めるスケーリング係数

無香料変換:
  1. 各シグマ点を非線形関数へ: Y_i = f(χ_i)  （または h）
  2. 変換後の平均:   y_mean = Σ_i Wm_i Y_i
  3. 変換後の共分散: Py = Σ_i Wc_i (Y_i - y_mean)(Y_i - y_mean)の転置

  Wm_i, Wc_i : 平均用・共分散用の重み（中心点と周辺点で異なる）

重要なのは、シグマ点が乱数ではなく決定論的に選ばれた最小限の点である点です。パーティクルフィルタが数百から数千の乱数サンプルを要するのに対し、UKFは 2n+1 点で済みます。そして無香料変換は、任意の非線形関数に対して変換後の平均・共分散をテイラー展開の2次項まで正しく捉える（EKFは1次まで）ため、強い非線形性の下でEKFより精度が高くなります。パラメータ α・κ・β はシグマ点の広がりと分布形状（β=2 がガウス最適）の補正に使われます。

EKF・UKF・粒子フィルタの計算量と精度

3手法は「分布をどう近似するか」で系統的に整理できます。

計算量の観点では、EKFとUKFはどちらも共分散行列の演算（行列積や逆行列）が支配的で、状態次元 n に対しておおむね n の3乗オーダーに収まります。UKFのシグマ点は 2n+1 個で n に線形なので、支配項は変わりません。つまりUKFはEKFと同程度の計算コストで、より高い精度とヤコビ不要の実装容易性を得られることが多く、非線形が強い問題での第一候補になります。一方パーティクルフィルタは多峰・非ガウス分布を扱える代わりに、必要サンプル数が状態次元に対して指数的に増える「次元の呪い」を受け、計算量が桁違いに重くなります。単峰で足りる姿勢追跡やセンサ融合ではEKF/UKFが、初期位置不明のグローバル位置推定など多峰性が避けられない場面では粒子フィルタが選ばれます。

収束と発散の条件

カルマンフィルタ系が収束する（推定誤差が有界に保たれる）には、線形ガウス系での可観測性・可制御性に相当する条件が要ります。非線形版では以下が実務上の発散要因になります。

数値面での代表的な破綻が、共分散行列 P が理論上は必ず対称正定であるべきなのに、浮動小数点の丸めや (I - K H) P の形の更新で対称性・正定性を失う現象です。これが起きると行列平方根やゲイン計算が破綻します。対策として、共分散そのものではなくその平方根因子を伝播させ正定性を保証する平方根フィルタ（EKFなら平方根形式、UKFならコレスキー更新を使う実装）や、対称化のための P ← (P + Pの転置)/2 の強制、Q・R を実データの残差統計から調整する適応フィルタが使われます。

まとめ

EKFとUKFは、非線形な運動・観測モデルに対してもガウス分布近似を維持したまま状態を推定するための2つの標準手法です。要点は、(1) カルマンフィルタが最適なのは線形ガウス系に限られ、現実の非線形性には関数近似（EKF）か分布近似（UKF）で対処すること、(2) EKFはヤコビ行列による1次線形化で平均は非線形関数・共分散は線形伝播という非対称構造をとり、実装は軽いが線形化誤差と自信過剰による発散が弱点であること、(3) UKFは決定論的なシグマ点を無香料変換で通すことで微分を使わずテイラー2次まで捉え、EKFと同等の n の3乗計算量でより高精度を得られること、(4) いずれも単峰ガウス限定であり、多峰性が必要なら粒子フィルタへ移るが計算量は桁違いに増えること、(5) 収束には可観測性と適切な雑音設定が要り、共分散の正定性を守る平方根フィルタが数値的発散を防ぐこと。この状態推定の骨格は、SLAMのEKF-SLAMやLiDAR・カメラのセンサ融合、視覚オドメトリの姿勢トラッキングなど、ロボティクスの推定問題全般で繰り返し現れます。