デジタルフィルタ入門 #6｜EKF と UKF の設計：線形化とシグマ点で非線形の対象を推定する

はじめに

#5 のカルマンフィルタは、対象が線形（状態の定数倍と足し算だけで書ける）という前提で最良の推定を返しました。等速モデルの位置と速度、IMU（ジャイロと加速度センサを 1 つにした部品）の傾き θ とジャイロのバイアス b（ゼロ点のずれ）は、どちらも線形の式で書けました。

ところが、IMU の式が線形で書けたのは傾きが小さいときだけです。加速度センサの出力は、本当は θ そのものではなく g sin θ（g は重力加速度）です。#5 では θ が小さいとして sin θ ≈ θ と置く小角近似を使い、出力を角度に直してから入力しました。この近似のまま、0° から 80° まで傾ける動きに線形のカルマンフィルタをかけたのが次の図です。

見るのは右の図の 80° の区間です。線形のカルマンフィルタは約 24° 低い値で止まり、そのまま動きません。フィルタの式が壊れているのではなく、「出力は gθ に比例する」というモデルの言うとおりに、測定へ追従した結果です。破線は、同じセンサの値に、この記事で導く EKF をかけた結果で、誤差の RMS（2 乗平均の平方根）は 0.05° ほどです。

線形でない対象は、ほかにもたくさんあります。レーダーのように距離と方位で測るセンサでは、測定は位置の平方根や逆正接になります。回転が入る姿勢の計算も、車輪の動きも、電池の電圧も線形ではありません。

この記事のゴールは、線形でない対象に対して「EKF か UKF か」を根拠を持って選び、設計して、正しく動いているかを数字で判定できるようになることです。EKF は対象を推定値のまわりで線形に近似する方法、UKF は代表の点を流して分布を運ぶ方法です。2 つの導出を 1 段ずつ追い、線形化が破れる条件を「曲率 × 分散」の式で求め、それを 2 つの設計例とモンテカルロ（乱数を変えて何度も試す計算）で確かめます。実機は使わず、すべて計算と数値シミュレーションで示します。

この記事で分かること： EKF と UKF が線形のカルマンフィルタの何を置き換えたのか、ガウス分布を線形でない関数に通すと何が起きるのか、線形化がどこで破れるのか、シグマ点（分布を表す代表点）でなぜ平均と共分散が合うのか、どちらを選ぶべき条件は何か、が分かります。

読み終えたときにできること： 線形でない f と h からヤコビ行列（偏微分を並べた行列。EKF の章で定義）を手で書き、数値微分で検算し、EKF を C で、UKF を Python で動かせます。角度の差の巻き戻しを正しく扱い、NIS と NEES（真の誤差を P で割って大きさをそろえた検証量）で設計を判定できます。

🧭 EKF・UKF とは

EKF と UKF は、線形でない対象の状態を、測定の列から再帰的に推定するアルゴリズムです。線形のカルマンフィルタの「予測と更新の 2 段」はそのまま使い、線形でない関数の扱い方だけを変えます。定義を 1 文ずつ書くと、次のとおりです。

語をほどきます。

• 線形でない（非線形）：f(2x) が 2f(x) にならない関数。sin x、x²、√x、掛け算を含む式など
• テイラー展開：関数を、ある点のまわりで 1 次、2 次、3 次の項の和に分けて書く方法。1 次の項だけを残すのが線形化で、曲線をその点の接線で置き換えることに当たる
• 代表点：分布の平均と広がりを、少ない個数の点と重みで言い表したもの

どちらも分布をガウス分布（釣り鐘形の分布）で表し続けるガウス近似の推定器です。線形でない関数に通した分布は、本当はガウス分布ではなくなります。それを平均と共分散だけで表すので、#5 のカルマンフィルタのような「分散が最小」という性質は成り立ちません。どちらも近似で、動くかどうかは対象と不確かさの大きさ次第です。どれだけ大きいと破れるのかを、後の章で式にします。

何を渡すと何が返るのか

#5 の表との違いだけを並べます。推定値 x̂ と不確かさ P が返ることは同じです。

偏微分（ほかの変数を止めて 1 つの変数だけで微分すること）とヤコビ行列は、EKF の章で定義します。

名前の由来

EKF の「拡張」は、線形のカルマンフィルタを線形でない対象へ広げたことを指します。1960 年代初め、NASA エイムズ研究センターの Stanley Schmidt らは、アポロの月周回航法の研究でカルマンフィルタを使い始めました。最初は決めておいた基準の軌道のまわりで線形化していましたが、そのときどきの推定値のまわりで線形化し直したほうが、真の状態に近いので近似が保たれると考えて式を変えました。この変更が、のちに extended Kalman filter と呼ばれるようになりました（McGee と Schmidt の回想、NASA TM-86847）。

UKF は、Simon Julier と Jeffrey Uhlmann が 1997 年に発表しました。Uhlmann の回想によると、研究室で「Uhlmann フィルタ」と呼ばれ始めたので別の名前を探していて、誰かの机の上の無香料（unscented）の制汗剤が目に入ったのが由来です。技術的な意味のある語ではありません。

予測と更新の 2 段は同じ

処理の流れは #5 と同じループです。箱の中の計算だけが変わります。

A[n] と C[n] は、ステップごとに作り直す線形化の行列です。線形のカルマンフィルタと違って毎回値が変わり、推定値にも依存します。これが #5 の「K と P は測定を見る前に計算できる」が成り立たなくなる理由です。

前提

次の 3 つを前提にします。

• ノイズは加法：w と v は、関数の外で足し算される（この記事の式の形）
• ノイズは平均 0 の白色：#5 と同じ。Q と R が分かっている
• ガウス近似が成り立つ程度の非線形：1 ステップの不確かさの範囲で、分布が釣り鐘形から大きく崩れない

3 つ目が崩れて分布の山が 2 つに割れるような対象は、次回 #7 のパーティクルフィルタの出番です。

📐 非線形の状態空間表現

#5 の状態空間表現の行列の掛け算を、一般の関数に置き換えます。

f が今の状態から次の状態を計算する関数、 h が状態からセンサの値を計算する関数です。 f(x,u) = Ax + Bu 、 h(x) = Cx と置けば #5 の式に戻ります。記号は次のとおりです。N は状態の数、M は測定の数です。

A[n] と C[n] の定義は EKF の章で式にします。#5 の A と C の役目を引き継ぐので同じ文字を使い、時刻で変わることを [n] で示します。

例 1：大きな傾きの加速度センサ

「はじめに」の例です。状態は #5 と同じ傾き θ とジャイロのバイアス b で、u はジャイロの読み ω です。センサは傾きに応じて重力の成分を出力します。

f は線形のままで、h だけが線形ではありません。このように片方だけが線形でない対象はよくあります。

例 2：距離と方位で測る

レーダーなどは、対象までの距離 r と、対象の見える方向（方位）φ を返します。平面の上で等速で動く物体を、センサを原点とする座標で追うとします。横方向の位置を p_1 、縦方向の位置を p_2 とし、速度を #5 と同じ上の点で書きます。

atan2 は、2 つの座標から −180° から +180° の範囲の角度を返す関数です。こちらは f が #5 の等速モデルの A を 2 次元に並べた線形の式で、h が線形ではありません。

線形のカルマンをそのまま当てられない理由

#5 の 5 本の式には、行列 A と C が直接入っています。線形でない対象では、そもそも A や C に当たる定数の行列がありません。さらに本質的な問題があります。#5 の予測の導出は「ガウス分布を線形の式に通すと、またガウス分布になる」ことを使っていました。線形でない関数に通すと、この性質がなくなります。何が起きるのかを、次の章で 1 次元の例で調べます。

🌀 ガウス分布を非線形関数に通す

EKF と UKF の違いは、「ガウス分布を線形でない関数に通した結果を、どう見積もるか」の違いです。この章の問題を 1 行で書きます。平均 m、分散 σ² のガウス分布に従う x を、y = h(x) に通したとき、y の平均と分散はいくつでしょうか。

真の平均と分散：テイラー展開で調べる

