iEKF的二维应用实例

news2025/6/2 8:15:27

如果熟悉 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_init
    for 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_pred
            K = P_ @ H.T @ np.linalg.inv(H @ P_ @ H.T + R)
            x_hat_new = x_hat[k] + K @ y
            P_new = (np.eye(4) - K @ H) @ P_
            # 检查收敛            
            if np.linalg.norm(x_hat_new - x_hat[k]) < epsilon:
                x_hat[k] = x_hat_new
                P = P_new
                break            
                x_hat[k] = x_hat_new
                P = P_new
    return 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_init
    x_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()

运行结果：