カルマンフィルタの理論とPython実装

import numpy as np

rng = np.random.default_rng(3)

P_minus = np.array([[2.0, 0.5], [0.5, 1.0]])
H = np.array([[1.0, 0.0]])
R = np.array([[0.5]])
S = H @ P_minus @ H.T + R
K_star = P_minus @ H.T @ np.linalg.inv(S)  # 式(15)の解析解

def trace_P(K):
    """式(12): Joseph形式によるP_{k|k}のトレース"""
    I = np.eye(2)
    P = (I - K @ H) @ P_minus @ (I - K @ H).T + K @ R @ K.T
    return np.trace(P)

print(f"K* (解析解) = {K_star.ravel()}")
print(f"tr(P) at K* = {trace_P(K_star):.6f}")

worse_count = 0
for _ in range(20000):
    dK = rng.normal(scale=0.05, size=K_star.shape)
    if trace_P(K_star + dK) < trace_P(K_star) - 1e-9:
        worse_count += 1
print(f"K* より tr(P) を下げる摂動の数（20000試行中）: {worse_count}")

K* (解析解) = [0.8 0.2]
tr(P) at K* = 1.300000
K* より tr(P) を下げる摂動の数（20000試行中）: 0

import numpy as np
import matplotlib.pyplot as plt

# ---- パラメータの設定 ----
dt = 1.0  # タイムステップ [s]

# 状態遷移行列（等速運動モデル）
A = np.array([[1, dt],
              [0,  1]])

# 観測行列（位置のみ観測可能）
H = np.array([[1, 0]])

# プロセスノイズ共分散（加速度ノイズから導出）
q = 0.1  # 加速度ノイズの分散
Q = q * np.array([[dt**3 / 3, dt**2 / 2],
                  [dt**2 / 2, dt]])

# 観測ノイズ共分散
R = np.array([[1.0]])

class KalmanFilter:
    """線形カルマンフィルタ"""

    def __init__(self, A, H, Q, R, x0, P0):
        self.A = A
        self.H = H
        self.Q = Q
        self.R = R
        self.x = x0.copy()
        self.P = P0.copy()

    def predict(self, u=None, B=None):
        """予測ステップ（式3, 4）"""
        self.x = self.A @ self.x
        if u is not None and B is not None:
            self.x += B @ u
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x.copy(), self.P.copy()

    def update(self, z):
        """更新ステップ（式5-9）"""
        # イノベーション
        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 = self.x + K @ y
        I = np.eye(len(self.x))
        self.P = (I - K @ self.H) @ self.P
        return self.x.copy(), self.P.copy()

np.random.seed(42)
T = 50  # タイムステップ数

# 真の初期状態
x_true = np.array([0.0, 1.0])  # 位置0, 速度1

# カルマンフィルタの初期化
x0 = np.array([0.0, 0.0])     # 初期推定（速度を知らないと仮定）
P0 = np.diag([1.0, 1.0])      # 初期共分散
kf = KalmanFilter(A, H, Q, R, x0, P0)

# 記録用配列
true_positions = [x_true[0]]
true_velocities = [x_true[1]]
measurements = []
est_positions = [x0[0]]
est_velocities = [x0[1]]
P_history = [P0.copy()]

for k in range(T):
    # 真の状態の更新
    x_true = A @ x_true + np.random.multivariate_normal([0, 0], Q)
    true_positions.append(x_true[0])
    true_velocities.append(x_true[1])

    # 観測の生成
    z = H @ x_true + np.random.multivariate_normal([0], R)
    measurements.append(z[0])

    # カルマンフィルタ
    kf.predict()
    x_est, P_est = kf.update(z)
    est_positions.append(x_est[0])
    est_velocities.append(x_est[1])
    P_history.append(P_est.copy())

true_positions = np.array(true_positions)
true_velocities = np.array(true_velocities)
measurements = np.array(measurements)
est_positions = np.array(est_positions)
est_velocities = np.array(est_velocities)
P_history = np.array(P_history)

# ---- 結果のプロット ----
time = np.arange(T + 1)
sigma_pos = np.sqrt(P_history[:, 0, 0])

fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)

# 位置の推定
axes[0].plot(time, true_positions, "b-", linewidth=1.5, label="True position")
axes[0].scatter(time[1:], measurements, c="gray", s=15, alpha=0.5,
                label="Measurements", zorder=3)
axes[0].plot(time, est_positions, "r--", linewidth=1.2, label="KF estimate")
axes[0].fill_between(time,
                      est_positions - 2 * sigma_pos,
                      est_positions + 2 * sigma_pos,
                      color="red", alpha=0.15, label="$\pm 2\sigma$")