平均 m のまわりでテイラー展開します。テイラー展開は、m からのずれ δ = x − m のべきの和で関数を書く方法です。 h' 、 h'' 、 h''' は x = m での 1 階、2 階、3 階の微分で、1 階は接線の傾き、2 階は曲がり方の強さ（曲率）を表します。

両辺の平均を取ります。δ は平均 0 のガウス分布なので、奇数乗の平均は 0 で、 E[\delta^2] = \sigma^2 、 E[\delta^4] = 3\sigma^4 です。E は平均を取る操作です。

2 項目に注目してください。曲がった関数に対称な分布を通すと、平均がずれます。 片側へ曲がっているので、上側の広がりと下側の広がりが釣り合わなくなるからです。同じ展開で分散も計算できます。

\sigma^4 の項の 1 つ目は、 (h''/2)\delta^2 の分散 (h''/2)^2 \cdot 2\sigma^4 （ \mathrm{Var}[\delta^2] = 3\sigma^4 - \sigma^4 = 2\sigma^4 を使う）です。2 つ目は、1 次の項と 3 次の項の掛け合わせから出ます。

線形化で運ぶと何が残るか

線形化は、1 次の項までで打ち切ることです。 y \approx h(m) + h'\delta の平均と分散は次の値です。

真の値と比べると、平均のずれ h''\sigma^2/2 がまるごと抜けています。 分散も \sigma^4 の項が抜けます。

式で書くと、測定のノイズの分散を R として、線形化が効く条件は次のとおりです。

曲率が大きいほど、入力の不確かさが大きいほど破れます。曲率が 0 なら、どんな分散でも線形化は正確です。線形カルマンが厳密なのは、この場合です。

図で比べる：σ が小さいときと大きいとき

h(x) = sin x に、平均 70° のガウス分布を通します。sin は平均も分散も厳密な式が書けるので、検算に向きます。比べるのは次の 3 つです。

• 真の分布：乱数 10 万点を h に通したもの
• 線形化：接線で運んだ平均と分散のガウス分布
• シグマ点：後の章で説明する 3 個の点を h に通して作ったガウス分布

図の左の列が σ = 5°、右の列が σ = 30° です。上の段は関数と接線、下の段は出力の分布で、縦線が 3 つの平均です。左右で、接線が曲線から離れるかどうかと、下の段の縦線の間隔を見てください。

σ = 30° の分布は、y = 1 で急に切れる形をしています。sin が 1 を超えないので、ガウス分布の形はもう残っていません。それでも、平均と分散という 2 つの数だけは、シグマ点でかなり正確に運べています。数字にしたのが次の表です。

ずれの式を当てはめます。σ = 5° のとき \sin 70° \times (5°)^2/2 = 0.00358 で、表の差 0.93969 − 0.93612 = 0.00357 と一致します。σ = 30° では式の値が 0.129、表の差が 0.120 で、高い次の項が効き始めています。

誤差の増え方：σ の 2 乗

σ を連続的に変えて、ずれの大きさを見ます。左の図は真の平均との差で、両対数のグラフです。実線が線形化、細い破線が式 |h''|\sigma^2/2 、点線がシグマ点です。赤い線は、この記事の例の加速度センサのノイズの大きさ（g を単位にして 0.004）です。右の図は、分散が真の値の何倍に見積もられるかです。実線がいつ赤い線を越えるかと、右の図で線が 1 から離れ始める σ を見てください。

線形化のずれが赤い線に届く σ は、条件の式から \sigma = \sqrt{2\sqrt{R}/|h''|} で、平均 70° なら 5.3°、平均 30° なら 7.2° です。曲率の大きい 70° のほうが、早く破れます。

線形化の分散の比は、平均 70° で 1 を下回り、平均 30° で 1 を上回っています。どちらへ外れるかは、 h''^2/2 + h'h''' の符号で決まります。平均 70° の例のように小さく見積もる側へ外れると、フィルタは自分の不確かさを実際より小さく信じることになります。

2 次元の例：極座標から直交座標へ

分布の形の崩れは、2 次元で見るともっとはっきりします。極座標（距離と方位で位置を表す座標）で、距離 1 m、方位 90° にある物体を測り、距離の標準偏差が 2 cm、方位の標準偏差が 2° または 15° だとします。この測定を直交座標へ移すと、点の雲はどんな形になるでしょうか。線形化は p_1 = r\cos\phi 、 p_2 = r\sin\phi の接線で運びます。シグマ点は、(r, φ) の 2 次元で 5 個の点を作って変換します。図の楕円は、それぞれの共分散の ±1σ の範囲です。左右で、楕円の中心が点の雲の真ん中に来ているかを見てください。

σ = 15° の真の平均は p_2 = 0.966 m で、線形化は 1.000 m のままです。ずれの 3.4 cm は、距離の標準偏差 2 cm を超えています。 p_2 の分散も、真の値 0.00259 に対して線形化は 0.00040 で、6 分の 1 以下です。シグマ点は 0.966 m、0.00267 で、ほぼ真の値です。

ここで「曲率 × 分散」を使ってみます。距離の円は半径 r なので曲率は 1/r で、方位の広がりは幅 r\sigma_\phi に当たります。ずれの大きさは (1/r)(r\sigma_\phi)^2/2 = r\sigma_\phi^2/2 です。これと距離のノイズ \sigma_r の比を表す、次の数を定義します。

ρ（ロー）は、ずれと距離のノイズの比の 2 倍です。左の図で ρ = 0.06、右の図で ρ = 3.4 です。ρ が 1 より十分に小さければ線形化は効きます。この数は、設計例 2 で主役になります。

線形化の弱点が式と図で分かりました。次の章では、この線形化を使い切る EKF を導きます。

🧮 EKF の導出

EKF の考え方は単純です。線形でない関数を、毎ステップ、推定値のまわりの接線で置き換え、#5 の導出をそのまま繰り返します。

ヤコビ行列

変数が 2 つ以上ある関数の接線の傾きは、偏微分で表します。偏微分は、ほかの変数を止めて 1 つの変数だけで微分することで、 \partial h/\partial p_1 のように書きます。出力の各成分を、入力の各成分で偏微分して行列に並べたものをヤコビ行列と呼びます。1 変数の h' の多変数版です。

行が出力、列が入力に対応します。線形の関数 Cx のヤコビ行列は C そのものです。EKF では、ステップごとの推定値を入れた 2 つのヤコビ行列を使います。

縦棒の右下は「この値で計算する」という印です。A[n] は 1 つ前の更新の結果 \hat{x}[n-1] で、C[n] は予測の結果 \hat{x}^-[n] で計算します。その時点で手元にあるいちばん新しい推定値を使う、という意味です。

予測の 2 本を導く

状態方程式の f を、 \hat{x}[n-1] のまわりでテイラー展開し、1 次で打ち切ります。e は #5 と同じ推定誤差 e = x - \hat{x} です。

w の平均は 0 で、e の平均も 0 と見なすので、状態の予測は 1 行目だけです。

予測そのものには線形化を使いません。 線形でない f をそのまま計算します。上の式からこの式を引くと、誤差の式が線形になります。

#5 の誤差の式と同じ形なので、共分散の式も同じです。

更新の 3 本を導く

観測方程式の h を、 \hat{x}^-[n] のまわりで展開します。

測定と予測の差、つまりイノベーション ν は、線形でない h をそのまま使って定義します。

2 行目は #5 の y - C\hat{x}^- = Ce^- + v と同じ形です。したがって、#5 の最小分散の導出（トレースを K で微分して 0 と置く手順）が、C を C[n] に変えるだけで全部通ります。結果は次の 3 本です。

状態は線形でない式で運び、共分散は線形化で運ぶ。これが EKF の全体です。前の章で見た弱点は、共分散の側と、平均のずれ h''\sigma^2/2 が式のどこにも入っていないことに現れます。

手でヤコビ行列を出す

距離と方位の例で C[n] を計算します。距離 r = \sqrt{p_1^2 + p_2^2} を p_1 で微分すると、外側の平方根と内側の 2 乗の微分を掛けて 2p_1/(2r) = p_1/r です。方位は \tan\phi = p_2/p_1 の両辺を微分して次の値になります。

