【基于PSINS工具箱】以速度为观测量的SINS/GNSS组合导航，UKF滤波

基于【PSINS工具箱】，提供一个MATLAB例程，仅以速度为观测量的SINS/GNSS组合导航（滤波方式为UKF）

工具箱

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

程序简述

原有例程的$153$组合导航是$SINS$/$GPS$下的位置观测或位置+速度观测，本文所述的代码是仅三轴位置观测的，使用UKF来滤波。
最后输出速度对比、速度误差、姿态对比、姿态误差、位置对比、位置误差等图片。如下：

运行结果

• 三轴AVP曲线：
  

• 三轴速度误差曲线：
  

• 滤波后$X$轴速度累积概率分布函数：
  

代码

部分代码如下：

% 【PSINS】速度观测的153，UKF
% 2024-09-17/Ver1
% 作者微信 ：matlabfilter（除前期达成一致外，咨询需付费）
clear;clc;close all;
rng(0);
glvs
psinstypedef(153);
trj = trjfile('trj10ms.mat');
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
% imuerr = imuerrset(0.03, 100, 0.001, 5);
imuerr = imuerrset(8, 14, 0.18, 57);
imu = imuadderr(trj.imu, imuerr);  % imuplot(imu);
davp0 = avperrset([1;1;10]*60, 0.1, [1;1;3]);%% 速度观测EKF
ins = insinit(avpadderr(trj.avp0,davp0), ts);
rk = [1;1;1];

完整代码下载链接：https://gf.bilibili.com/item/detail/1106602012

程序讲解

这段 MATLAB 代码实现了基于 UKF（无迹卡尔曼滤波）的速度观测处理，主要用于模拟和分析惯性导航系统（INS）与全球导航卫星系统（GNSS）结合的情况。以下是对代码的详细介绍：

MATLAB 代码教程：使用UKF进行速度观测

本教程将介绍一段使用扩展卡尔曼滤波（UKF）进行速度观测的 MATLAB 代码。该代码旨在通过融合惯性导航系统（INS）和GNSS数据来提高定位精度。以下是对代码的逐步解析。

1. 引言与基本设置

• 环境清理：使用 clear、clc 和 close all 清空工作空间，确保每次运行时环境干净。
• 随机种子：设置随机数种子以确保可重复的结果。
• 全局变量：初始化必要的全局变量和PSINS类型。
• 数据读取：从指定文件中加载IMU和GNSS数据。

2. 初始设置

• 时间序列参数：提取时间序列的基本参数。
• IMU误差设置：定义IMU传感器的误差，并向IMU数据添加噪声。
• 姿态误差：设置初始姿态误差，为后续计算提供基线。

3. UKF滤波的初始化

• INS初始化：创建INS的初始状态，包括位置、速度和姿态。
• UKF初始化：设置UKF的噪声模型、状态协方差矩阵和观测方程。

4. 主循环进行UKF处理

• 数据处理：循环遍历IMU数据，逐步更新INS状态和UKF滤波器。
• 速度观测：在每个整秒时，从真实数据中提取速度观测，并通过UKF进行状态更新。
• 结果保存：将每次的状态和协方差保存到预先分配的数组中。

5. 绘图与结果分析

• 结果可视化：绘制估计结果与真实数据的比较图，帮助分析滤波效果。
• 速度误差分析：展示X、Y和Z轴的速度误差。
• CDF绘制：绘制速度误差的累积概率分布函数，以便进一步分析估计精度。

6. 输出误差统计

• 误差统计：计算并输出X轴速度的平均误差，为评估滤波效果提供参考。

总结

通过本教程，可以了解如何使用UKF算法进行速度观测，以及如何通过融合IMU和GNSS数据来提高定位精度。