【PSINS工具箱】EKF与UKF滤波

描述

对工具箱SINS/GPS，153例程的修改，将EKF和UKF放在一个文件里面，一次运行可以得到两个滤波的结果。

片段

运行截图

程序完整源代码

在有工具箱的情况下，直接运行此代码，即可得到结果

% 基于PSINS工具箱的IMU数据生成与滤波
% date:2024-2-15
% Evand（evandworld@qq.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.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))));