速度の成分は h に入っていないので、その列は 0 です。

例えば予測の位置が (3 m, 4 m) なら、r = 5 m で次の行列です。

1 行目は「物体へ向かう単位ベクトル」（長さ 1 で向きだけを表すベクトル）で、その向きに 1 m 動けば距離が 1 m 変わるという意味です。2 行目の大きさは \sqrt{0.16^2 + 0.12^2} = 0.2 で、これは 1/r です。遠いほど、同じ 1 m の動きが見える角度は小さくなります。

手計算で 1 ステップ回す

#5 の手計算と同じ趣旨で、傾きの例を手で 1 周します。切りのよい数字にするため、T = 1 s、g = 9.8 m/s²、ω = 0 とします。角度は rad（ラジアン）です。

• 出発点： \hat{x}[0] = [\,0.5236\ \ 0\,]^{\mathsf{T}} （θ = 30°、b = 0）、 P[0] = \mathrm{diag}(0.01,\ 0.001) （diag は対角成分を並べた行列）
• Q = diag(0.001, 0.0001)、R = 0.1
• 測定：y[1] = 5.4 m/s²

f は線形なので A[1] は定数です。

① 状態の予測。 ω と b が 0 なので、θ は変わりません。

② 共分散の予測。 バイアスの不確かさが、T を通じて角度の不確かさへ流れ込み、非対角が負になります。b が大きいと θ の予測が小さくなる関係だからです。

③ 線形化。 ここが EKF だけの段です。予測の θ で h とヤコビ行列を計算します。

④ ゲイン。 CP^-C^{\mathsf{T}} は 8.487² × 0.012 = 0.8644 です。

K の単位に注意してください。1 行目は「測定の 1 m/s² を何 rad に直すか」で、1/8.487 = 0.118 に近い値です。測定の単位を角度へ変換する働きが、K に入っています。

⑤ 状態の更新。 イノベーションは 5.4 − 4.9 = 0.5 m/s² です。

θ は 30° から 33.03° へ動きました。測っていない b も、非対角を通じて動いています。

⑥ 共分散の更新。

θ の分散は 0.012 から 0.0012 へ、約 1/10 に減りました。

ここで近似の誤差が 1 つ見えます。更新後の θ を本物の h に入れると 9.8 sin(0.5764) = 5.341 ですが、線形化の直線は 4.9 + 8.487 × 0.0528 = 5.348 と言っていました。差の 0.007 が、接線と曲線の違いです。今の例では小さい値ですが、これが積み重なると EKF は自分の共分散を信じすぎるようになります。

EKF の式はこれで全部です。次の章では、接線を使わずに分布を運ぶ UKF を導きます。

🎯 UKF の導出

Julier と Uhlmann の出発点は、「線形でない関数を近似するより、確率分布を近似するほうが易しい」という考えです。関数は接線に置き換えず、そのまま使います。その代わり、分布を少数の点で表し、点を関数に通します。この点の組を関数に通して平均と共分散を作り直す計算を、アンセンテッド変換（UT）と呼びます。

シグマ点の作り方

平均 \hat{x} 、共分散 P の N 次元の分布を、次の 2N + 1 個の点で表します。点の記号は、状態 x と区別して \mathcal{X}_i と書きます。

L_i は P の行列の平方根 L の i 番目の列で、 LL^{\mathsf{T}} = P を満たします。数の平方根の行列版です。γ は点をどれだけ遠くへ置くかを決める数です。

重みは、平均用の W^{(m)} と共分散用の W^{(c)} の 2 組を使います。Wan と van der Merwe の形で書くと、次のとおりです。

添字の i は 1 から 2N までです。式に出てくる 3 つの調整の数は次の意味です（#5 の αβ フィルタの α、β とは別のものです）。

定番の値の組み合わせは後で検算します。先に点の配置を図で見ます。左は 1 次元で、N + λ = 3 の 3 個の点です。右は 2 次元で、#5 の手計算に出てきた傾いた楕円に 5 個の点を置いた例です。矢印の向きと長さが、行列の平方根の列です。

どの配置も、平均に対して点対称です。この対称性が、次の小節の結果を生みます。

なぜ 2 次まで合うのか：1 次元で確かめる

N = 1 で調べます。分布は平均 m、分散 σ² で、点は m と m \pm \gamma\sigma です。重みを W_0 、 W_1 （両側に同じ値）と書くと、 W_0 + 2W_1 = 1 、 2W_1\gamma^2 = 1 です。

まず、点の組そのものが分布の統計を再現していることを確かめます。

平均と分散が合いました。次に、点を h に通して重み付きで平均します。各点を m のまわりでテイラー展開すると、 +\gamma\sigma の点と -\gamma\sigma の点で奇数次の項が打ち消し合います。

\bar{y} は点から求めた出力の平均です。真の平均の式と比べると、2 項目の h''\sigma^2/2 が、微分を 1 度も計算せずに入っています。 線形化が失った平均のずれを、点が曲線の上を通ることで拾ったわけです。この結果は γ によらず成り立ちます。4 次の項は、真の値の h''''\sigma^4/8 = 3h''''\sigma^4/24 と比べると、 \gamma^2 = 3 のとき一致します。これが Julier らの N + κ = 3 の選び方の根拠で、 \gamma^2\sigma^4 が 4 次のモーメント（ずれの 4 乗の平均）を表していて、ガウス分布の値は 3\sigma^4 だからです。

分散も同じように展開します。点から求めた出力の分散を \bar{V} と書きます。N = 1 では \gamma^2 = \alpha^2(1+\kappa) なので、次の式になります。

真の分散の \sigma^4 の項（ h''^2\sigma^4/2 + h'h'''\sigma^4 ）と比べると、次の 2 つの条件が出ます。

1 つ目の条件が β の出どころです。α が小さく κ = 0 なら、β = 2 で h''^2 の項が合います。Wan と van der Merwe の「ガウス分布なら β = 2 が最適」は、この形で確かめられます。

ここから分かるとおり、α = 1、β = 2、κ = 3 − N を同時に使ってはいけません。 N = 1 なら α²κ + β = 4 で、分散を大きく見積もります。σ = 30° の sin の例で、4 つの組を計算したのが次の表です。真の値は平均 0.81932、分散 0.05007 です。

1 行目がいちばん真の値に近く、最後の行は分散が 1.6 倍です。σ = 2° では、上の分散の式と数値計算が 5 桁一致しました。

Julier らの言葉で、結果を整理します。シグマ点は、平均と共分散を 2 次まで正しく計算します。 線形化は平均を 1 次までしか運べません。ただし、共分散の \sigma^4 の項は α、β、κ の選び方に左右されるので、基本の点の組の共分散の精度は線形化と同じ次数です（Julier 2004）。UKF の強みの本体は平均で、共分散の改善は調整の数をうまく選んだときの上積みです。

行列の平方根：コレスキー分解

P の平方根は、コレスキー分解で計算します。対称で、どの向きの分散も正である行列（正定値の行列）を、下三角の行列 L（対角より右上がすべて 0 の行列）と転置の積 P = LL^{\mathsf{T}} に分ける計算です。2 × 2 なら手で書けます。

計算量は N の 3 乗に比例し、掛け算と平方根だけで済みます。平方根の中が負になると計算が止まるので、P が正定値でなくなると UKF は動きません（落とし穴の章で扱います）。

予測

UKF の 1 ステップを、#5 の 5 本に対応させて書きます。前のステップの \hat{x} と P からシグマ点 \mathcal{X}_i を作り、1 個ずつ f に通して、重み付きの平均と共分散を取ります。

更新

\hat{x}^- と P^- からシグマ点を作り直します。Q が足された分布の点を使うためです。点を h に通します。

\hat{y} は測定の予測です。EKF の h(\hat{x}^-) に当たりますが、曲率の効果が入っています。次に、出力どうしの共分散 S と、状態と出力の交差共分散 P_{xy} （状態のずれと出力のずれがどれだけ連動するかを表す行列）を計算します。

カルマンゲインと更新は次の 3 本です。

