![**カルマンフィルタを理解する：シンプルなレーダー例題**
カルマンフィルタは、ノイズの多い測定値からシステム（位置・速度など）の状態を推定するアルゴリズムです。
この例では、レーダーセンサーを使って移動物体を追跡する方法を見てみます。
1. **システムモデル**
- 状態ベクトル：\(\mathbf{x}_k = [x_k,\; y_k,\; \dot{x}_k,\; \dot{y}_k]^T\)
- 変換行列（定常速度モデル）：
\[
\mathbf{F} =
\begin{bmatrix}
1 & 0 & \Delta t & 0\\
0 & 1 & 0 & \Delta t\\
0 & 0 & 1 & 0\\
0 & 0 & 0 & 1
\end{bmatrix}
\]
- プロセスノイズ共分散：\(\mathbf{Q}\)（加速度不確かさに依存）
2. **測定モデル**
- レーダーは距離 \(r\) と方位角 \(\theta\) を返します。
- 測定ベクトル：\(\mathbf{z}_k = [r_k,\; \theta_k]^T\)
- 観測行列：
\[
\mathbf{H} =
\begin{bmatrix}
\dfrac{x}{\sqrt{x^2+y^2}} & \dfrac{y}{\sqrt{x^2+y^2}} & 0 & 0\\[4pt]
-\dfrac{y}{x^2+y^2} & \dfrac{x}{x^2+y^2} & 0 & 0
\end{bmatrix}
\]
- 測定ノイズ共分散：\(\mathbf{R}\)
3. **カルマンフィルタの手順**
- *予測*
\[
\hat{\mathbf{x}}_{k|k-1}= \mathbf{F}\,\hat{\mathbf{x}}_{k-1|k-1}
\]
\[
\mathbf{P}_{k|k-1}= \mathbf{F}\,\mathbf{P}_{k-1|k-1}\,\mathbf{F}^T + \mathbf{Q}
\]
- *更新*
イノベーション（残差）を計算：
\[
\mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})
\]
イノベーション共分散：
\[
\mathbf{S}_k = \mathbf{H}\,\mathbf{P}_{k|k-1}\,\mathbf{H}^T + \mathbf{R}
\]
カルマンゲイン：
\[
\mathbf{K}_k = \mathbf{P}_{k|k-1}\,\mathbf{H}^T\,\mathbf{S}_k^{-1}
\]
更新された状態と共分散：
\[
\hat{\mathbf{x}}_{k|k}= \hat{\mathbf{x}}_{k|k-1}+ \mathbf{K}_k\,\mathbf{y}_k
\]
\[
\mathbf{P}_{k|k}= (\mathbf{I}-\mathbf{K}_k\,\mathbf{H})\,\mathbf{P}_{k|k-1}
\]
4. **実装のヒント**
- レーダー測定関数が非線形の場合は、線形化したモデル（拡張カルマンフィルタ）を使用します。
- 追跡が失われた際には、頻繁にフィルタを再初期化してください。
- \(\mathbf{Q}\) と \(\mathbf{R}\) を実際のセンサーノイズ特性に合わせて調整します。
5. **結果の解釈**
- 追加測定が増えるにつれて、推定位置は真軌道に収束します。
- フィルタが十分なデータを蓄積し不確実性が減少すると、速度推定も改善されます。
これらの手順に従えば、シンプルなレーダー測定値でもカルマンフィルタで処理でき、物体運動の正確かつリアルタイムな追跡を実現できます。](/_next/image?url=%2Fscreenshots%2F2026-04-09%2F1775692539358.webp&w=3840&q=75)
2026/04/09 2:11
**カルマンフィルタを理解する：シンプルなレーダー例題** カルマンフィルタは、ノイズの多い測定値からシステム（位置・速度など）の状態を推定するアルゴリズムです。 この例では、レーダーセンサーを使って移動物体を追跡する方法を見てみます。 1. **システムモデル** - 状態ベクトル：\(\mathbf{x}_k = [x_k,\; y_k,\; \dot{x}_k,\; \dot{y}_k]^T\) - 変換行列（定常速度モデル）： \[ \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \] - プロセスノイズ共分散：\(\mathbf{Q}\)（加速度不確かさに依存） 2. **測定モデル** - レーダーは距離 \(r\) と方位角 \(\theta\) を返します。 - 測定ベクトル：\(\mathbf{z}_k = [r_k,\; \theta_k]^T\) - 観測行列： \[ \mathbf{H} = \begin{bmatrix} \dfrac{x}{\sqrt{x^2+y^2}} & \dfrac{y}{\sqrt{x^2+y^2}} & 0 & 0\\[4pt] -\dfrac{y}{x^2+y^2} & \dfrac{x}{x^2+y^2} & 0 & 0 \end{bmatrix} \] - 測定ノイズ共分散：\(\mathbf{R}\) 3. **カルマンフィルタの手順** - *予測* \[ \hat{\mathbf{x}}_{k|k-1}= \mathbf{F}\,\hat{\mathbf{x}}_{k-1|k-1} \] \[ \mathbf{P}_{k|k-1}= \mathbf{F}\,\mathbf{P}_{k-1|k-1}\,\mathbf{F}^T + \mathbf{Q} \] - *更新* イノベーション（残差）を計算： \[ \mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1}) \] イノベーション共分散： \[ \mathbf{S}_k = \mathbf{H}\,\mathbf{P}_{k|k-1}\,\mathbf{H}^T + \mathbf{R} \] カルマンゲイン： \[ \mathbf{K}_k = \mathbf{P}_{k|k-1}\,\mathbf{H}^T\,\mathbf{S}_k^{-1} \] 更新された状態と共分散： \[ \hat{\mathbf{x}}_{k|k}= \hat{\mathbf{x}}_{k|k-1}+ \mathbf{K}_k\,\mathbf{y}_k \] \[ \mathbf{P}_{k|k}= (\mathbf{I}-\mathbf{K}_k\,\mathbf{H})\,\mathbf{P}_{k|k-1} \] 4. **実装のヒント** - レーダー測定関数が非線形の場合は、線形化したモデル（拡張カルマンフィルタ）を使用します。 - 追跡が失われた際には、頻繁にフィルタを再初期化してください。 - \(\mathbf{Q}\) と \(\mathbf{R}\) を実際のセンサーノイズ特性に合わせて調整します。 5. **結果の解釈** - 追加測定が増えるにつれて、推定位置は真軌道に収束します。 - フィルタが十分なデータを蓄積し不確実性が減少すると、速度推定も改善されます。 これらの手順に従えば、シンプルなレーダー測定値でもカルマンフィルタで処理でき、物体運動の正確かつリアルタイムな追跡を実現できます。
RSS: https://news.ycombinator.com/rss
要約▶
Japanese Translation:
カルマンフィルタは、測定ノイズとプロセスノイズが存在する状況でシステムの状態を予測し修正する最適な再帰推定器です。単純な一次元レーダートラッキング例では、状態ベクトル (x=[r;;v]) が距離と速度を表します。初期時刻 (t_0) での測定値（(r=10,000 \text{ m}, v=200 \text{ m/s})）から、フィルタは一定速度モデルを用いて次状態を予測します（(\Delta t = 5,\text{s}\））:
(x_{1|0}=F x_{0|0}=[11,000;;200])。
予測された共分散行列は、遷移行列 (F) と必要に応じてプロセスノイズ (Q=\sigma_a^2\begin{bmatrix}\frac{\Delta t^4}{4}&\frac{\Delta t^3}{2}\ \frac{\Delta t^3}{2}&\Delta t^2\end{bmatrix} = \begin{bmatrix}6.25&2.5\ 2.5&1\end{bmatrix}) によって更新され、(P_{1|0}=28.5,;3.75;;3.75,;1.25) となります。
時刻 (t_1) での第二測定値（(r=11,020 \text{ m}, v=202 \text{ m/s}\））、共分散行列 (R_1=\begin{bmatrix}36&0\0&2.25\end{bmatrix}\）は、カルマンゲイン
(K_1=P_{1|0}(P_{1|0}+R_1)^{-1}= \begin{bmatrix}0.4048&0.6377\0.0399&0.3144\end{bmatrix})
を通じて組み込まれます。
更新された状態は (x_{1|1}=[11,009.37;;201.43])、共分散行列は
(P_{1|1}=(I-K_1)P_{1|0}= \begin{bmatrix}14.57&1.43\1.43&0.71\end{bmatrix}) となり、事前予測と測定の不確実性に比べて減少します。
フィルタは次に (t_2) に再び予測します：(x_{2|1}=F x_{1|1}=[12,016.5;;201.43])、共分散行列
(P_{2|1}=F P_{1|1} F^T + Q = \begin{bmatrix}52.86&7.47\7.47&1.71\end{bmatrix})。
この簡潔な説明は、すべての重要な定量的詳細を保持し、予測‑更新サイクルを示し、カルマンフィルタが不確実性をどのように減少させるかを強調しています。基本的な線形代数と推定概念に精通した読者には明瞭であり、不必要な専門用語は避けています。
本文
カルマンフィルタ – イントロダクションと例
1. なぜカルマンフィルタが必要なのか？
- 目的： 測定値にノイズが含まれ、システムの動的モデルが不確実である場合でも、システムの 状態（位置・速度など）を推定すること。
- 応用例： 物体追跡、航行、ロボティクス、金融分析、天気予報など。
2. コア概念
| 記号 | 意味 |
|---|---|
| (\mathbf{x}_k) | 時刻 (k) における状態ベクトル |
| (\mathbf{z}_k) | 時刻 (k) における測定ベクトル |
| (\mathbf{F}) | 状態遷移行列（運動モデル） |
| (\mathbf{H}) | 観測行列（状態を測定に写像） |
| (\mathbf{Q}) | 進化ノイズ共分散 |
| (\mathbf{R}_k) | 測定ノイズ共分散 |
| (\hat{\mathbf{x}}_{k | k-1}) |
| (\hat{\mathbf{x}}_{k | k}) |
| (\mathbf{P}_{k | k-1}) |
| (\mathbf{P}_{k | k}) |
| (\mathbf{K}_k) | カルマンゲイン |
3. 予測ステップ
[ \begin{aligned} \hat{\mathbf{x}}{k|k-1} &= \mathbf{F},\hat{\mathbf{x}}{k-1|k-1} + \mathbf{G},u_{k-1} \ \mathbf{P}{k|k-1} &= \mathbf{F},\mathbf{P}{k-1|k-1},\mathbf{F}^{!\top} + \mathbf{Q} \end{aligned} ]
制御入力が無い場合は (u_{k-1}=0)。
4. 更新ステップ
イノベーション（測定残差）
[ \tilde{\mathbf{y}}_k = \mathbf{z}k - \mathbf{H},\hat{\mathbf{x}}{k|k-1} ]
カルマンゲイン
[ \mathbf{K}k = \mathbf{P}{k|k-1},\mathbf{H}^{!\top}, \bigl(\mathbf{H},\mathbf{P}_{k|k-1},\mathbf{H}^{!\top} + \mathbf{R}_k\bigr)^{-1} ]
状態更新
[ \hat{\mathbf{x}}{k|k} = \hat{\mathbf{x}}{k|k-1} + \mathbf{K}_k,\tilde{\mathbf{y}}_k ]
共分散更新（ジョセフ形式）
[ \mathbf{P}_{k|k} = (\mathbf{I}-\mathbf{K}k,\mathbf{H}),\mathbf{P}{k|k-1}, (\mathbf{I}-\mathbf{K}_k,\mathbf{H})^{!\top}
- \mathbf{K}_k,\mathbf{R}_k,\mathbf{K}_k^{!\top} ]
簡略化した形：
(\displaystyle \mathbf{P}_{k|k} = (\mathbf{I}-\mathbf{K}k,\mathbf{H}),\mathbf{P}{k|k-1})
5. 一次元の特別ケース
状態と測定がスカラーの場合：
[ K_k = \frac{p_{k|k-1}}{p_{k|k-1}+r_k} ]
状態更新： (x_{k|k}=x_{k|k-1}+K_k(z_k-x_{k|k-1}))
共分散更新： (p_{k|k}=(1-K_k),p_{k|k-1})
6. 例 – 1‑D レーダー追跡
システム
- 状態: (\mathbf{x} = [,r,;v,]^{!\top})（距離・速度）
- 運動モデル（等速運動）
[ \mathbf{F}= \begin{bmatrix} 1 & \Delta t\ 0 & 1 \end{bmatrix},\qquad \Delta t =5,\text{s} ] - 観測行列: (\mathbf{H}= \mathbf{I})
初期化（時刻 (t_0)）
| 項目 | 値 |
|---|---|
| 測定 (\mathbf{z}_0=[10,000,;200]^{!\top}) | |
| 初期状態推定 (\hat{\mathbf{x}}_{0 | 0}=\mathbf{z}_0) |
| 測定共分散 | |
| (\mathbf{R}_0= |
\begin{bmatrix} 16 & 0\\ 0 & 0.25 \end{bmatrix}\) | |
| 初期誤差共分散 (\mathbf{P}_{0|0}=\mathbf{R}_0) | |
(t_1) への予測
[ \hat{\mathbf{x}}{1|0}= \mathbf{F},\hat{\mathbf{x}}{0|0} =\begin{bmatrix}11,000\200\end{bmatrix} ]
プロセスノイズ共分散（加速度標準偏差 (a=0.2,\text{m/s}^2)）:
[ \mathbf{Q}= \sigma_a^{2}! \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2}\[1ex] \frac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix} =\begin{bmatrix} 6.25&2.5\ 2.5&1 \end{bmatrix} ]
共分散予測:
[ \mathbf{P}{1|0}= \mathbf{F},\mathbf{P}{0|0},\mathbf{F}^{!\top} + \mathbf{Q} =\begin{bmatrix} 28.5&3.75\ 3.75&1.25 \end{bmatrix} ]
(t_1) の測定で更新
(\mathbf{z}_1=[11,020,;202]^{!\top})
測定共分散
[ \mathbf{R}_1= \begin{bmatrix} 36&0\ 0&2.25 \end{bmatrix} ]
カルマンゲイン:
[ \mathbf{K}1= \mathbf{P}{1|0}, (\mathbf{H},\mathbf{P}_{1|0},\mathbf{H}^{!\top}+\mathbf{R}_1)^{-1} =\begin{bmatrix} 0.4048&0.6377\ 0.0399&0.3144 \end{bmatrix} ]
イノベーション (\tilde{\mathbf{y}}_1 = \mathbf{z}1-\hat{\mathbf{x}}{1|0}= [20,;2]^{!\top})
更新後の状態:
[ \hat{\mathbf{x}}{1|1}= \hat{\mathbf{x}}{1|0}+ \mathbf{K}_1,\tilde{\mathbf{y}}_1 =\begin{bmatrix} 11,009.37\201.43 \end{bmatrix} ]
更新後の共分散（ジョセフ形式）:
[ \mathbf{P}_{1|1}= (\mathbf{I}-\mathbf{K}1,\mathbf{H}),\mathbf{P}{1|0} =\begin{bmatrix} 14.57&1.43\ 1.43&0.71 \end{bmatrix} ]
更新後の共分散は、予測共分散と測定共分散の両方よりも小さくなり、情報統合によって不確実性が低減したことを示しています。
次回予測（(t_2) へ）
[ \hat{\mathbf{x}}{2|1}= \mathbf{F},\hat{\mathbf{x}}{1|1} =\begin{bmatrix}12,016.5\201.43\end{bmatrix} ]
[ \mathbf{P}{2|1}= \mathbf{F},\mathbf{P}{1|1},\mathbf{F}^{!\top} + \mathbf{Q} = \begin{bmatrix} 52.86&7.47\ 7.47&1.71 \end{bmatrix} ]
7. カルマンループの概要
for each time step k: # 予測 x_pred = F * x_prev P_pred = F * P_prev * F^T + Q # 測定更新（測定が利用可能な場合） K = P_pred * H^T * inv(H*P_pred*H^T + R_k) x_up = x_pred + K * (z_k - H*x_pred) P_up = (I - K*H) * P_pred # Joseph form であればさらに安全 # 次ステップへ x_prev = x_up P_prev = P_up
8. ポイントまとめ
- 予測 は運動モデルを用いて状態を進め、プロセスノイズを加える。
- 更新 は新しい測定と予測を不確実性で重み付けしながら統合する。
カルマンゲインがその重みを決定する。 - 共分散行列は通常、更新後に縮小し、推定への信頼度が増すことを示す（測定が極端にノイズが大きい場合を除く）。
- 大きな不確実性を持つ測定でも推定精度は向上する。外れ値だけを排除すればよい。
より詳細で高度な例（非線形 EKF/UKF、センサーフュージョン、外れ値除去など）については、『Kalman Filter from the Ground Up』や無料オンラインチュートリアルをご参照ください。