【基于PSINS】UKF/SSUKF对比的MATLAB程序

发布于:2024-07-26 ⋅ 阅读:(96) ⋅ 点赞:(0)

请添加图片描述

UKF与SSUKF

UKF是:无迹卡尔滤波
SSUKF是:简化超球面无迹卡尔曼滤波

UKF

相较于传统的KF算法,UKF能够更好地处理非线性系统,并且具有更高的估计精度。它适用于多种应用场景,如机器人定位导航、目标跟踪、信号处理等。

SSUKF

简化超球面无迹卡尔曼滤波是一种用于状态估计的滤波算法。它是卡尔曼滤波(Kalman Filter)的一种扩展形式,用于解决非线性系统的状态估计问题。
SUKF的核心思想是将非线性函数通过高斯-埃尔米特(Gaussian-Hermite)积分转化成线性函数,并利用无迹变换(Unscented Transform)对状态空间进行采样。在SUKF中,状态空间被视为超球面,每个采样点代表一个可能的状态。通过对超球面上的采样点进行加权求和,可以得到状态的估计值和协方差矩阵。
简化超球面无迹卡尔曼滤波是一种常用的状态估计算法,但在实际应用中需要根据具体问题进行参数调整和优化,以获得最佳的估计结果。

例程

例程从PSINS的UKF153和SSUKF153的程序修改而来,如果有工具箱(版本不限),即可直接运行这个对比的例程。
例程将两者算法放在一个m文件里面,重新设计了一个带有加速、减速、爬升、下降、转弯的三维运动轨迹,让两个程序在同样的轨迹和相同的传感器误差特性下运行,结果可靠。
在工具箱原有的单个算法

运行结果

运行结果分成两部分:

  • 第一部分是借用了工具箱的绘图命令,绘制的UKF误差曲线,此部分误差是 真值 − U K F 真值-UKF 真值UKF 真值 − S S U K F 真值-SSUKF 真值SSUKF
  • 第二部分是我自己原创的三维轨迹图和三轴位置误差对比曲线
    UKF误差曲线:
    请添加图片描述
    SSUKF误差曲线:
    请添加图片描述
    三维轨迹图:
    请添加图片描述
    三轴的误差对比图:
    请添加图片描述

源代码

代码如下:【需要PSINS工具箱】

% 【PSINS】UKF/SSUKF对比
% 无迹卡尔滤波和简化超球面无迹卡尔曼滤波
% 使用三维轨迹(加速、减速、旋转、爬升下降)
% 对比滤波后的姿态、速度、位置
% Evand
% 2024-7-25/Ver1
clear;clc;close all;
glvs
psinstypedef('test_SINS_GPS_UKF_153_def');
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);
%% 初始化
% 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);
%% UKF
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,:) = []; 

fprintf('完整代码下载链接:https://gf.bilibili.com/item/detail/1105881012');
%% SSUKF filter
ins = insinit(avpadderr(trj.avp0,davp0), ts);

网站公告

今日签到

点亮在社区的每一天
去签到