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

最新推荐文章于 2025-01-14 10:47:55 发布

原创最新推荐文章于 2025-01-14 10:47:55 发布 · 542 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #算法 #github

卡尔曼专题免费专栏专栏收录该内容

98 篇文章

订阅专栏

本文介绍了如何使用PSINS工具箱自定义三维运动轨迹，并展示了通过轨迹计算得到的SINS和GNSS数据经过EKF和UKF滤波后的结果，以及两者之间的效果对比。作者还提供了原始的3D轨迹图绘制程序代码。

基于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滤波值');