注意！误差状态卡尔曼滤波器是一种拓展卡尔曼滤波器！大部分的公式是可以复用原生卡尔曼滤波器的，我们不会对共性部分做太多详细的解读，插班生请先复习：
误差状态卡尔曼滤波器？
误差状态卡尔曼滤波器(Error State Kalman Filter, ESKF)是一种拓展卡尔曼滤波器，其应用十分广泛，从GINS组合导航到视觉SLAM、外参自标定等任务中都有应用，也可以实现LIO、VIO等复杂系统功能。
相比原生的卡尔曼滤波器(Kalman Filter,KF)，ESKF不直接对完整状态建模，而是对误差状态（Error-State）进行建模。优点能归纳如下：
1.ESKF使用三维变量来表达旋转增量，而KF需要用四元数或者旋转矩阵来表达状态，又或使用有奇异性的欧拉角
2.ESKF总是工作在原点附近，离奇异点比较远，数值更稳定，并且不会出现离工作点太远导致不工作的问题
3.ESKF的状态量为小量，其二阶变量可以忽略，大多数的雅可比矩阵在小量情况下变得非常简单，甚至可以代替为单位矩阵
还记得我们上一期主要介绍了卡尔曼滤波器的五个公式对吗！那样做是为了能让我们尽快上手滤波器。但是直接给公式有些知其然但不知其所以然。我们本期从滤波器处理的两个过程入手，手搓ESKF！
预测过程
在误差状态卡尔曼滤波（ESKF）中，预测过程用于基于IMU测量值更新主状态，并传播误差状态。由于IMU提供的是线加速度和角速度的测量值，我们需要从这些测量值推导出系统的状态变化。
预测过程的核心任务有两个：1主状态的预测、2误差状态的传播。
主状态预测
假设我们只把小车的速度以及位置作为状态量，IMU的读数为加速度，则根据经典的牛顿运动学公式：
其中： 是时间步长。 是当前时间步 的加速度（已去除重力）。
把这个方程组用矩阵形式表达，就得到了状态转移矩阵：
其中：
这样我们就根据IMU提供的加速度更新了主状态方程（也有翻译为名义状态方程）！
误差状态传递
ESKF是对误差状态进行进行滤波，我们还需要再把误差状态传递出来以供更新过程使用！误差状态是对状态向量的误差进行建模，这里有位置误差和速度误差两个：
误差状态传播方程表示上一时刻的误差是如何变化为当前时刻的误差的：
其中:
注意！这里的是误差状态转移矩阵，它比状态转移矩阵要简单很多，这也是ESKF的优势之一。它的物理含义：
I 是单位矩阵 。 反映了速度误差对位置误差的影响： 位置误差 受速度误差 影响，随着时间 传播。 速度误差不会影响自己，因此下半部分是单位矩阵。
另外，是过程噪声，前面乘上一个过程噪声矩阵来表达噪声是如何影响误差状态的。至此，我们得到了ESKF的第二个公式：误差状态传播方程。
误差噪声协方差方程
这部分和KF是一致的，只不过KF是对整体状态传播方程推导状态噪声协方差，ESKF是对误差状态传播方程推导误差噪声协方差（大雾）。
总之呢，由于误差状态向量是符合高斯分布的，所以需要有一个矩阵来表达多个噪声之间的关系（也就是误差噪声协方差矩阵）。
根据协方差矩阵的性质（这里不再证明）：
再把上面计算得到的误差状态传播方程带入，就有误差协方差矩阵传递公式的简单形式：
但是由于我们的预测模型仍然有不确定性，所以加一个尾巴 加以修正：
这里，是协方差矩阵，表示两种误差之间的关系，同样不再证明。我们得到了第三个公式：误差噪声协方差传播方程。
更新过程
我们还是使用GNSS修正位置误差。由于GNSS仅提供位置测量值，误差状态更新方程和卡尔曼滤波的状态更新方程几乎一致，这里直接上公式：
后面的表达为GNSS观测值和预测值的差值
是卡尔曼增益，证明省略。
噪声协方差矩阵的更新
这个是留给下一轮迭代时使用的。
表示了如何将卡尔曼增益 应用到预测误差协方差 上。
误差状态应用到主状态
这一步主要把计算出的误差状态加到主状态上进行修正：
误差状态重置：
每次迭代之后要把误差状态重置：
import numpy as npimport matplotlib.pyplot as plt# 仿真参数dt = 0.1 # 采样时间间隔 (s)total_time = 10 # 总时间 (s)num_steps = int(total_time / dt) # 时间步数# 真实系统模型 (匀加速运动)true_acceleration = 1.0 # 真实加速度 (m/s^2)true_velocity = 0.0 # 初始速度true_position = 0.0 # 初始位置# 过程噪声 (IMU 误差)acc_noise_std = 0.1 # 加速度测量噪声标准差# GNSS 位置测量噪声gnss_noise_std = 0.5 # GNSS 位置噪声标准差gnss_update_rate = 1 # GNSS 每隔多少步更新一次# 状态变量 [p, v]X = np.array([[0.0], [0.0]]) # 初始状态 (位置, 速度)# 误差状态协方差矩阵 PP = np.diag([1.0, 1.0]) # 初始协方差# 过程噪声协方差矩阵 Q (假设 IMU 误差)Q = np.array([[0.5 * dt**2, 0.5 * dt],[0.5 * dt, dt]]) * acc_noise_std**2# 观测噪声协方差矩阵 R (GNSS 误差)R = np.array([[gnss_noise_std**2]])# 状态转移矩阵 FF = np.array([[1, dt],[0, 1]])# 过程噪声影响矩阵 GG = np.array([[0.5 * dt**2],[dt]])# 观测矩阵 H (GNSS 只测量位置)H = np.array([[1, 0]])# 记录数据true_positions = []true_velocities = []imu_measurements = []gnss_measurements = []estimated_positions = []estimated_velocities = []for k in range(num_steps):# 真实系统更新true_velocity += true_acceleration * dttrue_position += true_velocity * dt# IMU 测量加速度 (带噪声)imu_acc = true_acceleration + np.random.normal(0, acc_noise_std)# 预测步骤 (Propagation)X = F @ X + G * imu_accP = F @ P @ F.T + Q # 误差协方差传播# GNSS 测量 (每 gnss_update_rate 步进行一次)if k % gnss_update_rate == 0:gnss_position = true_position + np.random.normal(0, gnss_noise_std)# 计算卡尔曼增益S = H @ P @ H.T + RK = P @ H.T @ np.linalg.inv(S)# 计算创新项y = gnss_position - H @ X# 更新步骤 (Correction)X = X + K @ yP = (np.eye(2) - K @ H) @ Pgnss_measurements.append(gnss_position)else:gnss_measurements.append(None)# 记录数据true_positions.append(true_position)true_velocities.append(true_velocity)imu_measurements.append(imu_acc)estimated_positions.append(X[0, 0])estimated_velocities.append(X[1, 0])# 绘图plt.figure(figsize=(12, 5))# 位置估计对比plt.subplot(2, 1, 1)plt.plot(np.arange(num_steps) * dt, true_positions, label="True Position")plt.plot(np.arange(num_steps) * dt, estimated_positions, label="Estimated Position", linestyle="--")plt.scatter(np.arange(num_steps) * dt, gnss_measurements, label="GNSS Measurement", color="r", marker="x")plt.xlabel("Time (s)")plt.ylabel("Position (m)")plt.title("Position Estimation")plt.legend()plt.grid()# 速度估计对比plt.subplot(2, 1, 2)plt.plot(np.arange(num_steps) * dt, true_velocities, label="True Velocity")plt.plot(np.arange(num_steps) * dt, estimated_velocities, label="Estimated Velocity", linestyle="--")plt.xlabel("Time (s)")plt.ylabel("Velocity (m/s)")plt.title("Velocity Estimation")plt.legend()plt.grid()plt.tight_layout()plt.show()
往期回顾
