卡尔曼滤波简洁公式推导
1. 系统模型
-
状态转移方程
x k = A k − 1 x k − 1 + w k − 1 , w k − 1 ∼ N ( 0 , Q ) x_k = A_{k-1}\, x_{k-1} + w_{k-1},\quad w_{k-1} \sim \mathcal{N}(0, Q) xk=Ak−1xk−1+wk−1,wk−1∼N(0,Q) -
观测方程
z k = H k x k + v k , v k ∼ N ( 0 , R ) z_k = H_k\, x_k + v_k,\quad v_k \sim \mathcal{N}(0, R) zk=Hkxk+vk,vk∼N(0,R)
2. 预测步骤
-
状态预测
x ^ k ∣ k − 1 = A k − 1 x ^ k − 1 ∣ k − 1 \hat{x}_{k|k-1} = A_{k-1}\, \hat{x}_{k-1|k-1} x^k∣k−1=Ak−1x^k−1∣k−1 -
协方差预测
P k ∣ k − 1 = A k − 1 P k − 1 ∣ k − 1 A k − 1 T + Q P_{k|k-1} = A_{k-1}\, P_{k-1|k-1}\, A_{k-1}^T + Q Pk∣k−1=Ak−1Pk−1∣k−1Ak−1T+Q
3. 更新步骤
-
卡尔曼增益
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R ) − 1 K_k = P_{k|k-1}\, H_k^T \left( H_k\, P_{k|k-1}\, H_k^T + R \right)^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+R)−1 -
状态更新
x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k\, \bigl( z_k - H_k\, \hat{x}_{k|k-1} \bigr) x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1) -
协方差更新
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = \left( I - K_k\, H_k \right) P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1
4. 例子
import numpy as np
import matplotlib.pyplot as plt
# ---------------------------
# Parameter Settings
# ---------------------------
dt = 1.0 # Time step (seconds)
num_steps = 50 # Total number of steps
T_initial = 20.0 # Initial temperature (°C)
rate = 0.5 # Temperature increase rate (°C/second)
# Process noise and measurement noise covariance
Q = np.array([[0.05, 0],
[0, 0.01]]) # Small process noise
R = np.array([[2.0]]) # Relatively large measurement noise
# ---------------------------
# System Model Definition
# ---------------------------
# State transition matrix: constant rate model
A = np.array([[1, dt],
[0, 1]])
# Observation matrix: only measure temperature
H = np.array([[1, 0]])
# ---------------------------
# Initialize State and Covariance
# ---------------------------
# True state: initial temperature T_initial and temperature increase rate
x_true = np.array([T_initial, rate])
# Initial state estimate (can deviate from the true state)
x_est = np.array([T_initial - 2, rate - 0.1])
# Initial estimate covariance (large uncertainty)
P_est = np.eye(2) * 100
# Arrays to store data for plotting
true_temps = []
measured_temps = []
est_temps = []
# ---------------------------
# Kalman Filter Main Loop
# ---------------------------
for k in range(num_steps):
# True state update (temperature increases at constant rate with process noise)
w = np.random.multivariate_normal(mean=[0, 0], cov=Q)
x_true = A @ x_true + w
true_temps.append(x_true[0])
# Generate measurement: only measure temperature with added measurement noise
v = np.random.normal(0, np.sqrt(R[0, 0]))
z = H @ x_true + v
measured_temps.append(z[0])
# Kalman Filter Prediction Step
x_pred = A @ x_est
P_pred = A @ P_est @ A.T + Q
# Update Step
y = z - H @ x_pred # Measurement residual (innovation)
S = H @ P_pred @ H.T + R # Innovation covariance
K = P_pred @ H.T @ np.linalg.inv(S) # Kalman gain
x_est = x_pred + K @ y # Update state estimate
# Corrected covariance update
P_est = (np.eye(P_pred.shape[0]) - K @ H) @ P_pred
est_temps.append(x_est[0])
# ---------------------------
# Data Visualization
# ---------------------------
time = np.arange(num_steps) * dt
plt.figure(figsize=(10, 6))
plt.plot(time, true_temps, 'b-', label="True Temperature")
plt.plot(time, measured_temps, 'kx', label="Measured Temperature", markersize=6)
plt.plot(time, est_temps, 'r--', label="Estimated Temperature")
plt.xlabel("Time (s)")
plt.ylabel("Temperature (°C)")
plt.title("Kalman Filter Example: Temperature Increasing at a Constant Rate")
plt.legend()
plt.grid(True)
plt.show()
本示例模拟一个温度测量系统，其中温度随时间以恒定速率增加。我们采用标准卡尔曼滤波来估计温度，其状态向量定义为
x = [ T T ˙ ] , x = \begin{bmatrix} T \\ \dot{T} \end{bmatrix}, x=[TT˙],
其中 T T T 表示温度， T ˙ \dot{T} T˙ 表示温度上升速率。
系统模型
状态转移（预测）方程，温度随时间以恒定速率增加，可以使用匀速运动模型进行描述，其离散时间模型为
x k + 1 = A x k + w k , x_{k+1} = A\, x_k + w_k, xk+1=Axk+wk,
其中
- w k w_k wk 是过程噪声，满足 w k ∼ N ( 0 , Q ) w_k \sim \mathcal{N}(0, Q) wk∼N(0,Q)；
- 状态转移矩阵 A A A 为
A = [ 1 d t 0 1 ] . A = \begin{bmatrix} 1 & dt \\ 0 & 1 \end{bmatrix}. A=[10dt1].
解释：
- 第一行 T k + 1 = T k + d t × T ˙ k T_{k+1} = T_k + dt \times \dot{T}_k Tk+1=Tk+dt×T˙k表示下一个时刻的温度由当前温度和温度上升速率决定；
- 第二行 T ˙ k + 1 = T ˙ k \dot{T}_{k+1} = \dot{T}_k T˙k+1=T˙k 表示温度上升速率保持不变（即恒速）。
观测方程
本系统仅测量温度，因此观测模型为
z k = H x k + v k , z_k = H\, x_k + v_k, zk=Hxk+vk,
其中
- v k v_k vk 是测量噪声，满足 v k ∼ N ( 0 , R ) v_k \sim \mathcal{N}(0, R) vk∼N(0,R)；
- 观测矩阵 H H H 为
H = [ 1 0 ] . H = \begin{bmatrix} 1 & 0 \end{bmatrix}. H=[10].
解释：
该方程表明测量值 z k z_k zk等于真实温度 T T T 加上噪声，即
z k = T k + v k . z_k = T_k + v_k. zk=Tk+vk.
v = np.random.normal(0, np.sqrt(R[0, 0]))
z = H @ x_true + v
卡尔曼滤波基本步骤
卡尔曼滤波主要分为预测步骤和更新步骤。
1 预测步骤
-
状态预测：
x k ∣ k − 1 = A x k − 1 ∣ k − 1 x_{k|k-1} = A\, x_{k-1|k-1} xk∣k−1=Axk−1∣k−1
这表示利用上一次的后验状态估计来预测当前状态。
-
协方差预测：
P k ∣ k − 1 = A P k − 1 ∣ k − 1 A T + Q P_{k|k-1} = A\, P_{k-1|k-1}\, A^T + Q Pk∣k−1=APk−1∣k−1AT+Q
表示在预测时刻内的不确定性由上一时刻的不确定性通过状态转移加上过程噪声得到。
x_pred = A @ x_est
P_pred = A @ P_est @ A.T + Q
2 更新步骤
-
计算测量残差（创新）：
y k = z k − H x k ∣ k − 1 y_k = z_k - H\, x_{k|k-1} yk=zk−Hxk∣k−1
y = z - H @ x_pred
-
计算创新协方差：
S k = H P k ∣ k − 1 H T + R S_k = H\, P_{k|k-1}\, H^T + R Sk=HPk∣k−1HT+R
S = H @ P_pred @ H.T + R
-
计算卡尔曼增益：
K k = P k ∣ k − 1 H T S k − 1 K_k = P_{k|k-1}\, H^T\, S_k^{-1} Kk=Pk∣k−1HTSk−1
K = P_pred @ H.T @ np.linalg.inv(S)
-
状态更新：
x k ∣ k = x k ∣ k − 1 + K k y k x_{k|k} = x_{k|k-1} + K_k\, y_k xk∣k=xk∣k−1+Kkyk
x_est = x_pred + K @ y
-
协方差更新：
P k ∣ k = ( I − K k H ) P k ∣ k − 1 P_{k|k} = (I - K_k\, H) \, P_{k|k-1} Pk∣k=(I−KkH)Pk∣k−1
P_est = (np.eye(P_pred.shape[0]) - K @ H) @ P_pred