axes[0].set_ylabel("Position")
axes[0].set_title("Kalman Filter - 1D Constant Velocity Tracking")
axes[0].legend(loc="upper left", fontsize=9)
axes[0].grid(True, alpha=0.3)

# 速度の推定
sigma_vel = np.sqrt(P_history[:, 1, 1])
axes[1].plot(time, true_velocities, "b-", linewidth=1.5, label="True velocity")
axes[1].plot(time, est_velocities, "r--", linewidth=1.2, label="KF estimate")
axes[1].fill_between(time,
                      est_velocities - 2 * sigma_vel,
                      est_velocities + 2 * sigma_vel,
                      color="red", alpha=0.15, label="$\pm 2\sigma$")
axes[1].set_xlabel("Time step $k$")
axes[1].set_ylabel("Velocity")
axes[1].legend(loc="upper left", fontsize=9)
axes[1].grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig("kalman_filter_result.png", dpi=150)
plt.show()

# RMSE計算（フィルタなしの生観測とも比較）
rmse_pos = np.sqrt(np.mean((true_positions[1:] - est_positions[1:]) ** 2))
rmse_meas = np.sqrt(np.mean((true_positions[1:] - measurements) ** 2))
rmse_vel = np.sqrt(np.mean((true_velocities[1:] - est_velocities[1:]) ** 2))
print(f"Position RMSE (KF): {rmse_pos:.4f}")
print(f"Position RMSE (raw measurement): {rmse_meas:.4f}")
print(f"Velocity RMSE (KF): {rmse_vel:.4f}")
print(f"KF position error reduction vs raw measurement: {(1 - rmse_pos/rmse_meas)*100:.1f}%")

import numpy as np

np.random.seed(0)

P_minus = np.array([[1.0, 0.3], [0.3, 0.8]])

# 2つのセンサーがどちらも状態1番目（位置）を観測する「重複センサー」
H_dup = np.array([[1.0, 0.0],
                  [1.0, 0.0]])

for r_val, label in [(0.0, "R=0（重複ノイズフリーセンサー）"),
                     (1e-10, "R=1e-10（ほぼ重複、微小ノイズ）"),
                     (1e-2, "R=1e-2（典型的、良条件）")]:
    R_dup = r_val * np.eye(2)
    S = H_dup @ P_minus @ H_dup.T + R_dup
    cond = np.linalg.cond(S)
    print(f"--- {label} ---")
    print(f"cond(S) = {cond:.3e}")
    try:
        K = P_minus @ H_dup.T @ np.linalg.inv(S)
        print("np.linalg.inv(S) 成功")
    except np.linalg.LinAlgError as e:
        print(f"np.linalg.inv(S) 失敗: {e}")
    K_pinv = P_minus @ H_dup.T @ np.linalg.pinv(S)
    print(f"np.linalg.pinv(S) によるK =\n{K_pinv}\n")

--- R=0（重複ノイズフリーセンサー） ---
cond(S) = 1.394e+17
np.linalg.inv(S) 失敗: Singular matrix
np.linalg.pinv(S) によるK =
[[0.5  0.5 ]
 [0.15 0.15]]

--- R=1e-10（ほぼ重複、微小ノイズ） ---
cond(S) = 2.000e+10
np.linalg.pinv(S) によるK =
[[0.49999905 0.50000095]
 [0.14999967 0.15000018]]

--- R=1e-2（典型的、良条件） ---
cond(S) = 2.010e+02
np.linalg.pinv(S) によるK =
[[0.49751244 0.49751244]
 [0.14925373 0.14925373]]

import numpy as np

np.random.seed(7)

dt = 1.0
A = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])

q_true = 0.1
Q_true = q_true * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]])
R = np.array([[1.0]])

q_filter = 1e-6  # 真値より4桁小さい、誤ったQ
Q_filter = q_filter * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]])

class KalmanFilter:
    def __init__(self, A, H, Q, R, x0, P0):
        self.A, self.H, self.Q, self.R = A, H, Q, R
        self.x, self.P = x0.copy(), P0.copy()

    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x.copy(), self.P.copy()

    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 = self.x + K @ y
        I = np.eye(len(self.x))
        self.P = (I - K @ self.H) @ self.P
        return self.x.copy(), self.P.copy(), K.copy()

T = 300
x_true = np.array([0.0, 1.0])
kf = KalmanFilter(A, H, Q_filter, R, np.array([0.0, 1.0]), np.diag([1.0, 1.0]))

