基于IEKF迭代扩展卡尔曼滤波算法的数据跟踪matlab仿真,对比EKF和UKF

1.程序功能描述
       基于IEKF迭代扩展卡尔曼滤波算法的数据跟踪matlab仿真,对比EKF和UKF.仿真输出误差收敛曲线和误差协方差收敛曲线。

2.测试软件版本以及运行结果展示
MATLAB2022A版本运行

（完整程序运行后无水印）

3.核心程序

for i=1:Lekf-1
    z                          = z_bar(i,:);
    [x_u(:,i+1), cov_ukf(:,:,i+1)] = func_ukf(f, x_u(:,i), cov_ukf(:,:,i), h, z', Q_u, R_u);
end
Err_ekf       = x   - Ang_L(1:dt/Ltm:end,:)';
Err_iekf      = x_i - Ang_L(1:dt/Ltm:end,:)';
Err_ukf       = x_u - Ang_L(1:dt/Ltm:end,:)';
 
%协方差变化图
cov_ekf2      = reshape(cov_ekf, [4, Lekf]);
cov_iekf2     = reshape(cov_iekf, [4, Lekf]);
cov_ukf2      = reshape(cov_ukf, [4, Lekf]);
 
 
figure
plot(mod(Err_ekf(1,:)-pi, 2*pi).^2)
hold on
plot(mod(Err_iekf(1,:)-pi,2*pi).^2)
hold on
plot(mod(Err_ukf(1,:)-pi,2*pi).^2)
title('角度误差')
legend('EKF','iEKF','UKF');
 
 
figure
plot(cov_ekf2(1,:))
hold on
plot(cov_iekf2(1,:))
hold on
plot(cov_ukf2(1,:))
title('协方差1,1')
legend('EKF','iEKF','UKF');
 
 
figure
plot(cov_ekf2(4,:))
hold on
plot(cov_iekf2(4,:))
hold on
plot(cov_ukf2(4,:))
title('协方差2,2')
legend('EKF','iEKF','UKF');

4.本算法原理
        迭代扩展卡尔曼滤波（Iterated Extended Kalman Filter, IEKF）是一种改进型的滤波算法，旨在通过多次迭代的方式提高扩展卡尔曼滤波（Extended Kalman Filter, EKF）的估计精度。

       EKF是卡尔曼滤波的一种非线性扩展，适用于状态方程和观测方程均为非线性的情况。其基本思想是在每次滤波步骤中对非线性函数进行一阶泰勒展开，从而近似为线性系统处理。

        UKF避免了EKF中的线性化步骤，通过无迹变换直接处理非线性问题。UKF通过选择一组称为sigma点的特殊点，来近似状态分布和通过这些点传播非线性函数。

        通过将sigma点通过状态转移函数和观测函数传播，再通过加权平均来近似状态和协方差的预测与更新，具体步骤较为复杂，但核心在于保留了非线性函数的二阶特性。

       IEKF在EKF的基础上，对预测和/或更新步骤进行多次迭代，以进一步减小线性化误差。迭代可以是对非线性函数的泰勒展开进行更高阶的近似，或直接对预测和更新步骤进行多次执行，直到某种收敛条件满足。

比较与总结:

EKF通过一次线性化处理非线性问题，简单但可能因线性近似误差导致性能下降。

UKF无需线性化，通过sigma点直接在非线性空间内操作，提高了精度，但计算量相对较大。

IEKF通过多次迭代线性化过程，增强了EKF的非线性处理能力，尤其适用于非线性强或系统不确定性高的情况，但增加了计算复杂度和迭代次数的确定问题。