【逐行解析】PSINS工具箱中的UKF组合导航的代码解析（test_SINS_GPS_UKF_153）

详解工具箱的UKF153代码，最后给出运行结果的解读和代码修改思路

工具箱

本程序需要在安装工具箱后使用，工具箱是开源的，链接：http://www.psins.org.cn/kydm

程序简述

程序实现基于UKF（无迹卡尔曼滤波）的SINS（捷联惯性导航系统）与GPS集成导航仿真。
顺序如下：
通过循环迭代，从IMU数据中提取加速度和角速度。$\to$更新INS状态，进行一步预测。$\to$如果检测到GPS信号（时间戳能被1整除），则进行GPS位置模拟并更新卡尔曼滤波器。$\to$记录当前的AVP估计值和相关协方差。

函数详解

% 源代码：SINS/GPS intergrated navigation simulation using UKF.
% 下载自www.psins.org.cn网站
% 中文注释来自matlabfilter（闲鱼、VX同号）
% 2024-09-17/Ver1

glvs %工具箱统一初始化，将必备的变量加入工作区
psinstypedef('test_SINS_GPS_UKF_153_def'); %UKF153的初始化
trj = trjfile('trj10ms.mat'); %导入导航的数据
% initial settings
[nn, ts, nts] = nnts(2, trj.ts); %设定采样值
imuerr = imuerrset(0.03, 100, 0.001, 5); %IMU的误差设定
imu = imuadderr(trj.imu, imuerr);  % 给IMU理想值加上误差
davp0 = avperrset([1;1;10]*60, 0.1, [1;1;3]); %设置初始的AVP误差
ins = insinit(avpadderr(trj.avp0,davp0), ts); %初始化INS
% UKF 滤波部分
rk = poserrset([1;1;3]); %设置GPS的三轴误差（1，1,3分别代表纬度精度高度方向1m，1m，3m的误差）
kf = kfinit(ins, davp0, imuerr, rk); %卡尔曼滤波的初始化
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '15-state SINS/GPS UKF Simulation.'); 
ki = 1;
for k=1:nn:len-nn+1
    k1 = k+nn-1;  
    wvm = imu(k:k1,1:6);  t = imu(k1,end); %从imu里面提取出来加速度、角速度和时间戳
    ins = insupdate(ins, wvm); %INS更新（一步预测的迭代）
    kf.px = ins;
    kf = ukf(kf); %更新kf的相关系数
    if mod(t,1)==0 %检测t是否能被1整除（如果能整除，表示此刻有GPS信号）
        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滤波函数
        [kf, ins] = kffeedback(kf, ins, 1, 'avp'); %状态校正
        avp(ki,:) = [ins.avp', t]; %将当前时刻的滤波值赋给avp矩阵
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;
    end
    timebar;
end
avp(ki:end,:) = [];  xkpk(ki:end,:) = []; 
% show results
insplot(avp); %绘制滤波得到的avp值
avperr = avpcmpplot(trj.avp, avp); %计算滤波值的误差（并绘制误差图）
kfplot(xkpk, avperr, imuerr); %绘制误差的状态和方差

运行结果解读

运行得到avp图像：

误差曲线：

状态协方差曲线：

修改思路

% 绘制三维的轨迹（真值和滤波后的值）
figure;
plot3(trj.avp(:,8),trj.avp(:,7),trj.avp(:,9)); %真值
hold on
plot3(avp(:,8),avp(:,7),avp(:,9)); %滤波后的值
legend('真值轨迹','滤波后的轨迹')
xlabel('经度');ylabel('纬度');zlabel('高度');

% 计算经度误差值（只是一个示例，可以更改为纬度和高度）
fprintf('经度误差的平均值为%d\n',mean(avperr(:,8)));

修改后的结果