基于PSINS工具箱的EKF和UKF滤波与结果对比

基于PSINS工具箱，自定义三维的运动轨迹，并由轨迹换算出来的SINS数据、GNSS数据进行扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF的结果展示与效果对比。

注意

文件只有一个m文件，但是必须搭配工具箱，如果遇到我的私有函数，在github链接https://github.com/evandworld/psins240101上面下载。

如果不安装工具箱是无法正常运行的！！

部分代码

轨迹设置：

%% 轨迹设置
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % init avp
% trajectory segment setting
traj_ = [];
seg = trjsegment(traj_, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   10, traj_, 1);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'climb',        10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'descent',      10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'deaccelerate', 5,  traj_, 2); %2
seg = trjsegment(seg, 'uniform',      100);

原创的3D轨迹图绘制程序：

figure;
plot3(trj.avp(:,7),trj.avp(:,8),trj.avp(:,9));
hold on
plot3(trj.avp(1,7),trj.avp(1,8),trj.avp(1,9),'*');
plot3(avp_ekf(:,7),avp_ekf(:,8),avp_ekf(:,9));
plot3(avp_ukf(:,7),avp_ukf(:,8),avp_ukf(:,9));
title('原创——3D轨迹图');
legend('真实值','起点','EKF滤波值','UKF滤波值');

下载地址

https://download.csdn.net/download/callmeup/88863883