P_{xy} は、線形の場合の P^-C^{\mathsf{T}} に当たります。線形なら \mathcal{Y}_i - \hat{y} = C(\mathcal{X}_i - \hat{x}^-) なので、 P_{xy} = P^-C^{\mathsf{T}} で、K は #5 の式に戻ります。P の式も同じです。h が線形のときに UKF と線形カルマンを同じデータで回すと、状態の差は 10⁻¹⁵ 程度の丸め誤差だけでした。

UKF にヤコビ行列は 1 つも出てきません。f と h の中身が分岐を含んでいても、微分が書けなくても、関数として呼べれば動きます。

手計算の例を UKF で回す

EKF の手計算と同じ数字で比べます。N = 2、α = 1、β = 0、κ = 1 なので N + λ = 3 で、重みは W_0 = 1/3、ほかの 4 個が 1/6 です。 3P^- をコレスキー分解した L を ± に振ると、シグマ点の θ は 0.5236、0.7133、0.5236、0.3339、0.5236 の 5 個です（行列の平方根の 2 列目は θ の成分が 0 なので、その 2 点の θ は 0.5236 のままです）。

g sin θ に通すと 4.9、6.413、4.9、3.211、4.9 で、重み付きの平均は \hat{y} = 4.871 です。EKF の 4.9 より 0.029 小さく、これは平均のずれの式 h''\sigma^2/2 = -4.9 \times 0.012/2 = -0.029 と一致します。点を通すだけで、曲率の補正が入りました。

あとは同じ手順で、S = 0.9558、K = [0.1059, −0.0088] となり、イノベーションは 5.4 − 4.871 = 0.529 です。更新後の θ は 0.5797 rad = 33.21° で、EKF の 33.03° より 0.18° 大きくなります。

計算量

2 つの計算量を、状態の数ごとに比べます。測定の更新で点を作り直すので、コレスキー分解は 1 ステップに 2 回です。

どちらも行列の掛け算があるので、N に対しては 3 乗で増えます。f が線形なら、予測は #5 の式で計算して、更新だけシグマ点を使えます。設計例の 2 つはどちらも f が線形なので、f の評価は 1 回に減らせます。

ここまでで、EKF と UKF の式がそろいました。次の章では、#1〜#5 の周波数の道具が、線形でないフィルタにどこまで使えるかを見ます。

📊 周波数と制御工学の道具で見る

EKF と UKF のゲインは、状態によって時々刻々変わります。入力を 2 倍しても出力は 2 倍にならないので、線形時不変の前提が崩れ、伝達関数はありません。ただし、動作点（状態がとどまっている点）のまわりの小さな動きだけなら、話は別です。

動作点で凍らせる

傾きの例で、θ がある値 \theta_0 にとどまっていて、そのまわりで小さく動くとします。C[n] はほぼ一定の値 C = [\,g\cos\theta_0\ \ 0\,] になります。C が定数なら、EKF の式は #5 の線形カルマンと同じで、リカッチ方程式（#5 の、共分散を次のサンプルへ送る漸化式）の解である定常ゲイン K に収束します。

ただし、#5 と違って K は \theta_0 の関数です。測定の感度 g\cos\theta_0 が動作点で変わるからです。θ = 0° なら 1 rad の傾きで 9.8 m/s² 変わりますが、θ = 80° では 1.7 m/s² しか変わりません。測定のノイズを角度に直すと、 \sqrt{R}/(g\cos\theta_0) です。同じセンサが、動作点によって別の精度のセンサに見えます。

定常のフィルタを伝達関数で書きます。1 ステップを 1 本につなぐと \hat{x}[n] = \Phi\,\hat{x}[n-1] + K\,y[n] + \cdots （ \Phi = (I-KC)A ）です。測定を g\cos\theta_0 で割って角度に直した値から、 \hat{\theta} までの伝達関数は次の式です。I は単位行列です。

帯域が動く

次の図は、θ = 0°、45°、80° の H_\theta(z) を、#5 の定常カルマンの図と同じ 4 枚で描いたものです。条件は設計例 1 と同じ fs = 200 Hz、Q、R です。左上の振幅の丸印（−3 dB の点）の動きと、右上の群遅延（その周波数の信号が何秒遅れるか）の山の高さを見てください。

K₁ は K の 1 番目の成分です。EKF を θ = 45° に固定して回すと、K は [0.001579, −0.000885] に収束し、この表の計算の元になったリカッチ方程式の解と 3 桁以上一致しました。

見えてくることは 2 つあります。

① フィルタが、センサの感度に合わせて自分で帯域を動かしている。 測定があいまいになる動作点では測定を信じる割合を下げ、ジャイロの積分の側へ寄せます。#5 の「定常ゲインは Q/R の比で決まる」を動作点ごとに当てはめた形で、R が角度換算で 1/\cos^2\theta_0 倍になるので、帯域は cos θ が小さくなるのに合わせて下がります（この例では cos θ の 0.6 乗ほどです）。固定ゲインの相補フィルタには、この動きはありません。

② 制御ループへ入れるなら、位相の遅れも動作点で変わる。 #5 の「制御ループでの効き」と同じ見方をすると、帯域が下がる動作点では同じ周波数の位相の遅れが増えます。θ = 0° で設計した余裕が、80° でも残るとは限りません。線形の設計では 1 点で済んだ確認を、動作の範囲全体で行う必要があります。

感度が 0 になる点

\cos\theta_0 \to 0 、つまり θ が 90° に近づくと、g cos θ は 0 へ向かいます。測定は傾きの変化に反応しなくなり、角度換算のノイズは無限大へ向かいます。K₁ × g cos θ も 0 へ向かい、 H_\theta の帯域は 0 Hz まで下がります。#5 の言葉で言えば、可観測（出力の列から状態を決められること）でなくなる点です。この点で何が起きるかは、検証の章の「UKF でも救えない例」で確かめます。

周波数で見る道具は、動作点ごとの帯域と遅れを読むのに使えます。次の章では、2 つの設計例を要件から順に進めます。

🧪 設計例

手順は #5 と同じ 5 段です。

1. 要件：何を推定したいか、fs、測れる量を書き出す
2. モデル：状態を選び、f と h を物理の式から書く
3. Q と R：#5 と同じ考え方で決める
4. 初期値： \hat{x}[0] と P[0] を、最初の不確かさとして与える
5. 検証：NIS と、シミュレーションなら NEES で判定する

EKF か UKF かを決めるのは、3 段目までで分かる「曲率 × 分散」です。2 つの例で、差が出ない場合と出る場合を見ます。

設計例 1：IMU の大きな傾き

要件： #5 の設計例 1 と同じ IMU（fs = 200 Hz）で、0° から 80° まで傾く動きを推定したい。ジャイロのバイアスは 1 deg/s ある。

モデル： 状態は θ と b で、h(θ) = g sin θ です。角度の単位は rad です。

Q と R： 数値は #5 と同じノイズです。ジャイロの白色ノイズ \sigma_g = 0.5 deg/s、バイアスの 1 サンプルの動き \sigma_b = 0.002 deg/s、加速度センサのノイズ 4 mg（0.0392 m/s²）です。#5 では出力を角度に直してから入れましたが、ここでは m/s² のまま入れます。

初期値： \hat{x}[0] = 0 、P[0] は θ と b の両方に (2°)² です。

UKF の調整の数： N = 2 なので κ = 3 − N = 1、α = 1、β = 0 とします。前の章の 2 つの条件に沿った組で、重みはすべて正です。

どちらが要るかを先に見積もる： 「曲率 × 分散」を計算します。80° の動作点の θ の標準偏差は前の章の表から 0.087° で、曲率は g\sin 80° = 9.66 です。平均のずれは 9.66 \times (0.087°)^2/2 = 1.1×10⁻⁵ m/s² で、測定のノイズ 0.0392 m/s² の約 3500 分の 1 です。1 ステップの不確かさの範囲では、g sin θ はほとんど直線です。 線形化がよく効くので、EKF と UKF に差は出ないはずです。

