详解工具箱的UKF153代码，最后给出运行结果的解读和代码修改思路
文章目录
- 工具箱
- 程序简述
- 函数详解
- 运行结果解读
- 修改思路
- 修改后的结果
工具箱
本程序需要在安装工具箱后使用，工具箱是开源的，链接：http://www.psins.org.cn/kydm
程序简述
程序实现基于UKF（无迹卡尔曼滤波）的SINS（捷联惯性导航系统）与GPS集成导航仿真。
顺序如下：
通过循环迭代，从IMU数据中提取加速度和角速度。 → \rightarrow →更新INS状态，进行一步预测。 → \rightarrow →如果检测到GPS信号（时间戳能被1整除），则进行GPS位置模拟并更新卡尔曼滤波器。 → \rightarrow →记录当前的AVP估计值和相关协方差。
函数详解
% 源代码：SINS/GPS intergrated navigation simulation using UKF.
% 下载自www.psins.org.cn网站
% 中文注释来自matlabfilter（闲鱼、VX同号）
% 2024-09-17/Ver1glvs %工具箱统一初始化，将必备的变量加入工作区
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+1k1 = 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 noisekf = 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;endtimebar;
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)));
修改后的结果
