房山广州网站建设,网站开发合同编号如何编写,临淄找工作信息网,wordpress无法进入登录页面描述
对工具箱SINS/GPS#xff0c;153例程的修改#xff0c;将EKF和UKF放在一个文件里面#xff0c;一次运行可以得到两个滤波的结果。
片段 运行截图 程序完整源代码
在有工具箱的情况下#xff0c;直接运行此代码#xff0c;即可得到结果
% 基于PSINS工具箱的IMU数据…描述
对工具箱SINS/GPS153例程的修改将EKF和UKF放在一个文件里面一次运行可以得到两个滤波的结果。
片段 运行截图 程序完整源代码
在有工具箱的情况下直接运行此代码即可得到结果
% 基于PSINS工具箱的IMU数据生成与滤波
% date:2024-2-15
% Evandevandworldqq.com
% Ver1
clear;clc;close all;
glvs
psinstypedef(153);
ts 0.1; % sampling interval
%% 轨迹设置
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);
% generate, save plot
trj trjsimu(avp0, seg.wat, ts, 1);
% trjfile(trj10ms.mat, trj);
%% 初始化
% initial settings
[nn, ts, nts] nnts(2, trj.ts);
imuerr imuerrset(0.03, 100, 0.001, 5);
imu imuadderr(trj.imu, imuerr);
davp0 avperrset([0.5;-0.5;20], 0.1, [1;1;3]);
ins insinit(avpadderr(trj.avp0,davp0), ts);
% KF filter
rk poserrset([1;1;3]);
kf kfinit(ins, davp0, imuerr, rk);
kf.Pmin [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2; kf.pconstrain1;
len length(imu); [avp_ekf, xkpk] prealloc(fix(len/nn), 10, 2*kf.n1);
timebar(nn, len, KF);
ki 1;
for k1:nn:len-nn1k1 knn-1; wvm imu(k:k1,1:6); t imu(k1,end);ins insupdate(ins, wvm);kf.Phikk_1 kffk(ins);kf kfupdate(kf);if mod(t,1)0posGPS trj.avp(k1,7:9) davp0(7:9).*randn(3,1); % GPS pos simulation with some white noisekf kfupdate(kf, ins.pos-posGPS, M);[kf, ins] kffeedback(kf, ins, 1, avp);avp_ekf(ki,:) [ins.avp, t];xkpk(ki,:) [kf.xk; diag(kf.Pxk); t]; ki ki1;endtimebar;
end
avp_ekf(ki:end,:) []; xkpk(ki:end,:) [];
%% EKF show results
insplot(avp_ekf);
avperr avpcmpplot(trj.avp, avp_ekf);
kfplot(xkpk, avperr, imuerr);%% UKF filter
glvs
% psinstypedef(test_SINS_GPS_UKF_153_def);
[nn, ts, nts] nnts(2, trj.ts);
imuerr imuerrset(0.03, 100, 0.001, 5);
imu imuadderr(trj.imu, imuerr);
davp0 avperrset([0.5;-0.5;20], 0.1, [1;1;3]);
ins insinit(avpadderr(trj.avp0,davp0), ts);rk poserrset([1;1;3]);
kf kfinit(ins, davp0, imuerr, rk);
kf.fx largephiu15ukf;
len length(imu); [avp_ukf, xkpk] prealloc(fix(len/nn), 10, 2*kf.n1);
timebar(nn, len, 15-state SINS/GPS UKF仿真时间较长);
ki 1;
for k1:nn:len-nn1k1 knn-1; wvm imu(k:k1,1:6); t imu(k1,end);ins insupdate(ins, wvm);kf.px ins;kf ukf(kf);if mod(t,1)0posGPS trj.avp(k1,7:9) davp0(7:9).*randn(3,1); % GPS pos simulation with some white noisekf ukf(kf, ins.pos-posGPS, M); % UKF filter[kf, ins] kffeedback(kf, ins, 1, avp);avp_ukf(ki,:) [ins.avp, t];xkpk(ki,:) [kf.xk; diag(kf.Pxk); t]; ki ki1;endtimebar;
end
avp_ukf(ki:end,:) []; xkpk(ki:end,:) []; %% results
insplot(trj.avp);
imuplot(trj.imu);
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滤波值);
% 误差绘图
% 误差曲线图与累积分布函数图两种情况、三轴
figure;
subplot(3,2,1);
plot(1:10:len,avp_ekf(:,7)-trj.avp(1:10:len,7),1:10:len,avp_ukf(:,7)-trj.avp(1:10:len,7));
title(原创——X轴位置误差对比);legend(EKF滤波值,UKF滤波值);
subplot(3,2,3);
plot(1:10:len,avp_ekf(:,8)-trj.avp(1:10:len,8),1:10:len,avp_ukf(:,8)-trj.avp(1:10:len,8));
subplot(3,2,5);
plot(1:10:len,avp_ekf(:,9)-trj.avp(1:10:len,9),1:10:len,avp_ukf(:,9)-trj.avp(1:10:len,9));
subplot(3,2,2);
cdfplot(abs(avp_ekf(:,7)-trj.avp(1:10:len,7)));
hold on
cdfplot(abs(avp_ukf(:,7)-trj.avp(1:10:len,7)));
subplot(3,2,4);
cdfplot(abs(avp_ekf(:,8)-trj.avp(1:10:len,8)));
hold on
cdfplot(abs(avp_ukf(:,8)-trj.avp(1:10:len,8)));
subplot(3,2,6);
cdfplot(abs(avp_ekf(:,9)-trj.avp(1:10:len,9)));
hold on
cdfplot(abs(avp_ukf(:,9)-trj.avp(1:10:len,9)));
%% 误差输出
fprintf(ekf X轴位置误差最大值%d\n,max(abs(avp_ekf(:,7)-trj.avp(1:10:len,7))));
fprintf(ekf Y轴位置误差最大值%d\n,max(abs(avp_ekf(:,8)-trj.avp(1:10:len,8))));
fprintf(ekf Z轴位置误差最大值%d\n,max(abs(avp_ekf(:,9)-trj.avp(1:10:len,9))));
fprintf(ukf X轴位置误差最大值%d\n,max(abs(avp_ukf(:,7)-trj.avp(1:10:len,7))));