シミュレーションの信号は、3 秒から 6 秒かけて 80° まで傾け、9 秒間とどまり、6 秒かけて戻します。バイアスは 1 deg/s から始めて、モデルどおりのランダムウォーク（少しずつランダムに動く値）で動かします。上段は線形カルマン（小角近似）の誤差、中段は EKF と UKF の誤差と ±3σ の帯、下段はバイアスの推定です。

中段の帯は、θ = 0° の ±0.09° から 80° の ±0.26° へ広がります。前の章の表の標準偏差 0.027° と 0.087° のほぼ 3 倍で、動作点の分析と一致します。EKF と UKF の差は全区間で最大 0.0004° でした。見積もりのとおり、この例では計算の軽い EKF で十分です。

100 回のモンテカルロの結果は次の表です。RMSE は誤差の 2 乗平均の平方根で、誤差の大きさを 1 つの数で表します。最初の 2 秒を除いています。NIS は #5 の正規化イノベーション 2 乗で、測定が 1 個なら平均 1 が理想です。NEES（正規化推定誤差 2 乗）は、真の値との誤差を P で割って大きさをそろえた量で、不確かさの見積もりが正しければ平均が状態の数（ここでは 2）になります。真の値が要るので、シミュレーションでしか計算できません。式は検証の章で示します。

線形カルマンの NIS は 14 で、真の値を知らなくても設計の誤りを検出できます。

設計例 2：距離と方位で動く物体を追う

要件： レーダーのようなセンサで、平面を動く物体の位置と速度を 10 Hz（T = 0.1 s）で推定したい。

モデル： #5 の等速モデルを、 p_1 と p_2 の 2 組に並べます。f は線形で、A は定数です。

h と C[n] は EKF の章で求めた式です。

Q と R： Q は #5 のゼロ次ホールドの形を、両方の向きに独立に並べます。 \sigma_a = 0.02 m/s² です。R は距離と方位の分散を対角に並べます。

Γ は #5 と同じノイズの入り方を表す行列です。大文字のガンマで、シグマ点の広がりの γ とは別の記号です。

初期値： 最初の測定を直交座標へ変換して位置に使い、速度は 0 で不確かさを (2 m/s)² とします。

UKF の調整の数： N = 4 で κ = 3 − N = −1 にすると、中心の重みが −1/3 になります。重みが負だと共分散が正定値でなくなる恐れがあるので、α = 1、β = 2、κ = 0 とします。λ = 0 で、中心の平均の重みは 0、ほかの 8 個は 1/8、中心の共分散の重みは 2 です。

比べる 3 つ： EKF、UKF に加えて、線形のカルマンで済ませる定番の方法を比べます。測定を先に直交座標 y_c = [\,r\cos\phi\ \ r\sin\phi\,]^{\mathsf{T}} へ変換し、位置を測るセンサとして線形カルマンに入れる方法です。測定の共分散は、測定の値で計算したヤコビ行列 J で R_c = JRJ^{\mathsf{T}} と変換します。以下では「変換＋線形カルマン」と呼びます。

3 つの条件

物体は 1 m/s でセンサの左側を上へ進み、途中で方位の境界 ±180° を横切ります。条件を 3 つ用意します。

• 条件 A：遠い（r ≈ 50 m）、距離も粗い（σr = 1 m）、方位 σφ = 5°。ρ = 0.38
• 条件 B：近い（r ≈ 5 m）、距離が正確（σr = 2 cm）、方位 σφ = 5°。ρ = 1.9
• 条件 C：遠い（r ≈ 50 m）、距離が正確（σr = 2 cm）、方位 σφ = 5°。ρ = 19

図は、ある時刻の測定を 500 回繰り返して直交座標へ変換した点の雲です。横軸の範囲を縦軸より狭くしてあります。条件 C の雲が線のように細く弓なりになっていることを見てください。

\rho は、この弓なりの度合いです。前の章で見たように、距離の円の曲率 1/r と方位の広がりの 2 乗から出ています。

結果：効くのは近さではなく ρ

100 回のモンテカルロの結果です。上の段が位置の誤差の推移、下の段が NEES です。NEES は、フィルタの言う不確かさが実際の誤差と合っているかを表す量で、理想は状態の数の 4 です。条件を左から右へ見て、3 本の線の開き方を比べてください。

RMSE と NEES は 100 回の中央値で、最初の 1 秒を除いています。発散は、最後の時刻の位置誤差が、その条件のいちばん良い RMSE の 5 倍を超えた回数です。

近距離だから UKF が有利、遠距離なら差が無い、とは言えません。 距離だけが違う条件 B と C を比べると、差が大きいのは遠い C です。距離の円の曲率 1/r は遠いほど小さいのですが、方位の誤差が作る横幅 r\sigma_\phi がそれ以上に広がるので、曲率 × 分散は r に比例して増えます。逆に、距離のノイズが粗い A は、同じ 50 m でも差が出ません。

もう少し詳しく見ます。距離 50 m、方位 5° のまま、距離のノイズだけを 5 m から 2 cm まで変えたのが次の図です。横軸が ρ、縦軸が NEES の中央値です。縦線 ρ = 1 の前後で、線がどう開くかを見てください。乱数の列を変えて計算し直しているので、ρ が同じ行でも前の表と値は少し違います。NEES が大きい領域ほど、外れた試行の影響でばらつきます。

変換＋線形カルマンが崩れる理由は単純です。変換した測定の平均は、曲率の効果で e^{-\sigma_\phi^2/2} 倍に縮み、センサの側へずれます。距離 50 m、方位 5° なら 0.19 m です。それなのにフィルタは、距離の向きの標準偏差を 2 cm と信じています。測定の変換を補正する方法は、Lerro と Bar-Shalom の論文が扱っています。一方、RMSE は NEES ほど悪くありません。推定の値そのものより、不確かさの見積もりが壊れているのです。

選び方： ρ が 1 より十分に小さければ、3 つのどれでも動きます。その場合は、線形の式で済む変換＋線形カルマンが素直です。ρ が 1 前後から上は EKF か UKF にします。ρ が 10 を超える領域では UKF にも余裕がなく、NEES は理想の 10 倍以上です。ガウス近似そのものが苦しいので、#7 のパーティクルフィルタの候補になります。

設計例 3：電池の残量（概要）

SoC（充電状態。満充電を 100 % とした残量の割合）は直接測れません。電流を積算すれば SoC の変化は分かりますが、#5 のジャイロと同じで、ずれが積み重なります。一方、電池の開放電圧（電流を流していないときの電圧、OCV）は SoC で決まり、その関係は曲線です。

i は電流、 Q_{cap} は電池の容量、 R_0 は内部抵抗です。積算の式は線形で、OCV の曲線が線形でない h です。IMU の例と同じ構図で、OCV の傾きが小さい領域では θ が 90° に近いときと同じく、測定から SoC の情報が得にくくなります。Plett の 2004 年の連作は、この問題に EKF を適用したものです。

✅ 検証

線形カルマンでは、モデルと Q、R が合っていれば P は誤差の大きさを正しく言い当てました。EKF と UKF は近似なので、この性質が崩れていないかを確かめる必要があります。

NIS と NEES

NIS（正規化イノベーション 2 乗）は #5 で使いました。イノベーションだけから計算するので、真の値が分からない実機でも使えます。平均は測定の個数 M です。

シミュレーションでは真の値が分かるので、推定誤差そのものを P で正規化できます。これが NEES（Normalized Estimation Error Squared、正規化推定誤差 2 乗）です。

NIS と同じ考え方で、e が平均 0、共分散 P のガウス分布に従えば、NEES は自由度 N（独立な標準正規分布の 2 乗をいくつ足したか）の χ²（カイ 2 乗）分布に従い、平均は状態の数 N です。

• NEES の平均が N より大きい：フィルタは自信過剰。実際の誤差が P より大きい
• N より小さい：慎重すぎる。実際の誤差が P より小さい
• NEES が発散するのに NIS が普通：測定に見えない成分の誤差が育っている

100 回の平均なら、95 % の範囲は N = 2 で 1.63〜2.41、N = 4 で 3.46〜4.57 です。設計例 1 の 2.02 はこの範囲に入っています。

破る例：初期誤差を大きくする