true_pos, est_pos, P_pos, gains, nees = [x_true[0]], [0.0], [1.0], [], []
for k in range(T):
    x_true = A @ x_true + np.random.multivariate_normal([0, 0], Q_true)
    true_pos.append(x_true[0])
    z = H @ x_true + np.random.multivariate_normal([0], R)
    kf.predict()
    x_est, P_est, K = kf.update(z)
    est_pos.append(x_est[0])
    P_pos.append(P_est[0, 0])
    gains.append(K[0, 0])
    err = (x_true - x_est).reshape(-1, 1)
    nees.append(float((err.T @ np.linalg.inv(P_est) @ err).item()))

true_pos, est_pos, P_pos = np.array(true_pos), np.array(est_pos), np.array(P_pos)
nees = np.array(nees)

print(f"P_pos: k=10 -> {P_pos[10]:.4f}, k=100 -> {P_pos[100]:.4f}, k=299 -> {P_pos[299]:.4f}")
print(f"Kalman gain K_pos: k=10 -> {gains[10]:.4f}, k=100 -> {gains[100]:.4f}, k=299 -> {gains[299]:.4f}")
print(f"平均NEES（直近100ステップ）: {nees[-100:].mean():.1f}（整合していれば1近傍、カイ二乗検定99%棄却域は6.63）")
print(f"RMSE（直近100ステップ）: {np.sqrt(np.mean((true_pos[-100:]-est_pos[-100:])**2)):.4f}")
print(f"フィルタが報告するsigma（直近100ステップ平均）: {np.sqrt(P_pos[-100:]).mean():.4f}")

P_pos: k=10 -> 0.3161, k=100 -> 0.0469, k=299 -> 0.0437
Kalman gain K_pos: k=10 -> 0.2934, k=100 -> 0.0467, k=299 -> 0.0437
平均NEES（直近100ステップ）: 48924.8（整合していれば1近傍、カイ二乗検定99%棄却域は6.63）
RMSE（直近100ステップ）: 11.9092
フィルタが報告するsigma（直近100ステップ平均）: 0.2091

import numpy as np

np.random.seed(11)

dt = 1.0
A = np.array([[1, dt, 0], [0, 1, 0], [0, 0, 1]])  # 状態 [p, v, b]。bは一定
H = np.array([[1, 0, 1]])  # p + b のみ観測、pもbも単体では観測されない

O = np.vstack([H @ np.linalg.matrix_power(A, i) for i in range(3)])
rank = np.linalg.matrix_rank(O)
print("可観測性行列 O:")
print(O)
print(f"rank(O) = {rank}  (状態次元 n=3)")
print("可観測" if rank == 3 else "不可観測")

可観測性行列 O:
[[1. 0. 1.]
 [1. 1. 1.]
 [1. 2. 1.]]
rank(O) = 2  (状態次元 n=3)
不可観測

class KalmanFilter:
    def __init__(self, A, H, Q, R, x0, P0):
        self.A, self.H, self.Q, self.R = A, H, Q, R
        self.x, self.P = x0.copy(), P0.copy()

    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x.copy(), self.P.copy()

    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 = self.x + K @ y
        I = np.eye(len(self.x))
        self.P = (I - K @ self.H) @ self.P
        return self.x.copy(), self.P.copy()

Q = np.diag([0.01, 0.01, 1e-8])
R = np.array([[0.25]])
x_true = np.array([0.0, 1.0, 2.0])  # 真のバイアス b=2.0
kf = KalmanFilter(A, H, Q, R, np.array([0.0, 1.0, 0.0]), np.diag([1.0, 1.0, 4.0]))

P_b_hist, err_b_hist = [], []
for k in range(400):
    x_true = A @ x_true  # 真値はノイズなしで決定論的に遷移(可観測性のみに着目するため)
    z = H @ x_true + np.random.multivariate_normal([0], R)
    kf.predict()
    x_est, P_est = kf.update(z)
    P_b_hist.append(P_est[2, 2])
    err_b_hist.append(x_est[2] - x_true[2])

print(f"P_bias: k=10 -> {P_b_hist[9]:.4f}, k=50 -> {P_b_hist[49]:.4f}, k=399 -> {P_b_hist[398]:.4f}")
print(f"バイアス推定誤差: k=399 -> {err_b_hist[398]:.4f}")

P_bias: k=10 -> 0.9427, k=50 -> 0.9423, k=399 -> 0.9423
バイアス推定誤差: k=399 -> -0.1808