カルマンフィルター

🔥 Vibe プロンプト

「ノイズの多いGPS（σ=30m）とIMU（σ=20m）から車両の位置と速度を追跡。生データとフィルター後を比較。」

カルマンフィルターとは？

カルマンフィルターは、ノイズの多いセンサー測定値からシステムの状態を推定する最適アルゴリズムです。予測と測定をカルマンゲインKで最適にブレンドします。

| ステップ | 式 | 目的 | |---------|-----|------| | 予測 | x̂ = F·x | 運動モデルで次の状態を予測 | | 予測 | P̂ = F·P·Fᵀ + Q | 誤差共分散を予測（不確かさ増大） | | 更新 | y = z − H·x̂ | 残差を計算（測定との差） | | 更新 | K = P̂·Hᵀ·S⁻¹ | カルマンゲイン計算（最適ブレンド比） | | 更新 | x = x̂ + K·y | 測定値で状態推定を更新 | | 更新 | P = (I − K·H)·P̂ | 誤差共分散を更新（不確かさ減少） |

実装

import numpy as np

class KalmanFilter:
    def __init__(self):
        self.x = np.array([[0],[0],[0],[0]])
        self.P = np.eye(4) * 100
        self.F = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]])
        self.H = np.array([[1,0,0,0],[0,1,0,0]])
        self.R = np.eye(2) * 30
        self.Q = np.eye(4) * 0.1
    
    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
    
    def update(self, z):
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x += K @ y
        self.P = (np.eye(4) - K @ self.H) @ self.P

# シミュレーション
kf = KalmanFilter()
true_pos = np.array([100, 100])
true_vel = np.array([5, 3])

error_raws = []
error_filts = []

for t in range(20):
    true_pos = true_pos + true_vel
    gps = true_pos + np.random.normal(0, 30, 2)
    
    kf.predict(); kf.update(gps)
    
    err_raw = np.linalg.norm(gps - true_pos)
    err_filt = np.linalg.norm(kf.x[:2].flatten() - true_pos)
    error_raws.append(err_raw)
    error_filts.append(err_filt)
    
    print(f"t={t}: 生誤差={err_raw:.1f}, フィルター誤差={err_filt:.1f}")

print(f"\n平均: 生誤差={np.mean(error_raws):.1f}, フィルター誤差={np.mean(error_filts):.1f}")
print(f"改善率: {(1-np.sum(error_filts)/np.sum(error_raws))*100:.1f}%")

拡張カルマンフィルター（EKF）

非線形システムの場合、EKFは現在の推定値の周りで線形化します：

運動モデルのヤコビアンを使用:
  F_actual = ∂f/∂x（x̂で評価）

例：旋回車両の追跡:
  x' = x + v·cos(θ)·dt ← 非線形
  ヤコビアン F = [[1,0,dt·cos(θ),-v·dt·sin(θ)],
                  [0,1,dt·sin(θ), v·dt·cos(θ)],
                  [0,0,1,0],
                  [0,0,0,1]]

応用

| 分野 | 用途 | |------|------| | 🛰️ GPS/IMU | ドローン、自動運転車、スマホ位置推定 | | 🚀 航空宇宙 | ロケット誘導、衛星軌道決定 | | 🤖 ロボティクス | SLAM（自己位置推定と地図構築） | | 📈 金融 | 隠れマルコフモデル、レジーム検出 | | 🫀 医療 | ECGノイズ除去、血糖値モニタリング |

まとめ

| 項目 | 詳細 | |------|------| | 状態 | 追跡対象（位置、速度など） | | 予測 | 運動モデル(F) + プロセスノイズ(Q) | | 更新 | 測定(z) + 測定ノイズ(R) | | カルマンゲイン(K) | 予測と測定の信頼度を最適調整 | | 収束 | Kは十分なステップ後に一定値に収束 | | 拡張 | EKF(非線形), UKF, パーティクルフィルター |

勾配降下法コース完了！🎉

✅ 勾配降下法の基礎
✅ Momentum & Adam
✅ FFT / DFT
✅ PID制御
✅ カルマンフィルター
✅ EKF拡張

カルマンフィルター

🔥 Vibe プロンプト