目录
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');
71
4.本算法原理
迭代扩展卡尔曼滤波(Iterated Extended Kalman Filter, IEKF)是一种改进型的滤波算法,旨在通过多次迭代的方式提高扩展卡尔曼滤波(Extended Kalman Filter, EKF)的估计精度。
EKF是卡尔曼滤波的一种非线性扩展,适用于状态方程和观测方程均为非线性的情况。其基本思想是在每次滤波步骤中对非线性函数进行一阶泰勒展开,从而近似为线性系统处理。
UKF避免了EKF中的线性化步骤,通过无迹变换直接处理非线性问题。UKF通过选择一组称为sigma点的特殊点,来近似状态分布和通过这些点传播非线性函数。
通过将sigma点通过状态转移函数和观测函数传播,再通过加权平均来近似状态和协方差的预测与更新,具体步骤较为复杂,但核心在于保留了非线性函数的二阶特性。
IEKF在EKF的基础上,对预测和/或更新步骤进行多次迭代,以进一步减小线性化误差。迭代可以是对非线性函数的泰勒展开进行更高阶的近似,或直接对预测和更新步骤进行多次执行,直到某种收敛条件满足。
比较与总结:
EKF通过一次线性化处理非线性问题,简单但可能因线性近似误差导致性能下降。
UKF无需线性化,通过sigma点直接在非线性空间内操作,提高了精度,但计算量相对较大。
IEKF通过多次迭代线性化过程,增强了EKF的非线性处理能力,尤其适用于非线性强或系统不确定性高的情况,但增加了计算复杂度和迭代次数的确定问题。
5.完整程序
VVV