設計例 2 の条件で、 \sigma_\phi = 2° に下げて（ρ = 0.3）測定の曲率は問題ない状態にし、初期の位置の不確かさだけを大きくします。物体は距離 5 m ほどから出発します。初期の推定値は真の値から標準偏差 e_0 の乱数でずらし、P[0] にも e_0^2 を正直に入れます。#5 で勧めた、不確かさを正直に入れる設定です。

e_0 = 1 m と 5 m で、100 回の位置誤差を重ねたのが次の図です。上の段が EKF、下の段が UKF で、黒い線が中央値です。右上の線の束が、下へ戻っているかを見てください。

最初の 3 秒を除いた値です。発散の基準は前の章と同じで、この表では 0.2 m です。

「曲率 × 分散」で読みます。距離 5 m で初期の位置の標準偏差が 5 m なら、分散は 25 m²、距離の曲率は 0.2 /m で、ずれの見積もりは 2.5 m です。距離のノイズ 2 cm の 100 倍以上で、線形化が破れる条件を大きく超えています。EKF は最初の更新で、外れた場所で計算した C[n] と、とても小さい R で P を一気に縮めます。すると、まだ真の値から遠いのに K が小さくなり、以後は測定を信じません。図の線の束が 1 m 付近に居座るのは、このためです。

UKF のシグマ点は、広い P に応じて遠くへ散らばります。r や φ の曲がりが共分散に入るので、S が大きくなり、P を縮めすぎません。変換＋線形カルマンは、測定そのものが位置を直接教える形なので、ここでは強い方法です。表の最後の行の反復 EKF は、更新のあとの推定値で線形化し直して更新を繰り返す EKF で（ここでは 5 回）、線形化の点が真の値に近づくので、同じ場面に強くなります。

UKF でも救えない例

UKF は線形化の誤りを減らしますが、測定に含まれていない情報は作り出せません。 2 つの例を見ます。

① sin θ の鏡像。 sin 70° = sin 110° なので、1 軸の加速度センサは 70° と 110° を区別できません。θ の初期値を 0、P[0] の θ の標準偏差を 60° として、真の傾きが 70° の場合と 120° の場合を回します。左の図で、線がそれぞれの点線に向かうかを見てください。右の図は別の例で、75° から 90° まで傾けて 15 秒ほどとどめた場合です。

真値 70° で UKF が外れ、真値 120° で EKF が外れました。どちらに落ちるかは初期値と点の広がりで決まり、測定の情報では決まりません。鏡像の値でも NIS は正常です。測定から見ると、どちらも正解だからです。

EKF の最初の更新にも注目してください。真値 70° で、θ̂ は 53.6°、P の標準偏差は 0.23° です。真の値から 16° も離れているのに ±0.23° と言っています。自分の標準偏差の 70 倍の誤りを出しているわけで、初期値の接線の傾き g cos 0° = g で P を縮めた結果です。

② θ = 90° の感度 0。 右の図では、θ = 90° の区間で h の傾きが 0 になります。θ の標準偏差は 0° の 0.027° から、EKF で 0.45°、UKF で 0.47° に広がりました。実際の誤差の RMS は 1.0° と 0.9° で、帯の言う値の約 2 倍です。UKF も、同じように見失います。

対策はフィルタの変更ではなく、測定を足すことです。もう 1 つの軸の加速度 g cos θ を測れば、2 つの出力の組は 0〜360° で 1 通りに決まり、90° でも傾きに感度があります。可観測かどうかはモデルの問題で、推定器では直せません。

検証の道具がそろいました。次の章では、実装で踏みやすい穴をまとめます。

🕳️ 落とし穴

ヤコビ行列の書き間違い

EKF でいちばん多い誤りです。符号や添字を間違えても、フィルタはそれらしい値を出します。平均だけが合っていて、K がわずかに違う状態になるからです。設計の前に、数値微分（少しずらして差を取る近似の微分）と比べて検算します。

import numpy as np

def num_jac(h, x, eps=1e-6):
    y0 = h(x)
    J = np.zeros((y0.size, x.size))
    for i in range(x.size):
        d = np.zeros(x.size)
        d[i] = eps
        J[:, i] = (h(x + d) - h(x - d)) / (2 * eps)
    return J

手で書いた C[n] と num_jac(h, x) を、ランダムな x で 10 回ほど比べ、差が 10⁻⁶ 程度なら合格です。方位のように飛びがある関数では、±180° の境界の近くを避けて試します。

角度の差の巻き戻し

方位は −180° と +180° がつながっています。179° と −179° の差は 2° なのに、そのまま引くと 358° です。この巻き戻しを忘れた例が次の図です。物体がセンサの真左を横切るとき、方位の値は一瞬で 360° 飛びます。上の段が方位、下の段が位置誤差で、線が跳ね上がる時刻を見てください。

巻き戻さない EKF の誤差は、最大 0.26 m から 13.8 m へ増えて、最後まで戻りません。イノベーションに約 2π（360°）が入って、推定が大きく飛ぶからです。巻き戻しの処理は 1 行です。

mod は割った余りです。EKF では、イノベーションの方位の成分をこの関数に通します。UKF では、もう 2 か所が必要です。

• 点の差： \mathcal{Y}_i - \hat{y} の方位の成分も巻き戻す
• 点の平均：179° と −179° の平均は、そのまま足すと 0° になる。方位は sin と cos を重み付きで足して atan2 で戻す円周の平均で計算する

どちらも行わない UKF の誤差は、この例で 1.13 m まで跳ねました。状態に角度が入るときは、状態の差と平均にも同じ処理が要ります。

P が正定値でなくなる

UKF のコレスキー分解は、P が正定値でないと止まります。丸め誤差で P の対称性が崩れたり、更新の引き算で対角が小さくなりすぎたりすると起きます。対策は次の 3 つです。

• 更新のあとに P \leftarrow (P + P^{\mathsf{T}})/2 として対称に戻す
• 対角に小さな値を足す（ジッタ）。 P + \epsilon I の ε を、P の対角より数桁小さく取る
• 平方根そのものを更新していく平方根形の UKF を使う（行列の平方根 L を直接更新する形で、丸めに強い）

α を小さくしすぎる

α を小さくすると、シグマ点は平均のすぐ近くに集まります。 \lambda \approx -N になるので、中心の重み W_0^{(m)} は大きな負の値です。設計例 2 の条件 B で、N = 4、β = 2、κ = 0 として α を変えた結果が次の表です。

失敗はコレスキー分解が止まった回数です。倍精度（double、64 ビット）では α = 0.001 でも動きますが、単精度（float、32 ビット）では 30 回とも止まりました。±100 万倍の重みが打ち消し合う計算なので、float の有効桁約 7 桁では残りません。Wan と van der Merwe の例の値 10⁻³ は、そのまま float に持ち込めない値です。float で動かすなら α を 0.01 以上にするか、この記事の設計例の α = 1 の組を使います。

線形化点の選び方

C[n] の線形化点は \hat{x}^-[n] です。更新の結果 \hat{x}[n] は、測定を入れたぶん真の値に近いはずなので、そこでもう一度線形化し直して更新をやり直す方法があります。これを反復 EKF（IEKF）と呼びます。検証の章の表の 5 回反復では、初期誤差 5 m の発散が 43 回から 0 回になりました。h の計算が反復の回数だけ増えます。A[n] の線形化点は、測定を入れたあとの \hat{x}[n-1] です。

計算量と f の扱い

「UKF は重い」と言われますが、計算量の表のとおり、桁で違うわけではありません。f や h の評価が重い対象（中で数値積分を回す、表引きが多いなど）では、回数の 2N + 1 倍が効きます。f が線形なら予測を #5 の式にして、更新だけシグマ点を使うと軽くなります。

Q で見かけを整えない

Q と R の決め方は #5 と同じです。ただし EKF では、線形化の誤差を隠すために Q を大きくして NEES を理想に近づけることがよくあります。安定化ノイズと呼ばれる手法で、Julier 2004 でも触れられています。発散は止まりますが、帯域が上がってノイズが増えます。先に「曲率 × 分散」を計算して、UKF や測定の追加で直せないかを確かめます。

