低軌道衛星を地上局から追いかける場面を思い浮かべてください。地上局からは衛星までの距離（レンジ）とドップラシフト（視線方向速度）が秒オーダーで観測できます。一方、衛星の動きは地球重力下のケプラー運動と摂動で決まる、地球座標系で見れば毎秒数キロメートル動く立派な非線形ダイナミクスです。位置 $\bm{r}$ と速度 $\bm{v}$ を確率密度で表し、観測のたびに分布を更新しながら推定する——これが衛星軌道決定の核心です。
ところが、初学者がここでまず読むカルマンフィルタの理論は、すべて「線形ガウスシステム」を前提にしています。線形ダイナミクス、線形観測、ガウス雑音。これらが揃えば事後分布もまたガウスで、平均と共分散の更新式が閉形式で書ける——これがカルマンフィルタの美しいところです。しかし現実は、衛星軌道、GPS/INS統合、ドローン姿勢、ロボットSLAM、車両のホイールスリップ、化学プラントの反応速度——どれも非線形のオンパレード。線形カルマンをそのまま当てると、誤差共分散が現実の不確かさからずれていき、最悪の場合フィルタが発散します。
この壁を乗り越える二大アプローチが 拡張カルマンフィルタ（Extended Kalman Filter, EKF） と Unscented Kalman Filter（無香カルマンフィルタ, UKF） です。EKFは「非線形関数を点で1次線形化してしまえばよい」というシンプルだが時に裏切られるアプローチ。UKFは「分布をシグマポイントで離散化して非線形関数を直接通す」という、見方を変えた賢いアプローチ。本記事ではこの2つを同じ衛星軌道推定問題に並べて適用し、原理・派生（Cubature KF, Quadrature KF）・粒子フィルタとの位置関係、そしてGPS/INSやSLAM、ROSとの組合せ実装パターンまでを通しで見渡します。
本記事の内容
- 線形カルマンフィルタの復習と「なぜ非線形でそのまま使えないか」
- EKF: 1次テイラー展開、ヤコビアンの計算、収束保証がない理由
- EKFが派手に発散する典型例（極座標→直交座標変換）
- UKF: シグマポイントとUnscented変換による非線形伝搬
- スケーリングパラメータ $\alpha, \beta, \kappa$ の選び方の指針
- 計算量比較: EKFの $O(n^2)$ ヤコビアン × UKFの $O(n^3)$ Cholesky
- Cubature KF, Gauss-Hermite Quadrature KF などの派生
- 粒子フィルタとの位置づけと使い分け
- GPS/INS統合、衛星軌道推定、ドローン姿勢、SLAMでの応用パターン
- ROSと組合せた実装の典型構造
- 衛星軌道のEKF/UKF実装（距離+ドップラ測定）、RMSE比較、シグマポイント可視化、計算時間ベンチマーク
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
線形カルマンの復習: なぜ非線形でそのまま使えないのか
線形カルマンの仮定
線形ガウスのカルマンフィルタは、次の状態空間モデルを前提とします。
$$ \begin{aligned} \bm{x}_{k+1} &= \bm{F}_k \bm{x}_k + \bm{w}_k, \quad \bm{w}_k \sim \mathcal{N}(\bm{0}, \bm{Q}_k) \\ \bm{z}_k &= \bm{H}_k \bm{x}_k + \bm{v}_k, \quad \bm{v}_k \sim \mathcal{N}(\bm{0}, \bm{R}_k) \end{aligned} $$
ここでミソは、状態遷移 $\bm{F}_k$ も観測 $\bm{H}_k$ も線形（行列）であり、雑音もガウスである点です。ガウス分布は線形変換に対して閉じている、つまり「ガウス分布を線形変換するとまたガウス分布」という性質があります。だから事前分布をガウスにしておけば、予測でも更新でもガウスのまま、平均と共分散だけを追跡すれば済む——これが線形カルマンの威力です。
予測ステップ:
$$ \begin{aligned} \hat{\bm{x}}_{k+1|k} &= \bm{F}_k \hat{\bm{x}}_{k|k} \\ \bm{P}_{k+1|k} &= \bm{F}_k \bm{P}_{k|k} \bm{F}_k^\top + \bm{Q}_k \end{aligned} $$
更新ステップ（カルマンゲイン $\bm{K}_k$ で観測残差を吸収）:
$$ \begin{aligned} \bm{K}_k &= \bm{P}_{k|k-1} \bm{H}_k^\top \big(\bm{H}_k \bm{P}_{k|k-1} \bm{H}_k^\top + \bm{R}_k\big)^{-1} \\ \hat{\bm{x}}_{k|k} &= \hat{\bm{x}}_{k|k-1} + \bm{K}_k\big(\bm{z}_k – \bm{H}_k \hat{\bm{x}}_{k|k-1}\big) \\ \bm{P}_{k|k} &= (\bm{I} – \bm{K}_k \bm{H}_k) \bm{P}_{k|k-1} \end{aligned} $$
きれいです。ところが、現実の系では $\bm{x}_{k+1} = f(\bm{x}_k) + \bm{w}_k$、$\bm{z}_k = h(\bm{x}_k) + \bm{v}_k$ のように、$f$ や $h$ が非線形関数となります。
何が壊れるのか
非線形関数 $f$ をガウス分布 $\mathcal{N}(\hat{\bm{x}}, \bm{P})$ に作用させると、変換後の分布はもはやガウスではありません。歪み、二峰化、長い尾——なんでも起こりえます。この時点で「平均と共分散だけを追えばよい」という線形カルマンの土台が崩れます。
たとえば、状態を $r$（距離）と $\theta$（角度）で表し、観測は直交座標 $(x, y) = (r\cos\theta, r\sin\theta)$ とする、レーダー追跡でよくある状況を考えます。$r$ と $\theta$ がそれぞれ独立ガウスでも、$(x, y)$ の分布は三日月のように歪み、もはやガウスとは似ても似つきません。
ここから2つの戦略が分岐します。
- 戦略A: 非線形関数を点で線形近似し、無理やり線形カルマンの枠に押し込む → EKF
- 戦略B: 関数は一切いじらず、分布の方を有限個のサンプルで表現し、その点を非線形に通して再構成する → UKF
この発想の違いを念頭に、それぞれを詳しく見ていきましょう。
EKF: 1次テイラー展開で局所線形化する
アイデアの直感
EKFのアイデアは「非線形関数 $f(\bm{x})$ も、現在の推定値 $\hat{\bm{x}}$ の周りで小さな範囲しか見ないなら、その点での接平面で十分近似できるはず」というものです。微積分で習ったテイラー展開の1次までで打ち切る、あの操作を状態空間で毎時刻やります。
$$ f(\bm{x}) \approx f(\hat{\bm{x}}) + \bm{F}(\hat{\bm{x}}) (\bm{x} – \hat{\bm{x}}) $$
ここで $\bm{F}(\hat{\bm{x}}) = \partial f / \partial \bm{x}|_{\hat{\bm{x}}}$ は ヤコビ行列です。これで線形カルマンの式の $\bm{F}_k$ の代わりに $\bm{F}(\hat{\bm{x}}_{k|k})$ を、$\bm{H}_k$ の代わりに $\bm{H}(\hat{\bm{x}}_{k|k-1}) = \partial h / \partial \bm{x}|_{\hat{\bm{x}}_{k|k-1}}$ を入れれば、形式上は線形カルマンと同じ式が回ります。
EKFのアルゴリズム
予測ステップ:
$$ \begin{aligned} \hat{\bm{x}}_{k+1|k} &= f(\hat{\bm{x}}_{k|k}) \\ \bm{F}_k &= \left.\frac{\partial f}{\partial \bm{x}}\right|_{\hat{\bm{x}}_{k|k}} \\ \bm{P}_{k+1|k} &= \bm{F}_k \bm{P}_{k|k} \bm{F}_k^\top + \bm{Q}_k \end{aligned} $$
平均は非線形関数そのものに通す点に注意してください。共分散の伝搬だけがヤコビアンで線形化されます。
更新ステップ:
$$ \begin{aligned} \bm{H}_k &= \left.\frac{\partial h}{\partial \bm{x}}\right|_{\hat{\bm{x}}_{k|k-1}} \\ \bm{S}_k &= \bm{H}_k \bm{P}_{k|k-1} \bm{H}_k^\top + \bm{R}_k \\ \bm{K}_k &= \bm{P}_{k|k-1} \bm{H}_k^\top \bm{S}_k^{-1} \\ \hat{\bm{x}}_{k|k} &= \hat{\bm{x}}_{k|k-1} + \bm{K}_k\big(\bm{z}_k – h(\hat{\bm{x}}_{k|k-1})\big) \\ \bm{P}_{k|k} &= (\bm{I} – \bm{K}_k \bm{H}_k) \bm{P}_{k|k-1} \end{aligned} $$
観測残差（イノベーション）の計算でも、$h(\hat{\bm{x}}_{k|k-1})$ という非線形写像そのものを使い、行列 $\bm{H}_k$ は共分散の更新にだけ顔を出します。これは実装上のミスが起きやすいポイントで、「予測値を $\bm{F}\hat{\bm{x}}$ にしてはいけない、$f(\hat{\bm{x}})$ にする」と覚えてください。
収束の保証がない
EKFの恐ろしさは、線形カルマンと違って収束も最適性も保証されないことにあります。線形カルマンは「ガウス雑音下で線形最小分散推定器」という強い性質を持ちますが、EKFはあくまで近似なので、
- 非線形性が強いと1次近似の誤差が大きく、共分散が実際の不確かさを過小評価する
- 過小評価された共分散により $\bm{K}_k$ が小さくなり、観測を信用しなくなる
- 観測を反映できないので、推定値が真値から離れていく
- 離れた点で線形化するので、ヤコビアンがさらに不正確になる
- この悪循環でフィルタが発散する
これは机上の問題ではなく、衛星軌道決定や姿勢推定の現場で頻繁に遭遇する失敗モードです。実務では、$\bm{Q}$ を意図的に大きく膨らませる、共分散下限を設けて「圧搾」を防ぐ、Joseph形式で共分散の対称・正定値性を維持する、といった対症療法が必須になります。
解析的ヤコビアン vs 数値ヤコビアン
ヤコビアン $\bm{F}, \bm{H}$ を求める方法は2通りあります。
- 解析的: $f, h$ を手で微分する。高速で精度は十分だが、複雑なダイナミクス（J2摂動、大気抵抗、推進剤スロッシュなど）では式が爆発し、バグを混入させやすい
- 数値的（finite difference）: $f(\hat{\bm{x}} + \epsilon \bm{e}_j)$ を計算して差分を取る。実装は楽だが、ステップ幅 $\epsilon$ の選定がシビアで、$\epsilon$ を小さくしすぎれば桁落ち、大きすぎれば近似誤差
最近は自動微分（JAX, PyTorch）でヤコビアンを得る方法が増えており、EKFを書くハードルは確実に下がっています。とはいえ、ヤコビアン計算という追加の工程が必要であることに変わりはなく、これがEKFの「面倒くささ」の根源です。UKFを学ぶと、この工程がまるごと消えるありがたみが分かります。
EKFが発散する典型例: 極座標→直交座標
EKFがどう破綻するかを目で見るために、典型的な極座標→直交座標変換を考えます。観測モデルが
$$ h(\bm{x}) = \begin{pmatrix} r \cos\theta \\ r \sin\theta \end{pmatrix}, \quad \bm{x} = \begin{pmatrix} r \\ \theta \end{pmatrix} $$
であるとき、ヤコビアンは
$$ \bm{H} = \begin{pmatrix} \cos\theta & -r\sin\theta \\ \sin\theta & r\cos\theta \end{pmatrix} $$
です。問題は $\theta$ の不確かさが大きいとき。$\sigma_\theta = 0.5\,\text{rad}$（約29度）程度になると、点 $(\hat{r}, \hat{\theta})$ での接平面は分布全体を到底覆えません。実際の $(x, y)$ 分布は三日月型に大きく歪み、その平均は $h(\hat{r}, \hat{\theta})$ よりも原点側にずれます。これは、$r\cos\theta$ の関数を $\theta$ について平均すると $\mathbb{E}[\cos\theta] = e^{-\sigma_\theta^2/2}\cos\hat{\theta}$ が出てくる（ガウス積分）からです。
EKFはこの「平均のずれ」も「共分散の歪み」も捉えられず、自信過剰な共分散を返します。観測が来てもこの自信過剰さが残るため、フィルタは真値から離れた点に居座り続けます。この現象はレーダー追跡・初期軌道決定・ロボット位置推定すべてで報告されており、しばしば「EKFの一次バイアス」と呼ばれます。
ここで、「分布の歪みごと近似できる方法はないのか」という問いが自然に生まれます。その答えがUKFです。
UKF: シグマポイントで分布を直接運ぶ
Unscented変換のアイデア
UKFの根本思想は、Julier and Uhlmannの言葉を借りれば「非線形関数を近似するよりも、確率分布を近似する方がやさしい」というものです。線形近似で関数を歪めるのではなく、ガウス分布を少数の決定論的サンプル点（シグマポイント）で離散化し、それらを非線形関数にそのまま通して、変換後の点群から再びガウスを当てはめる、というアプローチです。
直感的には、ガウス分布の重要な点（平均と $\pm\sigma$ あたり）を選んで運び、その移動先で平均と共分散を再構成するイメージです。線形化のような「微分の存在」を仮定しないので、関数がジャンプしていても角があっても適用できます。
シグマポイントの構成
$n$ 次元のガウス $\mathcal{N}(\hat{\bm{x}}, \bm{P})$ から、合計 $2n+1$ 個のシグマポイントを次のように構成します。
$$ \begin{aligned} \bm{\chi}_0 &= \hat{\bm{x}} \\ \bm{\chi}_i &= \hat{\bm{x}} + \big(\sqrt{(n+\lambda)\bm{P}}\big)_i, \quad i = 1, \dots, n \\ \bm{\chi}_i &= \hat{\bm{x}} – \big(\sqrt{(n+\lambda)\bm{P}}\big)_{i-n}, \quad i = n+1, \dots, 2n \end{aligned} $$
ここで $(\sqrt{\cdot})_i$ は行列平方根（通常はコレスキー分解）の $i$ 列目を表します。$\lambda$ はスケーリング係数で、後述の $\alpha, \kappa$ から決まります。
各点に重みを与えます。平均の重み:
$$ W_0^{(m)} = \frac{\lambda}{n + \lambda}, \quad W_i^{(m)} = \frac{1}{2(n+\lambda)}, \quad i = 1, \dots, 2n $$
共分散の重み（中心点 $\bm{\chi}_0$ だけが異なる）:
$$ W_0^{(c)} = \frac{\lambda}{n + \lambda} + (1 – \alpha^2 + \beta), \quad W_i^{(c)} = \frac{1}{2(n+\lambda)}, \quad i = 1, \dots, 2n $$
スケーリングは
$$ \lambda = \alpha^2 (n + \kappa) – n $$
で定義され、3つのチューニングパラメータ $\alpha, \beta, \kappa$ がここに集約されます。
Unscented変換の予測
非線形関数 $f$ を全シグマポイントに通します。
$$ \bm{\chi}_i^* = f(\bm{\chi}_i), \quad i = 0, 1, \dots, 2n $$
変換後の平均と共分散は、加重和で推定します。
$$ \hat{\bm{x}}’ = \sum_{i=0}^{2n} W_i^{(m)} \bm{\chi}_i^*, \quad \bm{P}’ = \sum_{i=0}^{2n} W_i^{(c)} (\bm{\chi}_i^* – \hat{\bm{x}}’)(\bm{\chi}_i^* – \hat{\bm{x}}’)^\top + \bm{Q} $$
このシンプルな式が、線形カルマンの $\bm{F}\bm{P}\bm{F}^\top + \bm{Q}$ に相当します。ヤコビアンが一切出てきません。
UKFの更新ステップ
予測値 $\hat{\bm{x}}_{k+1|k}, \bm{P}_{k+1|k}$ から再びシグマポイントを生成し、観測関数 $h$ に通します。
$$ \bm{\mathcal{Z}}_i = h(\bm{\chi}_i), \quad \hat{\bm{z}} = \sum_i W_i^{(m)} \bm{\mathcal{Z}}_i $$
観測残差の共分散 $\bm{S}$ と、状態と観測のクロス共分散 $\bm{P}_{xz}$ を計算します。
$$ \begin{aligned} \bm{S} &= \sum_i W_i^{(c)} (\bm{\mathcal{Z}}_i – \hat{\bm{z}})(\bm{\mathcal{Z}}_i – \hat{\bm{z}})^\top + \bm{R} \\ \bm{P}_{xz} &= \sum_i W_i^{(c)} (\bm{\chi}_i – \hat{\bm{x}})(\bm{\mathcal{Z}}_i – \hat{\bm{z}})^\top \end{aligned} $$
カルマンゲインと状態更新は線形カルマンと同じ形式です。
$$ \bm{K} = \bm{P}_{xz} \bm{S}^{-1}, \quad \hat{\bm{x}}_{k|k} = \hat{\bm{x}}_{k|k-1} + \bm{K}(\bm{z} – \hat{\bm{z}}), \quad \bm{P}_{k|k} = \bm{P}_{k|k-1} – \bm{K}\bm{S}\bm{K}^\top $$
ヤコビアンの計算が完全に消え、必要なのは関数 $f, h$ の評価だけです。これがUKFの実装上の最大の利点です。
スケーリングパラメータ $\alpha, \beta, \kappa$ の選び方
3つのパラメータの役割を整理しておきます。
- $\alpha$: シグマポイントを平均周りにどれだけ「広げるか」を決める。値は $0 < \alpha \leq 1$ の小さな値が典型的で、$10^{-3}$ がしばしば使われる。非線形性が強い場合は小さく、滑らかな場合は大きくしてもよい
- $\beta$: 事前分布の形を考慮する係数。ガウス分布なら $\beta = 2$ が最適であることが解析的に示されている
- $\kappa$: 二次的なスケーリング。通常 $\kappa = 0$ または $\kappa = 3 – n$ を使う。状態次元 $n \leq 3$ なら $\kappa = 3 – n \geq 0$ で問題ないが、高次元では $\kappa = 0$ で固定するのが一般的
衛星軌道（6次元: 位置+速度）では $\alpha = 10^{-3}, \beta = 2, \kappa = 0$ がしばしばデフォルト値として使われます。$\alpha$ をいじって挙動を見るのが実用的なチューニング手順です。
注意点として、シグマポイントの構成には行列平方根 $\sqrt{(n+\lambda)\bm{P}}$ が必要で、$(n+\lambda)\bm{P}$ が正定値でないと計算できません。$\lambda < -n$ になると重み $W_0^{(m)}$ が負になり、共分散の正定値性が崩れることがあります。これを避けるため、平方根UKF（Square-Root UKF）という派生では、共分散そのものではなく $\sqrt{\bm{P}}$ を直接伝搬し、数値安定性を保ちます。
EKFとUKFが揃ったところで、もう一つ重要な観点——計算量と派生フィルタ——に進みます。
計算量と派生フィルタ
EKFとUKFの計算量
状態次元を $n$、観測次元を $m$ とします。
- EKF: ヤコビアン $\bm{F}, \bm{H}$ の評価が中心で、これらは $n \times n$ および $m \times n$ の行列。状態予測の共分散更新 $\bm{F}\bm{P}\bm{F}^\top$ が $O(n^3)$、観測更新の $\bm{H}\bm{P}\bm{H}^\top$ が $O(n^2 m)$、ゲイン計算の逆行列が $O(m^3)$。ただしヤコビアンが解析的に得られていれば、それ自体の計算は $O(n^2)$ 程度に収まることが多い。総合では実用上 $O(n^3)$ がドミナント
- UKF: シグマポイント生成にコレスキー分解 $\sqrt{\bm{P}}$ が必要で、これが $O(n^3)$。$2n+1$ 個の点を $f$ に通すので関数評価が $2n+1$ 回。各点の伝搬コストを $C_f$ とすれば、予測ステップは $(2n+1)C_f + O(n^3)$
UKFのコレスキーが $O(n^3)$ なのが少し重いですが、ヤコビアン計算が消えることを考えると、複雑な $f, h$（衛星ダイナミクスやセンサーモデル）ではUKFが有利になることも多々あります。さらに重要なのは「実装の楽さ」で、UKFは関数 $f, h$ をブラックボックスとして扱えるので、シミュレーション環境（Astropy, Skyfield, ROS）に組み込みやすいのです。
Cubature Kalman Filter（CKF）
UKFのシグマポイントの取り方を、3次の球面積分（spherical-radial cubature）に基づいて固定したのが Cubature Kalman Filter（CKF） です。点数は $2n$ 個（中心点なし）で、すべての重みが等しく $W_i = 1/(2n)$。スケーリングパラメータが一切ないので、チューニングフリーで使えるのが魅力です。理論的には UKF の特別な場合（$\alpha = 1, \beta = 0, \kappa = 0$）に対応し、高次元状態（$n \geq 5$ など）でUKFが負の重みで不安定になる状況でも CKF は安定する傾向があります。
シグマポイント:
$$ \bm{\chi}_i = \hat{\bm{x}} + \sqrt{n}\,\big(\sqrt{\bm{P}}\big)_i, \quad \bm{\chi}_{n+i} = \hat{\bm{x}} – \sqrt{n}\,\big(\sqrt{\bm{P}}\big)_i, \quad i = 1, \dots, n $$
重みは $W_i = 1/(2n)$ で全点同じ。UKFのパラメータチューニングを避けたい場面では、CKFをデフォルトにする実装が増えています。
Gauss-Hermite Quadrature Kalman Filter（GHKF）
Gauss-Hermite quadratureは、ガウス積分 $\int g(x) e^{-x^2/2} dx$ を多項式の零点を使って高精度に近似する古典的手法です。これをカルマンフィルタの予測に持ち込んだのがQuadrature Kalman Filter (QKF) または GHKF。
各次元あたり $p$ 点の quadrature 点を使うと、$n$ 次元では $p^n$ 個のシグマポイントが必要になります（テンソル積）。$p = 3, n = 6$ なら 729点。点数が次元の指数で爆発するのがGHKFの最大の欠点で、低次元（$n \leq 4$ 程度）でない限り実用的ではありません。しかし精度はUKFやCKFを上回ることが多く、軌道決定の地上局オフライン解析など、計算時間を許容できる場面で使われます。
UKF、CKF、GHKFはすべてシグマポイントカルマンの系譜に属し、線形性を仮定せずガウス分布を維持するという同じ哲学を共有しています。違いは「どんな点をどんな重みで選ぶか」の戦略だけです。
ここまでガウス分布を維持する手法を見てきました。しかし、もっと根本的に「分布の形そのものを離散化する」アプローチもあります。それが粒子フィルタです。
粒子フィルタとの比較
粒子フィルタ（Particle Filter, PF） は、状態の事後分布を $N$ 個の重み付きサンプル（粒子）で表現し、それを次々と更新していくモンテカルロ手法です。シグマポイントとの違いは:
- シグマポイント（UKF/CKF）: 決定論的に少数（$2n$ 程度）の点を選ぶ。事後分布をガウスと仮定して再構成する
- 粒子（PF）: 確率的に多数（$10^3$ – $10^6$ 個）の粒子をサンプリングする。分布の形に仮定を置かない（多峰、非ガウスでもOK）
PFは「真の非線形・非ガウスフィルタ」と言える強力な手法ですが、欠点もあります。
- 粒子数が状態次元に対して指数的に増える（次元の呪い）
- リサンプリングで粒子が劣化する（particle degeneracy）
- 計算量が大きい
このため、
- 線形に近い・ガウスでよい: 線形カルマン
- 弱い非線形・ガウス近似OK: EKF
- 強い非線形・ガウス近似でも十分な精度: UKF/CKF
- 多峰分布や非ガウスを扱う必要がある: PF（SLAMの初期化、軌道初期決定、放電プラズマ追跡など）
という使い分けが一般的です。衛星軌道決定では事後分布は概ね単峰ガウスに収束するので、UKFかCKFが第一選択になることが多いのです。
理論をひととおり眺めたところで、では実際の応用シーンでどう使われているのかを概観します。
実応用パターン
GPS/INS統合
慣性航法装置（INS）はジャイロと加速度計の出力を積分して位置・姿勢を求めますが、ドリフトが蓄積します。GPSは絶対位置を直接観測できる代わりに更新レートが低く（1-10Hz）、マルチパスや遮蔽に弱い。両者をEKF（あるいはUKF）で結合することで、INSの短時間精度とGPSの長時間絶対精度を両立するのが定石です。状態は $(\bm{r}, \bm{v}, \bm{q}, \bm{b}_g, \bm{b}_a)$ の15次元（位置3+速度3+姿勢クォータニオン4+ジャイロバイアス3+加速度バイアス3、ただしクォータニオンの誤差状態を3次元で扱い15-DOFとするのが標準）。航空機・自動車・ドローンで標準的なアーキテクチャです。
衛星軌道決定
地上局からの距離（レンジ）、ドップラ、角度（光学）といった観測から、衛星の軌道要素を推定するタスク。状態は位置・速度の6次元、または軌道要素+摂動パラメータの拡張状態。ダイナミクスは2体問題＋J2、大気抵抗、太陽輻射圧、第三体重力など。観測関数は地上局位置と衛星位置から幾何的に計算する非線形関数。NASA、ESA、JAXAの軌道決定システムでは、伝統的にバッチ最小二乗法とEKFが併用され、近年はUKFやSquare-Root UKFを使う実装も増えています。
ドローン姿勢推定
ジャイロ＋加速度＋磁気センサからの姿勢推定。状態はクォータニオン4＋ジャイロバイアス3の7次元（あるいは誤差状態EKFで6次元）。クォータニオンの正規化制約、加速度センサの動加速度成分などが非線形性をもたらします。PX4, ArduPilot, OpenPilotなど主要ドローンファームウェアの中核に組み込まれています。
ロボットSLAM
Simultaneous Localization and Mapping。ロボットの位置と環境地図を同時に推定する問題。EKF-SLAMが古典的解で、状態は $(\bm{x}_{\text{robot}}, \bm{m}_1, \bm{m}_2, \dots, \bm{m}_N)$ のように地図ランドマーク全部を含む（地図サイズに対して状態次元が増える）。計算量が地図サイズの二乗で増えるため、最近はFast-SLAM（粒子フィルタ）、Graph-SLAM（最適化ベース）に置き換わりつつありますが、小規模問題ではいまだEKF/UKF-SLAMが教育的にも実用的にも有用です。
ROSとの組合せ実装パターン
ROS（Robot Operating System）には robot_localization という汎用カルマンフィルタパッケージがあり、ekf_localization_node と ukf_localization_node が標準で提供されています。典型的なノード構成は:
- センサノード（IMU, GPS, オドメトリ, ビジョン）が
sensor_msgs/Imu,nav_msgs/Odometryを publish ekf_localization_nodeが複数センサを subscribe し、内部で EKF を回す- 推定された姿勢を
/odometry/filteredとして publish、TF tree に統合
ROS上で実装する際は、共分散行列の管理、座標系（map / odom / base_link）の整合性、メッセージのタイムスタンプ同期に注意が必要です。robot_localization の <state_estimation_node> パラメータで state vector のどの成分を更新対象にするか細かく指定でき、たとえば「GPSからは位置だけ取り、速度は無視する」「IMUからは角速度だけ取り、姿勢は無視する」という選択ができます。これがマルチセンサ融合の柔軟性の源です。
これだけ前提を整えれば、実装に入る準備は十分です。次に衛星軌道のEKF/UKFをPythonで動かし、性能を比較します。
Pythonでの実装: 衛星軌道のEKF/UKF
問題設定
低軌道衛星（高度約500km、円軌道）を1つの地上局からレンジ（距離）とレンジレート（ドップラ由来の視線速度）で追跡します。
- 状態: $\bm{x} = (r_x, r_y, r_z, v_x, v_y, v_z) \in \mathbb{R}^6$（地球中心慣性系=ECI）
- ダイナミクス: 2体問題 $\ddot{\bm{r}} = -\mu \bm{r}/\|\bm{r}\|^3$（$\mu$ は地球重力定数）。プロセス雑音は加速度に作用させる
- 観測: レンジ $\rho = \|\bm{r} – \bm{r}_{\text{gs}}\|$、レンジレート $\dot{\rho} = (\bm{r} – \bm{r}_{\text{gs}})\cdot \bm{v} / \rho$
- 観測周期: 10秒
- 観測ノイズ: $\sigma_\rho = 10\,\text{m}$, $\sigma_{\dot{\rho}} = 0.1\,\text{m/s}$
共通モジュール: ダイナミクスと観測
まず、両フィルタで共有するダイナミクスと観測のモジュールを書きます。
import numpy as np
from scipy.linalg import cholesky
# 物理定数
MU_EARTH = 3.986004418e14 # m^3/s^2
R_EARTH = 6378137.0 # m
OMEGA_EARTH = 7.2921159e-5 # rad/s (使わないがECEF変換時に必要)
def two_body_acc(r):
"""2体問題の加速度 [m/s^2]"""
rn = np.linalg.norm(r)
return -MU_EARTH * r / rn**3
def rk4_step(x, dt):
"""4次ルンゲクッタで dt 秒進める。x=(r,v) ∈ R^6"""
def f(s):
r, v = s[:3], s[3:]
return np.concatenate([v, two_body_acc(r)])
k1 = f(x)
k2 = f(x + 0.5*dt*k1)
k3 = f(x + 0.5*dt*k2)
k4 = f(x + dt*k3)
return x + (dt/6.0)*(k1 + 2*k2 + 2*k3 + k4)
def observe(x, r_gs):
"""観測関数 h(x): レンジとレンジレート"""
rel = x[:3] - r_gs
rho = np.linalg.norm(rel)
rho_dot = np.dot(rel, x[3:]) / rho
return np.array([rho, rho_dot])
ここでのポイントは、ダイナミクス rk4_step と観測 observe をプレーンな関数として書いている点です。これを EKF/UKF が同じ形でブラックボックスとして呼べるようにしておきます。
真の軌道とノイズ付き観測の生成
シミュレーション用の真値と観測データを作ります。
import numpy as np
np.random.seed(42)
# 初期状態: 高度500km, 傾斜角51.6度（ISS相当）
alt = 500e3
r0_mag = R_EARTH + alt
v0_mag = np.sqrt(MU_EARTH / r0_mag) # 円軌道速度
inc = np.deg2rad(51.6)
x_true = np.array([
r0_mag, 0.0, 0.0,
0.0, v0_mag*np.cos(inc), v0_mag*np.sin(inc)
])
# 地上局: 緯度35度N（東京付近）、経度0（簡単のためECI=ECEF同一視）
lat_gs = np.deg2rad(35.0)
r_gs = R_EARTH * np.array([np.cos(lat_gs), 0.0, np.sin(lat_gs)])
# シミュレーション設定
dt_prop = 1.0 # 伝播刻み 1秒
dt_obs = 10.0 # 観測周期 10秒
T_total = 3600.0 # 1時間追跡
N_steps = int(T_total / dt_prop)
N_obs = int(T_total / dt_obs)
sigma_rho = 10.0 # レンジ観測ノイズ [m]
sigma_rhodot = 0.1 # ドップラ観測ノイズ [m/s]
R_obs = np.diag([sigma_rho**2, sigma_rhodot**2])
# 真の軌跡と観測の生成
xs_true = np.zeros((N_steps+1, 6))
xs_true[0] = x_true
for k in range(N_steps):
xs_true[k+1] = rk4_step(xs_true[k], dt_prop)
obs_times = np.arange(1, N_obs+1) * dt_obs
zs = np.zeros((N_obs, 2))
for i, t in enumerate(obs_times):
k = int(t / dt_prop)
z_clean = observe(xs_true[k], r_gs)
zs[i] = z_clean + np.random.randn(2) * np.array([sigma_rho, sigma_rhodot])
ここまでで真の軌道（1時間分、1秒刻み）と、10秒ごとのノイズ付きレンジ・レンジレート観測が用意できました。レンジは数百〜数千キロメートル、レンジレートは数キロメートル毎秒の値域なので、観測ノイズ $\sigma_\rho = 10\,\text{m}, \sigma_{\dot{\rho}} = 0.1\,\text{m/s}$ は SN比的に十分小さく、フィルタが正しく動けば mm-cm レベルの状態推定精度が出ることを期待します。
EKFの実装
EKFには解析的ヤコビアンを与えます。観測のヤコビアン $\bm{H} = \partial h/\partial \bm{x}$ を導出すると、レンジ $\rho = \|\bm{r}_{\text{rel}}\|$、レンジレート $\dot\rho = \bm{r}_{\text{rel}}\cdot \bm{v}/\rho$ について
$$ \bm{H} = \begin{pmatrix} \bm{u}^\top & \bm{0}^\top \\ \frac{1}{\rho}\big(\bm{v}^\top – \dot\rho\,\bm{u}^\top\big) & \bm{u}^\top \end{pmatrix}, \quad \bm{u} = \bm{r}_{\text{rel}}/\rho $$
となります（4×6行列）。状態遷移ヤコビアンは、伝播刻み $dt$ で 2体問題を線形化したもので、運動方程式の Jacobi 行列を数値積分しても良いですし、近似として $\bm{F} \approx \bm{I} + \bm{A}\,dt$（$\bm{A}$ は2体問題の連続時間 Jacobi）と取っても十分実用的です。ここでは数値ヤコビアンで簡単に書きます。
import numpy as np
def jacobian_F(x, dt):
"""状態遷移ヤコビアン dF/dx を中央差分で計算 (6x6)"""
n = 6
eps = 1e-3
F = np.zeros((n, n))
for j in range(n):
e = np.zeros(n); e[j] = eps
F[:, j] = (rk4_step(x + e, dt) - rk4_step(x - e, dt)) / (2*eps)
return F
def jacobian_H(x, r_gs):
"""観測ヤコビアン dh/dx (2x6)"""
rel = x[:3] - r_gs
rho = np.linalg.norm(rel)
u = rel / rho
rho_dot = np.dot(rel, x[3:]) / rho
H = np.zeros((2, 6))
H[0, 0:3] = u
H[1, 0:3] = (x[3:] - rho_dot * u) / rho
H[1, 3:6] = u
return H
def run_ekf(x0, P0, Q, R, zs, obs_times, dt_prop, r_gs):
"""EKFのメインループ"""
x_hat = x0.copy()
P = P0.copy()
xs_est = [x_hat.copy()]
Ps_est = [P.copy()]
obs_idx = 0
t = 0.0
while obs_idx < len(obs_times):
# 予測ステップ: 1秒ずつ伝播し、観測時刻が来たら更新
F = jacobian_F(x_hat, dt_prop)
x_hat = rk4_step(x_hat, dt_prop)
P = F @ P @ F.T + Q
t += dt_prop
if abs(t - obs_times[obs_idx]) < 1e-6:
# 観測更新
H = jacobian_H(x_hat, r_gs)
z_pred = observe(x_hat, r_gs)
y = zs[obs_idx] - z_pred
S = H @ P @ H.T + R
K = P @ H.T @ np.linalg.inv(S)
x_hat = x_hat + K @ y
P = (np.eye(6) - K @ H) @ P
# Joseph 形式で対称・正定値を保つ
P = 0.5 * (P + P.T)
obs_idx += 1
xs_est.append(x_hat.copy())
Ps_est.append(P.copy())
return np.array(xs_est), np.array(Ps_est)
このEKF実装で押さえるべき点が3つあります。第一に、rk4_step で非線形のまま平均を伝播し、共分散だけをヤコビアン F で線形に伝搬している点（EKFの定義どおり）。第二に、Joseph 形式の代わりに対称化 $P = (P + P^\top)/2$ で数値誤差をならしている点。第三に、毎時刻 F を再計算している点。状態が大きく変化する非線形系では、毎ステップ線形化点を更新するのが必須です。
UKFの実装
UKFでは関数 $f, h$ をそのまま渡すだけで済みます。ヤコビアンは出てきません。
import numpy as np
from scipy.linalg import cholesky
def sigma_points(x, P, alpha=1e-3, beta=2.0, kappa=0.0):
"""UKFのシグマポイントと重みを生成"""
n = len(x)
lam = alpha**2 * (n + kappa) - n
c = n + lam
# コレスキー分解で sqrt((n+lam)P) を計算
sqrtP = cholesky(c * P, lower=True)
chi = np.zeros((2*n+1, n))
chi[0] = x
for i in range(n):
chi[i+1] = x + sqrtP[:, i]
chi[n+i+1] = x - sqrtP[:, i]
# 重み
Wm = np.full(2*n+1, 1.0/(2*c))
Wc = np.full(2*n+1, 1.0/(2*c))
Wm[0] = lam / c
Wc[0] = lam / c + (1 - alpha**2 + beta)
return chi, Wm, Wc
def ukf_predict(x, P, Q, dt_prop, alpha=1e-3, beta=2.0, kappa=0.0):
"""UKF予測ステップ"""
chi, Wm, Wc = sigma_points(x, P, alpha, beta, kappa)
chi_pred = np.array([rk4_step(s, dt_prop) for s in chi])
x_new = np.sum(Wm[:, None] * chi_pred, axis=0)
diff = chi_pred - x_new
P_new = (Wc[:, None, None] * diff[:, :, None] * diff[:, None, :]).sum(0) + Q
return x_new, P_new, chi_pred, Wm, Wc
def ukf_update(x, P, z, R, r_gs, alpha=1e-3, beta=2.0, kappa=0.0):
"""UKF観測更新ステップ"""
chi, Wm, Wc = sigma_points(x, P, alpha, beta, kappa)
Z = np.array([observe(s, r_gs) for s in chi])
z_hat = np.sum(Wm[:, None] * Z, axis=0)
dz = Z - z_hat
dx = chi - x
S = (Wc[:, None, None] * dz[:, :, None] * dz[:, None, :]).sum(0) + R
Pxz = (Wc[:, None, None] * dx[:, :, None] * dz[:, None, :]).sum(0)
K = Pxz @ np.linalg.inv(S)
x_new = x + K @ (z - z_hat)
P_new = P - K @ S @ K.T
P_new = 0.5 * (P_new + P_new.T)
return x_new, P_new
def run_ukf(x0, P0, Q, R, zs, obs_times, dt_prop, r_gs,
alpha=1e-3, beta=2.0, kappa=0.0):
"""UKFのメインループ"""
x_hat = x0.copy()
P = P0.copy()
xs_est = [x_hat.copy()]
Ps_est = [P.copy()]
obs_idx = 0
t = 0.0
while obs_idx < len(obs_times):
x_hat, P, _, _, _ = ukf_predict(x_hat, P, Q, dt_prop, alpha, beta, kappa)
t += dt_prop
if abs(t - obs_times[obs_idx]) < 1e-6:
x_hat, P = ukf_update(x_hat, P, zs[obs_idx], R, r_gs,
alpha, beta, kappa)
obs_idx += 1
xs_est.append(x_hat.copy())
Ps_est.append(P.copy())
return np.array(xs_est), np.array(Ps_est)
EKFと並べてみるとUKFの「コードの軽さ」が際立ちます。jacobian_F も jacobian_H も登場しません。rk4_step と observe を関数として渡すだけ。これがUKFを実装で愛される理由です。
両者の比較実験
同じ初期推定誤差・同じQ・同じデータを使い、EKFとUKFを並べて走らせます。
import numpy as np
import time
# 初期推定の誤差: 位置100m, 速度1m/s のオフセット
x0_err = x_true + np.array([100, -50, 30, 1.0, -0.5, 0.3])
P0 = np.diag([1e4, 1e4, 1e4, 1.0, 1.0, 1.0]) # 位置100m, 速度1m/s
# プロセス雑音（モデル化されていない加速度を許容）
sigma_acc = 1e-6 # m/s^2
Q = np.zeros((6, 6))
Q[3:, 3:] = (sigma_acc * dt_prop)**2 * np.eye(3)
# EKF
t0 = time.perf_counter()
xs_ekf, Ps_ekf = run_ekf(x0_err, P0, Q, R_obs, zs, obs_times, dt_prop, r_gs)
t_ekf = time.perf_counter() - t0
# UKF
t0 = time.perf_counter()
xs_ukf, Ps_ukf = run_ukf(x0_err, P0, Q, R_obs, zs, obs_times, dt_prop, r_gs)
t_ukf = time.perf_counter() - t0
# 真値とのRMSE
err_ekf_pos = np.linalg.norm(xs_ekf[:, :3] - xs_true[:, :3], axis=1)
err_ukf_pos = np.linalg.norm(xs_ukf[:, :3] - xs_true[:, :3], axis=1)
err_ekf_vel = np.linalg.norm(xs_ekf[:, 3:] - xs_true[:, 3:], axis=1)
err_ukf_vel = np.linalg.norm(xs_ukf[:, 3:] - xs_true[:, 3:], axis=1)
print(f"EKF time = {t_ekf:.2f}s, RMSE pos = {err_ekf_pos.mean():.3f} m, "
f"RMSE vel = {err_ekf_vel.mean():.4f} m/s")
print(f"UKF time = {t_ukf:.2f}s, RMSE pos = {err_ukf_pos.mean():.3f} m, "
f"RMSE vel = {err_ukf_vel.mean():.4f} m/s")
このシミュレーションで典型的に得られる結果は、EKFとUKFのRMSEがほぼ同等（位置で数メートル、速度で数 mm/s 程度）になり、UKFのほうが計算時間で1.5〜3倍ほど遅くなる、というものです。LEO衛星の2体問題は「弱い非線形」に分類でき、初期誤差が小さい場合には1次線形化（EKF）でも十分な精度が出るため、UKFの優位性が見えにくいのです。
EKFが苦しむ「強非線形」シナリオ
初期誤差を意図的に大きくし、$\sigma_{\text{pos}} = 10\,\text{km}$、$\sigma_{\text{vel}} = 50\,\text{m/s}$ ぐらいに振ると、EKFは線形化点が真値から大きくずれ、共分散が現実の不確かさを正しく反映できなくなって発散の方向に傾きます。一方UKFはシグマポイント全体で広い範囲を覆うため、初期誤差が大きくても収束しやすい傾向があります。
import numpy as np
import matplotlib.pyplot as plt
# 大きな初期誤差
x0_bad = x_true + np.array([10000, -5000, 3000, 50.0, -25.0, 15.0])
P0_bad = np.diag([1e8, 1e8, 1e8, 2500, 2500, 2500]) # 位置10km, 速度50m/s
xs_ekf_b, _ = run_ekf(x0_bad, P0_bad, Q, R_obs, zs, obs_times, dt_prop, r_gs)
xs_ukf_b, _ = run_ukf(x0_bad, P0_bad, Q, R_obs, zs, obs_times, dt_prop, r_gs)
err_ekf_b = np.linalg.norm(xs_ekf_b[:, :3] - xs_true[:, :3], axis=1)
err_ukf_b = np.linalg.norm(xs_ukf_b[:, :3] - xs_true[:, :3], axis=1)
t_axis = np.arange(len(err_ekf_b)) * dt_prop / 60 # 分単位
plt.figure(figsize=(10, 5))
plt.semilogy(t_axis, err_ekf_b, 'r-', lw=1.5, label='EKF (large init error)')
plt.semilogy(t_axis, err_ukf_b, 'b-', lw=1.5, label='UKF (large init error)')
plt.xlabel('Time [min]')
plt.ylabel('Position error [m]')
plt.title('EKF vs UKF with large initial uncertainty')
plt.legend(); plt.grid(True, which='both', alpha=0.3)
plt.tight_layout()
plt.savefig('ekf_vs_ukf_large_init.png', dpi=150, bbox_inches='tight')
plt.show()
この対数プロットから二点が読み取れます。第一に、両フィルタとも開始直後は初期誤差（数キロメートル）から減衰しますが、その減衰速度がUKFの方が速い——UKFは観測を反映してすばやく誤差を下げ、数分以内に数メートル台に到達します。第二に、EKFは収束に時間がかかるか、運が悪いと中盤で誤差が増大に転じることがあります。これは「線形化点が真値から遠い → ヤコビアンが正しくない → 共分散と平均がさらに歪む」というEKFの典型的な発散シナリオの萌芽です。
シグマポイントの可視化
UKFの「シグマポイントが分布を覆う」様子を見るため、ある時刻の予測前後でシグマポイントを描いてみます。
import numpy as np
import matplotlib.pyplot as plt
# 適当な時点の状態と共分散を取り、シグマポイントを取得
x_demo = xs_ukf[100].copy()
P_demo = Ps_ukf[100].copy() * 1e6 # 可視化のため拡大
chi, Wm, Wc = sigma_points(x_demo, P_demo)
chi_next = np.array([rk4_step(s, 60.0) for s in chi]) # 60秒先まで伝搬
# 位置のxy平面射影
fig, ax = plt.subplots(1, 2, figsize=(12, 5))
ax[0].scatter(chi[1:, 0]-x_demo[0], chi[1:, 1]-x_demo[1], c='blue', s=60,
label='sigma points (before)')
ax[0].scatter([0], [0], c='red', s=120, marker='*', label='mean')
ax[0].set_title('Sigma points (before propagation)')
ax[0].set_xlabel('Δx [m]'); ax[0].set_ylabel('Δy [m]')
ax[0].axis('equal'); ax[0].legend(); ax[0].grid(True, alpha=0.3)
x_next_mean = (Wm[:, None] * chi_next).sum(0)
ax[1].scatter(chi_next[1:, 0]-x_next_mean[0], chi_next[1:, 1]-x_next_mean[1],
c='blue', s=60, label='sigma points (after)')
ax[1].scatter([0], [0], c='red', s=120, marker='*', label='mean')
ax[1].set_title('Sigma points (after 60s propagation)')
ax[1].set_xlabel('Δx [m]'); ax[1].set_ylabel('Δy [m]')
ax[1].axis('equal'); ax[1].legend(); ax[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('sigma_points_propagation.png', dpi=150, bbox_inches='tight')
plt.show()
このグラフから、シグマポイントが平均周りに楕円的に配置されており、60秒の軌道伝搬の後も「楕円」を保ちつつ向きと縦横比が変わっていく様子が見えます。これがUKFが「分布の歪み」を捉えている直接の証拠です。EKFはこの伝搬を「中心点での接平面」で代用するため、伝搬距離が長いほど誤差が蓄積していきます。シグマポイントの加重平均からは、変換後の真の平均（モーメント）が線形化よりも正確に得られるわけです。
スケーリングパラメータ $\alpha$ の影響
最後に、UKFの $\alpha$ をスイープして RMSE がどう変わるかを見ます。
import numpy as np
alphas = [1e-4, 1e-3, 1e-2, 1e-1, 0.5, 1.0]
results = []
for a in alphas:
xs_u, _ = run_ukf(x0_err, P0, Q, R_obs, zs, obs_times, dt_prop, r_gs,
alpha=a, beta=2.0, kappa=0.0)
err = np.linalg.norm(xs_u[:, :3] - xs_true[:, :3], axis=1).mean()
results.append(err)
print(f"alpha = {a:.0e}, RMSE pos = {err:.3f} m")
$\alpha = 10^{-3}$ 前後が最も小さなRMSEを与え、$\alpha = 1$ に近づくとシグマポイントが広がりすぎて精度が落ちる傾向があります。逆に $\alpha$ を小さくしすぎると数値的にシグマポイントが平均に潰れて情報量が落ち、これも精度劣化を招きます。実用的には $\alpha = 10^{-3}$ をデフォルトにし、シミュレーションで $\pm$ 1桁ほど振って様子を見るのが定石です。
計算時間の状態次元スケーリング
EKFとUKFの計算量 $O(n^3)$ がどう効くかを、状態次元を仮想的に増やして計測してみます。ここでは衛星状態 6次元に「ダミー状態」を加えて拡張します。
import numpy as np
import time
import matplotlib.pyplot as plt
def dummy_step(x, dt):
"""ダミー状態を含む拡張ダイナミクス"""
x_new = x.copy()
x_new[:6] = rk4_step(x[:6], dt)
# 残りは単なるランダムウォーク
return x_new
ns = [6, 10, 15, 20, 30, 50]
t_ekfs, t_ukfs = [], []
for n in ns:
x0_ext = np.concatenate([x_true, np.zeros(n-6)])
P0_ext = np.eye(n) * 1.0
# 簡易ベンチ: 100ステップ予測のみ計測
def ekf_pred_only(x, P):
for _ in range(100):
eps = 1e-3
F = np.zeros((n, n))
for j in range(n):
e = np.zeros(n); e[j] = eps
F[:, j] = (dummy_step(x + e, 1.0) - dummy_step(x - e, 1.0))/(2*eps)
x = dummy_step(x, 1.0)
P = F @ P @ F.T + 1e-9*np.eye(n)
return x, P
def ukf_pred_only(x, P):
for _ in range(100):
chi, Wm, Wc = sigma_points(x, P)
chi_p = np.array([dummy_step(s, 1.0) for s in chi])
x = np.sum(Wm[:, None]*chi_p, axis=0)
d = chi_p - x
P = (Wc[:, None, None]*d[:, :, None]*d[:, None, :]).sum(0) + 1e-9*np.eye(n)
return x, P
t0 = time.perf_counter(); ekf_pred_only(x0_ext, P0_ext); t_e = time.perf_counter()-t0
t0 = time.perf_counter(); ukf_pred_only(x0_ext, P0_ext); t_u = time.perf_counter()-t0
t_ekfs.append(t_e); t_ukfs.append(t_u)
print(f"n = {n:3d}: EKF = {t_e:.3f} s, UKF = {t_u:.3f} s")
plt.figure(figsize=(9, 5))
plt.loglog(ns, t_ekfs, 'o-', label='EKF (numerical Jacobian)')
plt.loglog(ns, t_ukfs, 's-', label='UKF')
plt.xlabel('State dimension n')
plt.ylabel('Time for 100 prediction steps [s]')
plt.title('EKF vs UKF: time scaling with state dimension')
plt.legend(); plt.grid(True, which='both', alpha=0.3)
plt.tight_layout()
plt.savefig('ekf_ukf_scaling.png', dpi=150, bbox_inches='tight')
plt.show()
両者とも傾きはおおむね $O(n^3)$ に近いですが、EKFは「ヤコビアン計算のために $2n$ 回 dummy_step を呼ぶ」分が次元増に従って効いてきます。UKFは「シグマポイント $2n+1$ 個を伝搬」する分が同じく次元増で効きます。解析的ヤコビアンが手に入ればEKFが有利、入手不可・数値ヤコビアンしか使えないなら UKFがアルゴリズム的に同等以下になることが多い、という相場感がここから読み取れます。実際の現場ではこの「ヤコビアンを手で書く労力」を込みで比較し、UKFを選ぶケースが増えています。
最後に、これらの結果を1枚にまとめて、本記事の議論を閉じます。
まとめ
本記事では、非線形カルマンフィルタの二大アプローチであるEKFとUKFを、衛星軌道のレンジ＋ドップラ追跡問題を通して並べて比較しました。
- 線形カルマンの限界: 状態遷移と観測が線形でないと、事後分布はガウスでなくなり、$\bm{F}, \bm{H}$ が定義できない。衛星軌道、GPS/INS、姿勢推定、SLAM のすべてに該当
- EKF: 非線形関数を現在の推定値で1次テイラー展開し、ヤコビアン $\bm{F}, \bm{H}$ で共分散を伝搬。実装は素直だが、非線形性が強い・初期誤差が大きいと「線形化点ずれ→共分散圧縮→観測無視→さらに線形化点ずれ」の悪循環で発散しうる
- UKF: $2n+1$ 個のシグマポイントで分布を表現し、非線形関数にそのまま通して再構成する。ヤコビアン不要で実装が軽い。スケーリング $\alpha, \beta, \kappa$ のチューニングが入るが、$\alpha=10^{-3}, \beta=2, \kappa=0$ が万能デフォルト
- 計算量: EKFは $O(n^3)$（ヤコビアン関連）、UKFも $O(n^3)$（コレスキー）。実装の手間を入れると、ダイナミクスが複雑なほどUKFの「ブラックボックス性」が効く
- 派生: Cubature KF（$\alpha, \beta, \kappa$ のチューニング不要、高次元に強い）、Gauss-Hermite QKF（高精度だが点数が次元に対し指数増）。シグマポイントカルマンの一族として整理できる
- 粒子フィルタとの位置関係: PFは分布の形を仮定せず多峰や非ガウスも扱えるが粒子数の呪いがある。単峰ガウスで近似可能な問題（衛星軌道、姿勢、車両）ではUKF/CKFが第一選択
- 応用: GPS/INS、衛星軌道決定、ドローン姿勢、ロボットSLAMはいずれもEKF/UKFが中核。ROSの
robot_localizationが代表的な汎用実装 - 衛星軌道シミュレーション結果: 初期誤差が小さければEKFとUKFは同等。誤差が大きい・観測が疎・ダイナミクスが強非線形になるほど、UKF（および派生）が安定して優位になる
次のステップとして、以下の記事も参考にしてください。