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

news2025/6/8 17:05:39

请添加图片描述

UKF与SSUKF

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

UKF

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

SSUKF

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

例程

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

运行结果

运行结果分成两部分：

第一部分是借用了工具箱的绘图命令，绘制的UKF误差曲线，此部分误差是 $真值 - U K F$ 或 $真值 - SS U K F$
第二部分是我自己原创的三维轨迹图和三轴位置误差对比曲线
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);