💻 実装

式をコードにします。IMU の EKF は手で展開した float 版、UKF は汎用の Python 版です。

C：2 状態の EKF

設計例 1 の θ と b です。#5 の kf2_t と同じ見せ方で、P は対称なので 3 個だけ持ちます。

#include <math.h>

typedef struct {
    float T;                 // サンプリング周期 [s]
    float g;                 // 重力加速度 [m/s^2]
    float q11, q22;          // プロセスノイズの共分散 Q の対角
    float r;                 // 観測ノイズの分散 R
    float th, b;             // 推定値（傾き [rad] とバイアス [rad/s]）
    float P11, P12, P22;     // 誤差共分散 P（対称なので 3 個）
} ekf2_t;

float ekf2_step(ekf2_t *f, float gyro, float y)
{
    const float T = f->T;

    // 予測：x^- = f(x, u)、P^- = A P A^T + Q（A = [1 -T; 0 1]）
    float x1 = f->th + T * (gyro - f->b);
    float x2 = f->b;
    float p12 = f->P12 - T * f->P22;
    float m11 = f->P11 - T * (f->P12 + p12) + f->q11;
    float m12 = p12;
    float m22 = f->P22 + f->q22;

    // 線形化：C[n] = [g cos(th^-)  0]
    float c = f->g * cosf(x1);
    float yhat = f->g * sinf(x1);

    // 更新：S = C P^- C^T + R、K = P^- C^T / S
    float S = c * c * m11 + f->r;
    float K1 = m11 * c / S;
    float K2 = m12 * c / S;
    float nu = y - yhat;     // イノベーション

    f->th = x1 + K1 * nu;
    f->b = x2 + K2 * nu;

    // P = (I - K C) P^-。1 - K1 c = r / S なので引き算を避けられる
    float a = f->r / S;
    f->P11 = a * m11;
    f->P12 = a * m12;
    f->P22 = m22 - K2 * c * m12;

    return f->th;
}

gyro は 1 つ前のサンプルからの区間の読み（deg/s でなく rad/s）、y はその時刻の加速度 [m/s²] です。線形の kf2_step との違いは、cosf と sinf の 2 行と、それを使う c、yhat だけです。設計例 1 の値なら、初期化子は次のとおりです。

static ekf2_t ekf = {
    .T = 0.005f, .g = 9.80665f,
    .q11 = 1.904e-9f, .q22 = 1.218e-9f,      // (T sg)^2, sb^2 [rad^2, (rad/s)^2]
    .r = 1.539e-3f,                          // (4 mg)^2 [(m/s^2)^2]
    .P11 = 1.218e-3f, .P22 = 1.218e-3f,      // (2 deg)^2 [rad^2]
};

設計例 1 の信号 6000 サンプルをこのコードに入れると、倍精度で同じ式を回した結果との差は、θ で最大 7×10⁻⁵ 度でした。P も 4 桁まで一致します。

Python：汎用の UKF

シグマ点の生成、予測、更新を関数に分けた汎用の版です。f(x) と h(x) を渡すだけで動きます。

import numpy as np


def sigma_points(x, P, alpha=1.0, beta=2.0, kappa=0.0):
    """シグマ点（列）と重みを返す。x は長さ N、P は N × N。"""
    N = x.size
    lam = alpha**2 * (N + kappa) - N
    c = N + lam
    L = np.linalg.cholesky(c * P)          # c P = L L^T
    X = np.column_stack([x, x[:, None] + L, x[:, None] - L])
    Wm = np.full(2 * N + 1, 0.5 / c)
    Wc = Wm.copy()
    Wm[0] = lam / c
    Wc[0] = lam / c + 1.0 - alpha**2 + beta
    return X, Wm, Wc


def predict(x, P, f, Q, **ut):
    """予測：点を f に通して x^-、P^- を作る。"""
    X, Wm, Wc = sigma_points(x, P, **ut)
    FX = np.column_stack([f(X[:, i]) for i in range(X.shape[1])])
    xm = FX @ Wm
    d = FX - xm[:, None]
    Pm = (d * Wc) @ d.T + Q
    return xm, (Pm + Pm.T) / 2


def update(xm, Pm, y, h, R, residual=np.subtract, ymean=None, **ut):
    """更新：点を h に通して S、P_xy、K を作る。"""
    X, Wm, Wc = sigma_points(xm, Pm, **ut)
    Y = np.column_stack([h(X[:, i]) for i in range(X.shape[1])])
    ym = Y @ Wm if ymean is None else ymean(Y, Wm)
    dY = np.column_stack([residual(Y[:, i], ym) for i in range(Y.shape[1])])
    dX = X - xm[:, None]
    S = (dY * Wc) @ dY.T + R
    Pxy = (dX * Wc) @ dY.T
    K = np.linalg.solve(S.T, Pxy.T).T      # K = P_xy S^-1
    x = xm + K @ residual(y, ym)
    P = Pm - K @ S @ K.T
    return x, (P + P.T) / 2

np.linalg.cholesky は下三角の L を返します（SciPy の scipy.linalg.cholesky は既定で上三角を返すので、置き換えるときは lower=True を付けます）。K は逆行列を作らず、連立方程式として解いています。

設計例 2 の距離と方位なら、巻き戻しの 2 つの関数を渡します。

def wrap(a):
    return (a + np.pi) % (2 * np.pi) - np.pi


def rb_residual(a, b):
    return np.array([a[0] - b[0], wrap(a[1] - b[1])])


def rb_mean(Y, W):
    return np.array([Y[0] @ W, np.arctan2(np.sin(Y[1]) @ W, np.cos(Y[1]) @ W)])


T = 0.1
A = np.eye(4)
A[0, 2] = A[1, 3] = T
Gam = np.array([[T**2 / 2, 0], [0, T**2 / 2], [T, 0], [0, T]])
Q = 0.02**2 * Gam @ Gam.T
R = np.diag([0.02**2, np.radians(5)**2])

f = lambda x: A @ x
h = lambda x: np.array([np.hypot(x[0], x[1]), np.arctan2(x[1], x[0])])

x = np.array([-5.0, -5.0, 0.0, 0.0])
P = np.diag([0.1, 0.1, 4.0, 4.0])
for y in [np.array([7.0, -2.3]), np.array([6.95, -2.32])]:
    x, P = predict(x, P, f, Q)
    x, P = update(x, P, y, h, R, residual=rb_residual, ymean=rb_mean)
print("x =", np.round(x, 4))
print("position sigma =", np.round(np.sqrt(np.diag(P)[:2]), 4))

x = [-4.7804 -5.0272  0.6876  0.08  ]
position sigma = [0.2599 0.2501]

このコードを設計例 2 の 100 サンプルの信号で回すと、図と表を作ったコードとの差は 10⁻¹³ 以下でした。alpha、beta、kappa は predict と update の引数で変えられます。

実務で使うなら、FilterPy（Roger Labbe による Python のライブラリ）に同じ α、β、κ の形の UKF があります。

コードがそろいました。最後に、EKF と UKF が実際にどこで使われているかを見ます。

💡 活用事例

EKF と UKF の使われ方を、「なぜそれなのか」の理由とともに挙げます。

宇宙機の姿勢。 姿勢の計算では、回転を四元数（回転を 4 つの数で表す方法）で持ちます。4 つの数には「大きさが 1」という制約があるので、そのまま EKF にかけると制約が崩れます。小さな回転の誤差を 3 つの数で持つ乗法型の EKF（MEKF）が定番で、Lefferts、Markley、Shuster の 1982 年の論文がよく引かれます。Crassidis と Markley の 2003 年の論文は、これを UKF にして、大きな初期誤差からの収束が速いことを示しています。検証の章の初期誤差の例と同じ傾向です。

GNSS と INS の統合。 GNSS は衛星測位、INS は IMU を積分して位置を求める慣性航法です。INS は速く更新できますが時間とともにずれていき、GNSS は更新が遅い代わりに絶対の位置を返します。#5 の相補の考え方を、回転を含む線形でないモデルに広げたものが、この統合に使われる EKF です。

