文章目录
扩展卡尔曼滤波的原理、公式推导及计算步骤
一、原理
扩展卡尔曼滤波（Extended Kalman Filter，EKF）是卡尔曼滤波的一种变种，主要用于处理非线性系统的状态估计问题。传统卡尔曼滤波假设系统模型和观测模型都是线性的，而EKF通过线性化这些非线性模型，使得卡尔曼滤波的框架可以应用于非线性系统。
在EKF中，非线性系统模型被近似为局部线性的，通常通过计算雅可比矩阵（Jacobian matrix）来实现。雅可比矩阵描述了非线性函数相对于其输入变量的偏导数，从而实现了对非线性函数的线性化近似。
二、公式推导
(1) 状态预测方程
对于非线性系统，状态预测方程通常表示为：
x k ∣ k − 1 = f ( x k − 1 ∣ k − 1 , u k − 1 ) x_{k|k-1} = f(x_{k-1|k-1}, u_{k-1}) xk∣k−1=f(xk−1∣k−1,uk−1)
其中， x k ∣ k − 1 x_{k|k-1} xk∣k−1 是基于 k − 1 k-1 k−1 时刻状态预测得到的 k k k 时刻状态， f f f 是非线性状态转移函数， x k − 1 ∣ k − 1 x_{k-1|k-1} xk−1∣k−1 是 k − 1 k-1 k−1 时刻的后验状态估计， u k − 1 u_{k-1} uk−1 是 k − 1 k-1 k−1 时刻的控制输入。
(2) 协方差预测方程
状态协方差的预测方程为：
P k ∣ k − 1 = F k − 1 P k − 1 ∣ k − 1 F k − 1 T + Q k − 1 P_{k|k-1} = F_{k-1} \, P_{k-1|k-1} \, F_{k-1}^T + Q_{k-1} Pk∣k−1=Fk−1Pk−1∣k−1Fk−1T+Qk−1
其中， P k ∣ k − 1 P_{k|k-1} Pk∣k−1 是预测的 k k k 时刻状态协方差， F k − 1 F_{k-1} Fk−1 是状态转移函数 f f f 关于状态 x k − 1 ∣ k − 1 x_{k-1|k-1} xk−1∣k−1 的雅可比矩阵， P k − 1 ∣ k − 1 P_{k-1|k-1} Pk−1∣k−1 是 k − 1 k-1 k−1 时刻的后验状态协方差， Q k − 1 Q_{k-1} Qk−1 是过程噪声的协方差矩阵。
(3) 观测方程
对于非线性观测模型，观测方程通常表示为：
z k = h ( x k ) + v k z_{k} = h(x_{k}) + v_{k} zk=h(xk)+vk
其中， z k z_{k} zk 是 k k k 时刻的观测值， h h h 是非线性观测函数， x k x_{k} xk 是 k k k 时刻的真实状态， v k v_{k} vk 是观测噪声。
(4) 观测雅可比矩阵
观测方程的雅可比矩阵为：
H k = ∂ h ( x k ) ∂ x k ∣ x k = x k ∣ k − 1 H_{k} = \frac{\partial h(x_{k})}{\partial x_{k}} \Bigg|_{x_{k}=x_{k|k-1}} Hk=∂xk∂h(xk) xk=xk∣k−1
其中， H k H_{k} Hk 是观测函数 h h h 关于状态 x k x_{k} xk 的雅可比矩阵，在 x k = x k ∣ k − 1 x_{k}=x_{k|k-1} xk=xk∣k−1 处计算。
(5) 卡尔曼增益
卡尔曼增益的计算公式为：
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 K_{k} = P_{k|k-1} \, H_{k}^T \, \Bigl( H_{k} \, P_{k|k-1} \, H_{k}^T + R_{k} \Bigr)^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
其中， K k K_{k} Kk 是卡尔曼增益， R k R_{k} Rk 是观测噪声的协方差矩阵。
(6) 状态更新方程
状态更新方程为：
x k ∣ k = x k ∣ k − 1 + K k ( z k − h ( x k ∣ k − 1 ) ) x_{k|k} = x_{k|k-1} + K_{k} \Bigl( z_{k} - h(x_{k|k-1}) \Bigr) xk∣k=xk∣k−1+Kk(zk−h(xk∣k−1))
其中， x k ∣ k x_{k|k} xk∣k 是 k k k 时刻的后验状态估计。
(7) 协方差更新方程
协方差更新方程为：
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = \Bigl( I - K_{k} \, H_{k} \Bigr) \, P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1
其中， P k ∣ k P_{k|k} Pk∣k 是 k k k 时刻的后验状态协方差， I I I 是单位矩阵。
三、计算步骤
-
初始化：设置初始状态估计 x 0 ∣ 0 x_{0|0} x0∣0 和初始状态协方差 P 0 ∣ 0 P_{0|0} P0∣0。
-
状态预测：使用状态预测方程计算预测状态 x k ∣ k − 1 x_{k|k-1} xk∣k−1。
-
协方差预测：使用协方差预测方程计算预测协方差 P k ∣ k − 1 P_{k|k-1} Pk∣k−1。
-
计算观测雅可比矩阵：计算观测函数 h h h 关于状态 x k x_{k} xk 的雅可比矩阵 H k H_{k} Hk。
-
计算卡尔曼增益：使用卡尔曼增益的计算公式计算卡尔曼增益 K k K_{k} Kk。
-
状态更新：使用状态更新方程计算后验状态估计 x k ∣ k x_{k|k} xk∣k。
-
协方差更新：使用协方差更新方程计算后验状态协方差 P k ∣ k P_{k|k} Pk∣k。
-
重复：对于下一个时间步 k + 1 k+1 k+1，重复步骤 2 到步骤 7。
四、例子
import numpy as np
import matplotlib.pyplot as plt
# 系统参数
dt = 0.1 # 时间步长
T = 10 # 总时间
num_steps = int(T / dt)
# 状态转移函数 (非线性)
def f(x, dt):
F = np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]])
return F @ x
# 观测函数 (非线性)
def h(x):
px, py, _, _ = x
r = np.sqrt(px**2 + py**2) # 距离
theta = np.arctan2(py, px) # 角度
return np.array([r, theta])
# 状态转移矩阵的雅可比矩阵
def F_jacobian(x, dt):
return np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]])
# 观测矩阵的雅可比矩阵
def H_jacobian(x):
px, py, _, _ = x
r = np.sqrt(px**2 + py**2)
H = np.array([[px / r, py / r, 0, 0],
[-py / (r**2), px / (r**2), 0, 0]])
return H
# 初始化
x_true = np.array([0, 0, 1, 1]) # 真实状态 [px, py, vx, vy]
x_est = np.array([0, 0, 1, 1]) # 估计状态
P_est = np.eye(4) # 估计协方差
Q = np.diag([0.1, 0.1, 0.1, 0.1]) # 过程噪声协方差
R = np.diag([0.5, 0.1]) # 观测噪声协方差
# 存储结果
true_states = []
estimated_states = []
measurements = []
# EKF主循环
for t in range(num_steps):
# 真实状态更新
x_true = f(x_true, dt)
true_states.append(x_true)
# 生成观测值 (带噪声)
z_true = h(x_true)
z_noise = np.random.multivariate_normal([0, 0], R)
z = z_true + z_noise
measurements.append(z)
# 预测步骤
x_pred = f(x_est, dt)
F = F_jacobian(x_est, dt)
P_pred = F @ P_est @ F.T + Q
# 更新步骤
H = H_jacobian(x_pred)
y = z - h(x_pred) # 残差
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S) # 卡尔曼增益
x_est = x_pred + K @ y
P_est = (np.eye(4) - K @ H) @ P_pred
estimated_states.append(x_est)
# 转换为NumPy数组
true_states = np.array(true_states)
estimated_states = np.array(estimated_states)
measurements = np.array(measurements)
# 可视化
plt.figure(figsize=(12, 8))
# 真实轨迹 vs 估计轨迹
plt.subplot(2, 2, 1)
plt.plot(true_states[:, 0], true_states[:, 1], label="True Trajectory", color="blue")
plt.plot(estimated_states[:, 0], estimated_states[:, 1], label="Estimated Trajectory", color="red", linestyle="--")
plt.scatter(measurements[:, 0] * np.cos(measurements[:, 1]), measurements[:, 0] * np.sin(measurements[:, 1]),
label="Measurements", color="green", marker="x")
plt.xlabel("Position X")
plt.ylabel("Position Y")
plt.title("True vs Estimated Trajectory")
plt.legend()
plt.grid()
# 位置误差
position_error = np.linalg.norm(true_states[:, :2] - estimated_states[:, :2], axis=1)
plt.subplot(2, 2, 2)
plt.plot(position_error, label="Position Error", color="purple")
plt.xlabel("Time Step")
plt.ylabel("Error")
plt.title("Position Error")
plt.legend()
plt.grid()
# 速度误差
velocity_error = np.linalg.norm(true_states[:, 2:] - estimated_states[:, 2:], axis=1)
plt.subplot(2, 2, 3)
plt.plot(velocity_error, label="Velocity Error", color="orange")
plt.xlabel("Time Step")
plt.ylabel("Error")
plt.title("Velocity Error")
plt.legend()
plt.grid()
plt.tight_layout()
plt.show()
运行结果：
(1) 系统模型
设状态向量为
x = [ p x p y v x v y ] , x = \begin{bmatrix} p_x \\ p_y \\ v_x \\ v_y \end{bmatrix}, x=
pxpyvxvy
,
其中 p x , p y p_x, p_y px,py表示位置， v x , v y v_x, v_y vx,vy 表示速度。
(2)状态转移函数（预测方程）
状态转移采用匀速模型，离散化形式为：
x k + 1 = f ( x k , d t ) = F x k , x_{k+1} = f(x_k, dt) = F \, x_k, xk+1=f(xk,dt)=Fxk,
其中状态转移矩阵为
F = [ 1 0 d t 0 0 1 0 d t 0 0 1 0 0 0 0 1 ] . F = \begin{bmatrix} 1 & 0 & dt & 0 \\ 0 & 1 & 0 & dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}. F=
10000100dt0100dt01
.
预测方程的展开解释
在本例中，我们假设目标遵循匀速运动模型，即在每个时间步内速度保持不变，位置根据当前速度更新。状态向量定义为
x = [ p x p y v x v y ] , x = \begin{bmatrix} p_x \\ p_y \\ v_x \\ v_y \end{bmatrix}, x=
pxpyvxvy
,
其中 p x p_x px 和 p y p_y py 表示位置分量， v x v_x vx 和 v y v_y vy 表示速度分量。
连续时间下的运动模型
在连续时间下，匀速运动的基本运动学公式为
p x ( t + Δ t ) = p x ( t ) + v x ( t ) ⋅ Δ t , p_x(t + \Delta t) = p_x(t) + v_x(t) \cdot \Delta t, px(t+Δt)=px(t)+vx(t)⋅Δt,
p y ( t + Δ t ) = p y ( t ) + v y ( t ) ⋅ Δ t , p_y(t + \Delta t) = p_y(t) + v_y(t) \cdot \Delta t, py(t+Δt)=py(t)+vy(t)⋅Δt,
v x ( t + Δ t ) = v x ( t ) , v_x(t + \Delta t) = v_x(t), vx(t+Δt)=vx(t),
v y ( t + Δ t ) = v y ( t ) . v_y(t + \Delta t) = v_y(t). vy(t+Δt)=vy(t).
离散化（预测方程）
将上述连续时间公式在固定时间步长 d t dt dt 下离散化，我们得到
p x ( k + 1 ) = p x ( k ) + d t ⋅ v x ( k ) , p_x(k+1) = p_x(k) + dt \cdot v_x(k), px(k+1)=px(k)+dt⋅vx(k),
p y ( k + 1 ) = p y ( k ) + d t ⋅ v y ( k ) , p_y(k+1) = p_y(k) + dt \cdot v_y(k), py(k+1)=py(k)+dt⋅vy(k),
v x ( k + 1 ) = v x ( k ) , v_x(k+1) = v_x(k), vx(k+1)=vx(k),
v y ( k + 1 ) = v y ( k ) . v_y(k+1) = v_y(k). vy(k+1)=vy(k).
可以将这些公式用矩阵形式表达为
x k + 1 = f ( x k , d t ) = F x k , x_{k+1} = f(x_k, dt) = F \, x_k, xk+1=f(xk,dt)=Fxk,
其中状态转移矩阵 F F F 为 ，表示匀速运动
F = [ 1 0 d t 0 0 1 0 d t 0 0 1 0 0 0 0 1 ] . F = \begin{bmatrix} 1 & 0 & dt & 0 \\ 0 & 1 & 0 & dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}. F=
10000100dt0100dt01
.
-
第一行表示：
p x ( k + 1 ) = 1 ⋅ p x ( k ) + d t ⋅ v x ( k ) , p_x(k+1) = 1 \cdot p_x(k) + dt \cdot v_x(k), px(k+1)=1⋅px(k)+dt⋅vx(k),
即下一个时刻的横向位置等于当前横向位置加上横向速度乘以时间步长 d t dt dt。 -
第二行表示：
p y ( k + 1 ) = 1 ⋅ p y ( k ) + d t ⋅ v y ( k ) , p_y(k+1) = 1 \cdot p_y(k) + dt \cdot v_y(k), py(k+1)=1⋅py(k)+dt⋅vy(k),
同理，纵向位置更新类似。 -
第三、四行表示：
v x ( k + 1 ) = v x ( k ) , v y ( k + 1 ) = v y ( k ) , v_x(k+1) = v_x(k), \quad v_y(k+1) = v_y(k), vx(k+1)=vx(k),vy(k+1)=vy(k),
表示速度保持不变（匀速运动）。
因此，预测方程的展开实际上就是利用基本运动学原理，将状态在下一个时间步的预测表示为当前状态乘以状态转移矩阵 F F F。
(3)观测函数
观测函数将直角坐标转换为极坐标：
z k = h ( x k ) = [ r θ ] = [ p x 2 + p y 2 arctan 2 ( p y , p x ) ] . z_k = h(x_k) = \begin{bmatrix} r \\ \theta \end{bmatrix} = \begin{bmatrix} \sqrt{p_x^2 + p_y^2} \\ \arctan2(p_y, p_x) \end{bmatrix}. zk=h(xk)=[rθ]=[px2+py2arctan2(py,px)].
(4) 雅可比矩阵
状态转移函数的雅可比矩阵
由于状态转移为线性形式，雅可比矩阵直接为
F k = ∂ f ∂ x = F = [ 1 0 d t 0 0 1 0 d t 0 0 1 0 0 0 0 1 ] . F_k = \frac{\partial f}{\partial x} = F = \begin{bmatrix} 1 & 0 & dt & 0 \\ 0 & 1 & 0 & dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}. Fk=∂x∂f=F=
10000100dt0100dt01
.
观测函数的雅可比矩阵
对观测函数 (h(x)) 求导，得到：
H k = ∂ h ( x ) ∂ x = [ ∂ r ∂ p x ∂ r ∂ p y 0 0 ∂ θ ∂ p x ∂ θ ∂ p y 0 0 ] . H_k = \frac{\partial h(x)}{\partial x} = \begin{bmatrix} \frac{\partial r}{\partial p_x} & \frac{\partial r}{\partial p_y} & 0 & 0 \\ \frac{\partial \theta}{\partial p_x} & \frac{\partial \theta}{\partial p_y} & 0 & 0 \end{bmatrix}. Hk=∂x∂h(x)=[∂px∂r∂px∂θ∂py∂r∂py∂θ0000].
其中，
∂ r ∂ p x = p x r , ∂ r ∂ p y = p y r , \frac{\partial r}{\partial p_x} = \frac{p_x}{r},\quad \frac{\partial r}{\partial p_y} = \frac{p_y}{r}, ∂px∂r=rpx,∂py∂r=rpy,
∂ θ ∂ p x = − p y r 2 , ∂ θ ∂ p y = p x r 2 . \frac{\partial \theta}{\partial p_x} = -\frac{p_y}{r^2},\quad \frac{\partial \theta}{\partial p_y} = \frac{p_x}{r^2}. ∂px∂θ=−r2py,∂py∂θ=r2px.
因此，
H k = [ p x r p y r 0 0 − p y r 2 p x r 2 0 0 ] . H_k = \begin{bmatrix} \frac{p_x}{r} & \frac{p_y}{r} & 0 & 0 \\ -\frac{p_y}{r^2} & \frac{p_x}{r^2} & 0 & 0 \end{bmatrix}. Hk=[rpx−r2pyrpyr2px0000].
（注意：当 (r) 非常小时，为避免除零可设定下限。）
(5) EKF 核心步骤
预测步骤
-
状态预测
x ˉ k = f ( x k − 1 , d t ) = F x k − 1 . \bar{x}_k = f(x_{k-1}, dt) = F \, x_{k-1}. xˉk=f(xk−1,dt)=Fxk−1. -
协方差预测
P k − = F k P k − 1 F k T + Q , P_k^{-} = F_k \, P_{k-1} \, F_k^T + Q, Pk−=FkPk−1FkT+Q,
其中 (Q) 为过程噪声协方差矩阵。
更新步骤
-
测量预测
z ˉ k = h ( x ˉ k ) . \bar{z}_k = h(\bar{x}_k). zˉk=h(xˉk). -
计算残差（创新）
y k = z k − z ˉ k . y_k = z_k - \bar{z}_k. yk=zk−zˉk. -
计算创新协方差
S k = H k P k − H k T + R , S_k = H_k \, P_k^{-} \, H_k^T + R, Sk=HkPk−HkT+R,
其中 (R) 为观测噪声协方差矩阵。 -
计算卡尔曼增益
K k = P k − H k T S k − 1 . K_k = P_k^{-} \, H_k^T \, S_k^{-1}. Kk=Pk−HkTSk−1. -
状态更新
x k = x ˉ k + K k y k . x_k = \bar{x}_k + K_k \, y_k. xk=xˉk+Kkyk. -
协方差更新
P k = ( I − K k H k ) P k − . P_k = (I - K_k \, H_k) \, P_k^{-}. Pk=(I−KkHk)Pk−.
以上公式对应了 Python 代码中 EKF 的核心观测与预测方程、雅可比计算以及主要更新步骤。