#0172（2025/06/16）カルマンフィルタ

# カルマンフィルタ


## 1. カルマンフィルタが解く課題

### 典型シナリオ

  1. **移動体の位置**を IMU + GPS で推定したい
  2. **温度/気圧**センサの瞬間的な突発値をならしたい
  3. **株価**の滑らかなトレンドを抽出したい

### なぜ単純平均ではダメか？

  * センサはそれぞれ **ノイズ特性が違う**
  * 時間経過で **状態そのものが変化**
  * 真値を観測できない瞬間がある

### KF が刺さる理由

  1. “**モデルによる予測**” と “**観測による修正**” を自動で重み付け
  2. 出力は "推定値 + 推定誤差" なので **信頼度が数値で得られる**
  3. 状態が線形変換 & ノイズがガウスであれば **最適 (最小分散推定)**

---

## 2. ざっくり分かるアルゴリズム構造

| ステップ        | 何をする?       | 直感的な説明                           |
| ----------- | ----------- | -------------------------------- |
| **Predict** | モデルで次の状態を予測 | 前回の推定値に “移動量” を足す。誤差も大きくなる。      |
| **Update**  | 新しい観測で修正    | センサ値が信用できるほど推定値に寄せ、信用できなければ無視する。 |

> カルマンゲイン： $K_k$ が "寄せ具合" を自動計算してくれる魔法の係数。

---

## 3. 数学的な理解

線形代数に苦手意識があっても以下だけ覚えれば動かせる。

```
[予測ステップ (k-1 → k)]

  x̂ₖ|ₖ₋₁ = A · x̂ₖ₋₁|ₖ₋₁ + B · uₖ₋₁
    → 状態遷移行列 A によって前の状態から現在を予測
    
  Pₖ|ₖ₋₁ = A · Pₖ₋₁|ₖ₋₁ · Aᵀ + Q
    → 予測の不確かさ P は前の不確かさに加えてプロセスノイズ Q を反映

[更新ステップ (観測 zₖ を取得後)]

  Kₖ = Pₖ|ₖ₋₁ · Hᵀ · (H · Pₖ|ₖ₋₁ · Hᵀ + R)⁻¹
    → 観測の信頼度 R と予測の不確かさ P から重み K（カルマンゲイン）を計算
    
  x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ · (zₖ - H · x̂ₖ|ₖ₋₁)
    → 予測と観測の差を Kₖ 倍して状態に反映
    
  Pₖ|ₖ = (I - Kₖ · H) · Pₖ|ₖ₋₁
    → 不確かさを更新（観測で不確かさが減る）
  ```


* $A$ : **状態遷移行列**―速度を積分して位置が変わる等
* $Q$ : **プロセスノイズ共分散**―モデルをどれだけ疑うか
* $H$ : **観測行列**―状態から観測がどう計算されるか
* $R$ : **観測ノイズ共分散**―センサの信用度

> 数式の見方: 位置だけの 1D なら全部がスカラーになり、実際は足し算割り算だけで処理できる。

---

## 4. パラメータの意味とチューニング指針

* ### Q (プロセスノイズ)

  * **大きく** → モデルを信用しない → センサ値に追随しやすい (ただし振動)
  * **小さく** → モデルを信用 → ノイズをスムーズにできるが外れ値に弱い
* ### R (観測ノイズ)

  * 実機からデータを取り **標準偏差²** を推定するのが王道
* ### 初期共分散 P₀

  * 未知なら **とりあえず大きく** (万が一の偏りを緩和)
* ### チェックポイント

  1. 残差 `z_k - Hx̂` の分散が R と合っているか?
  2. 推定誤差が収束せずブレ続けるなら Q ↗ or モデル改善

---

## 5. 自動運転での使われ方

| レイヤ       | 使われる KF バリエーション | 具体例                       |
| --------- | --------------- | ------------------------- |
| 車両ローカライザ  | EKF, UKF        | GPS + IMU + LiDAR 地図マッチング |
| 物体追跡      | 位置+速度の 4D KF    | レーダー点群で前走車を追跡             |
| センサフュージョン | Federated KF    | ECU 間で推定を分散融合             |

> 実際の自動運転スタックでは "EKF (拡張 KF)" が主流。速度変換や方位角が非線形だからです。

---

## 6. Python で動かす 2 次元 KF サンプル

以下は **位置 & 速度** を状態に含め、x–y 平面を等加速度で動く物体を推定するミニマムコード。

```python
import numpy as np

# ===== 0. シミュレーション ==========
dt = 0.1; N = 200
ax, ay = 0.2, 0.05
rng = np.random.default_rng(0)

# 真値生成
t      = np.arange(N)*dt
x_true = 0.5*ax*t**2
y_true = 0.5*ay*t**2
Z = np.stack([
    x_true + rng.normal(0, 0.3, N),
    y_true + rng.normal(0, 0.3, N)
], axis=1)

# ===== 1. 行列定義 ====================
A = np.block([[np.eye(2), dt*np.eye(2)],
              [np.zeros((2,2)), np.eye(2)]])
H = np.block([[np.eye(2), np.zeros((2,2))]])
Q = np.diag([1e-4,1e-4,1e-2,1e-2])
R = np.diag([0.3**2, 0.3**2])

# ===== 2. 逐次フィルタリング =========
x_hat = np.zeros(4); P = np.eye(4)
traj  = np.empty((N,4))
for k,z in enumerate(Z):
    # Predict
    x_hat = A @ x_hat
    P     = A @ P @ A.T + Q
    # Update
    S   = H @ P @ H.T + R
    K   = P @ H.T @ np.linalg.inv(S)
    x_hat += K @ (z - H @ x_hat)
    P   = (np.eye(4) - K @ H) @ P
    traj[k] = x_hat

print(traj[:5])  # 最初の5ステップ確認
```

* **行列サイズ** : A=4×4, H=2×4, Q=4×4, R=2×2
* **実運用**ではここに加速度を別センサから観測して H を拡張するなど柔軟に拡張可

---

## 7. さらに一歩先へ

| 拡張版          | 何がうれしい?         | ユースケース         |
| ------------ | --------------- | -------------- |
| **EKF**      | 非線形を 1 次近似で扱える  | 角度→座標変換が入るロボット |
| **UKF**      | シグマ点で高次の非線形も高精度 | ドローン 6DoF 姿勢推定 |
| **RTS スムーザ** | 過去も後向き補正してトゲを除去 | 事後解析・ログの品質改善   |
| **AdapKF**   | Q,R をオンライン推定    | ノイズ特性が時間で変わる環境 |

---

## 8. まとめ

* KF は「**モデル予測 + 観測修正**」を自動でブレンドする万能フィルタ
* 実装は行列を 4 つ用意し、**Predict–Update** の 2 行で完結
* チューニングは **Q / R** と **モデル設計** が 8 割
* 自動運転・IoT・金融… ノイズがある時系列ならまず KF を試そう

> 📌 **Tip:** 行列を変えてもループ本体は同じなので、応用先ごとに "設定ファイル化" しておくと再利用がラクです。