ドローン。 PX4 の EKF2 は、#5 でも触れた飛行制御ソフトの推定器です。公式の説明によると、推定する状態は 24 個の配列で、四元数、速度、位置、IMU のバイアス、地磁気、風などが入っています。設計例 1 の θ と b を 3 次元へ広げ、測定を増やした形です。

ロボットの SLAM。 SLAM は、自分の位置を推定しながら地図も同時に作る問題です。ロボットの姿勢と目印の位置を 1 本の状態に入れて EKF で解くのが EKF-SLAM で、初期の SLAM の標準の形でした。測定は設計例 2 と同じ距離と方位で、状態の数が目印の数だけ増えるので、共分散の計算量が大きな課題になります。ROS の robot_localization にも、線形でない状態推定のノードが用意されています。

電池の SoC。 設計例 3 の構図です。測定できない SoC を、電流の積算と電圧の線形でないモデルから推定します。

自動車の運転支援。 ADAS（先進運転支援システム）のレーダーは、距離と方位で先行車を測ります。設計例 2 と同じ h で、カメラなどの別のセンサと合わせる場面でも、測定の座標が違うセンサを 1 つの状態へまとめられることがカルマンの形の利点です。

✅ まとめ

• EKF は f と h を推定値のまわりで線形化し、#5 の 5 本の式を A[n]、C[n] で回す。状態は線形でない式で、共分散は線形化で運ぶ
• UKF は分布を 2N + 1 個のシグマ点で表し、点を関数に通して平均と共分散を作り直す。ヤコビ行列は要らず、平均の h''\sigma^2/2 が自動で入る
• 線形化が破れる条件は「曲率 × 分散」。 |h''|\sigma^2/2 が測定のノイズより小さければ EKF で足り、大きければ UKF が効く
• 距離と方位の測定では、効く数は近さではなく \rho = r\sigma_\phi^2/\sigma_r 。ρ が 1 より十分に小さければ 3 方式に差はない
• 大きな初期誤差では EKF は P を縮めすぎて動かなくなることがある。UKF や反復 EKF はこれに強い
• 測定に情報がない対象は、UKF でも救えない。可観測かどうかはモデルの問題で、測定を足して直す
• 検証は NIS に加えて、シミュレーションでは NEES とモンテカルロで行う。角度の巻き戻し、ヤコビ行列の数値検算、α と float の相性を実装で確かめる

ここまでで、線形でない対象に対して、曲率 × 分散を見積もって方式を選び、設計して、数字で判定できるようになりました。次の一手は、手元の対象の h を書いてこの見積もりを計算し、この記事の Python の UKF か C の EKF に差し替えてみることです。

ただし、ここまでの方法は、どれも分布を平均と共分散の 2 つの数で表していました。鏡像のように分布の山が 2 つに割れる対象や、ρ が大きくて弓なりの分布になる対象では、この近似が限界です。次回の #7 では、ガウス近似も捨てて、分布を多数のサンプルそのもので持つパーティクルフィルタへ進みます。

よくある質問（FAQ）

Q. EKF と UKF はどちらを使えばいいですか？

A. 先に「曲率 × 分散」を計算します。 |h''|\sigma^2/2 が測定のノイズより十分に小さければ、結果はほぼ同じなので、計算の軽い EKF で足ります。この記事の IMU の例では 3500 倍の開きがあり、差は 0.0004° でした。不確かさが大きい、初期値が悪い、ヤコビ行列が書きにくい、といった場合は UKF にします。

Q. ヤコビ行列が手で書けないときはどうしますか？

A. UKF を使うか、数値微分でヤコビ行列を作ります。UKF は f と h を呼べれば動くので、表引きや分岐を含むモデルにも使えます。手で書いた場合は、数値微分と比べる検算を必ず行います。

Q. UKF の α、β、κ はどう決めますか？

A. N が小さければ κ = 3 − N、α = 1、β = 0 が 1 次元の分析と合う組です。N が 4 以上で κ が負になるなら、α = 1、β = 2、κ = 0 として中心の重みの負を避けます。α = 1、β = 2、κ = 3 − N を同時に使うと、分散を大きく見積もります。α を 10⁻³ のように小さくする場合は、float では動かないことがあります。

Q. 線形カルマンで済むのはどんなときですか？

A. f と h が線形か、動作の範囲が狭く、線形化を固定しても誤差が小さいときです。距離と方位の測定なら、ρ が 1 より十分に小さければ、測定を直交座標へ変換して線形カルマンに入れる方法でも同じ結果が出ます。大きな傾きのように動作点が大きく動く対象は、固定の線形化では足りません。

Q. EKF が発散したときは、まず何を見ますか？

A. 順に、①ヤコビ行列の数値検算 ②角度の巻き戻し ③初期の P が小さすぎないか ④NEES が理想より大きいか、を見ます。線形化の誤差が原因なら、反復 EKF か UKF を試します。それでも直らなければ、可観測かどうかを疑います。

関連記事

この連載：

• #1 基礎編 — 伝達関数、ボード線図、白色ノイズの測り方、C の骨格
• #2 ローパスフィルタの設計 — 同じカットオフでの比較とナイキスト線図
• #3 ハイパスフィルタの設計 — DC とドリフトを消す
• #4 バンドパスとノッチの設計 — 狙った周波数を扱う
• #5 カルマンフィルタの設計 — 5 本の式、定常ゲイン、相補と αβ
• #7 パーティクルフィルタの設計 — 重要度サンプリングとリサンプリング、粒子が足りなくなる条件、使い分けの勘所

センサの入口：

• 加速度センサ KXR94-2050 で作るデジタル水平器 — 重力の向きから角度を計算する

参考

取得日はいずれも 2026-11-07 です。図と数値はすべて筆者が Python（NumPy 2.4.6、SciPy 1.17.1、Matplotlib 3.11.2）で計算したものです。乱数のシードは固定です。

UKF

• Simon J. Julier, Jeffrey K. Uhlmann「A New Extension of the Kalman Filter to Nonlinear Systems」（Proc. SPIE 3068, 182–193, 1997）
• Simon J. Julier, Jeffrey K. Uhlmann「Unscented Filtering and Nonlinear Estimation」（Proceedings of the IEEE, 92(3), 401–422, 2004）
• Eric A. Wan, Rudolph van der Merwe「The Unscented Kalman Filter for Nonlinear Estimation」（IEEE AS-SPCC 2000, 153–158）
• Jeffrey Uhlmann「First-Hand: The Unscented Transform」（Engineering and Technology History Wiki）

EKF

• Leonard A. McGee, Stanley F. Schmidt「Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry」（NASA TM-86847、1985 年）
• Yaakov Bar-Shalom, X. Rong Li, Thiagalingam Kirubarajan「Estimation with Applications to Tracking and Navigation」（Wiley）
• Dan Simon「Optimal State Estimation」（Wiley）

距離と方位の追尾

• Don Lerro, Yaakov Bar-Shalom「Tracking with Debiased Consistent Converted Measurements Versus EKF」（IEEE Trans. Aerospace and Electronic Systems, 29(3), 1015–1022, 1993）

活用事例

• E. J. Lefferts, F. L. Markley, M. D. Shuster「Kalman Filtering for Spacecraft Attitude Estimation」（Journal of Guidance, Control, and Dynamics, 5(5), 417–429, 1982）
• John L. Crassidis, F. Landis Markley「Unscented Filtering for Spacecraft Attitude Estimation」（Journal of Guidance, Control, and Dynamics, 26(4), 536–542, 2003）
• PX4 Guide「Using PX4’s Navigation Filter (EKF2)」
• Gregory L. Plett「Extended Kalman filtering for battery management systems of LiPB-based HEV battery packs」（Journal of Power Sources, 134, 2004）
• Hugh Durrant-Whyte, Tim Bailey「Simultaneous Localization and Mapping: Part I」（IEEE Robotics & Automation Magazine, 13(2), 99–110, 2006）
• cra-ros-pkg/robot_localization

ライブラリ

• FilterPy「UnscentedKalmanFilter」
• scipy.linalg.cholesky（SciPy v1.18.0 Manual）