描述
对工具箱SINS/GPS，153例程的修改，将EKF和UKF放在一个文件里面，一次运行可以得到两个滤波的结果。
片段
运行截图
程序完整源代码
在有工具箱的情况下，直接运行此代码，即可得到结果
matlab
% 基于PSINS工具箱的IMU数据生成与滤波
% date:2024-2-15
% Evand（[email protected]）
% 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.pconstrain=1;
len = length(imu); [avp_ekf, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, 'KF');
ki = 1;
for k=1:nn:len-nn+1
k1 = k+nn-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)==0
posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1); % GPS pos simulation with some white noise
kf = 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 = ki+1;
end
timebar;
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.n+1);
timebar(nn, len, '15-state SINS/GPS UKF仿真，时间较长');
ki = 1;
for k=1:nn:len-nn+1
k1 = k+nn-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)==0
posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1); % GPS pos simulation with some white noise
kf = 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 = ki+1;
end
timebar;
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))));