如果熟悉 EKF 与卡尔曼的推导的话，iEKF 就比较容易理解，关于卡尔曼滤波的推导以及EKF，可以参考以前的文章：

卡尔曼滤波原理：https://blog.csdn.net/a_xiaoning/article/details/130564473?spm=1001.2014.3001.5502

EKF：https://blog.csdn.net/a_xiaoning/article/details/132786569?spm=1001.2014.3001.5502

iEKF 简单理解就是在状态更新之后，多次迭代更新雅可比矩阵，状态估计，以及协方差矩阵，设定收敛条件与最大迭代次数，满足条件再退出迭代。

二维例子

假设有以下的一个二维运动模型：

其中 w 为过程噪声，过程噪声 w 与协方差矩阵 Q 满足：

量测模型为：

其中 v 为量测噪声，量测噪声 v 与协方差矩阵 R 满足：

进而求解 F 与 H 矩阵：

然后就可以编写python的测试代码：

import numpy as np
import matplotlib.pyplot as plt
"""    运动模型：    
x(k) = x(k-1) + x_dot(k-1) * dt + w(k, 0)    
y(k) = y(k-1) + y_dot(k-1) * dt + w(k, 1)    
x_dot(k) = x_dot(k-1) + ax * dt + w(k, 2)    
y_dot(k) = y_dot(k-1) + ay * dt + w(k, 3)    
w -- 过程噪声        
观测模型：    
z(k) = [x(k)  y(k)]' + v   
v--量测噪声"""
# F 为先验对 x 的偏导    H 为量测对 x 的偏导
# 系统参数
N = 100  # 采样个数
dt = 0.1  # 时间步长
# 初始状态 [x, y, x_dot, y_dot]
x_init = np.array([0.0, 0.0, 1.0, 1.0])
a_x = 0.1  # 加速度
a_y = 0.2  # 加速度
Q = np.diag([0.1, 0.1, 0.1, 0.1])   # 过程噪声
w = np.random.multivariate_normal(np.zeros(4), Q, N).T  # 生成过程噪声
R = np.diag([0.5, 0.5])
v = np.random.multivariate_normal(np.zeros(2), R, N).T  # 量测噪声
# 生成真实轨迹和观测数据
def generate_data(x_init, N, dt, a_x, a_y, w, v):x_real = np.zeros((N, 4))  # 真实状态 [x, y, dx, dy]    z = np.zeros((N, 2))  # 观测数据 [z_x, z_y]    x_real[0] = x_initfor k in range(1, N):# 真实状态更新        x_real[k, 0] = x_real[k-1, 0] + x_real[k-1, 2] * dt + w[0, k]x_real[k, 1] = x_real[k-1, 1] + x_real[k-1, 3] * dt + w[1, k]x_real[k, 2] = x_real[k-1, 2] + a_x * dt + w[2, k]x_real[k, 3] = x_real[k-1, 3] + a_y * dt + w[3, k]# 观测模型        z[k, 0] = x_real[k, 0] + v[0, k]z[k, 1] = x_real[k, 1] + v[1, k]return x_real, z
# iEKF迭代
def iekf_step(x_hat, P, z, dt, Q, R, max_iter=10, epsilon=10):for k in range(1, N):# 时间更新        F = np.array([[1, 0, dt, 0],[0, 1, 0, dt],[0, 0, 1, 0],[0, 0, 0, 1]])x_hat[k] = F @ x_hat[k-1]P_ = F @ P @ F.T + Q# 测量更新       for _ in range(max_iter):H = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])z_pred = H @ x_hat[k]y = z[k] - z_predK = P_ @ H.T @ np.linalg.inv(H @ P_ @ H.T + R)x_hat_new = x_hat[k] + K @ yP_new = (np.eye(4) - K @ H) @ P_# 检查收敛            if np.linalg.norm(x_hat_new - x_hat[k]) < epsilon:x_hat[k] = x_hat_newP = P_newbreak            x_hat[k] = x_hat_newP = P_newreturn x_hat
# Press the green button in the gutter to run the script.
if __name__ == '__main__':# 生成真实轨迹与量测数据    x_real, z = generate_data(x_init, N, dt, a_x, a_y, w, v)# iEKF 估计    x_hat = np.zeros((N, 4))  # 估计状态    P = np.eye(4)  # 初始协方差矩阵    x_hat[0] = x_initx_hat = iekf_step(x_hat, P, z, dt, Q, R)# 绘制结果    plt.figure(figsize=(10, 6))plt.plot(x_real[:, 0], x_real[:, 1], label='True Trajectory', color='blue')plt.plot(x_hat[:, 0], x_hat[:, 1], label='Estimated Trajectory', color='red', linestyle='--')plt.scatter(z[:, 0], z[:, 1], label='Measurements', color='green', marker='o')plt.legend()plt.xlabel('X')plt.ylabel('Y')plt.title('iEKF')plt.grid(True)plt.show()

运行结果：

可以调整一下迭代收敛的阈值，阈值越小的话，就会越相信量测值，例如把阈值调到很小，波形就会变成：

后续在实际项目中实践算法的优劣。