基于【PSINS工具箱】,提供一个MATLAB例程,仅以速度为观测量的SINS/GNSS组合导航(滤波方式为UKF)
文章目录
- 工具箱
- 程序简述
- 运行结果
- 代码
- 程序讲解
- MATLAB 代码教程:使用UKF进行速度观测
- 1. 引言与基本设置
- 2. 初始设置
- 3. UKF滤波的初始化
- 4. 主循环进行UKF处理
- 5. 绘图与结果分析
- 6. 输出误差统计
- 总结
工具箱
本程序需要在安装工具箱后使用,工具箱是开源的,链接:http://www.psins.org.cn/kydm
程序简述
原有例程的
153
153
153组合导航是
S
I
N
S
SINS
SINS/
G
P
S
GPS
GPS下的位置观测或位置+速度观测,本文所述的代码是仅三轴位置观测的,使用UKF来滤波。
最后输出速度对比、速度误差、姿态对比、姿态误差、位置对比、位置误差等图片。如下:
运行结果
-
三轴AVP曲线:
-
三轴速度误差曲线:
-
滤波后 X X X轴速度累积概率分布函数:
代码
部分代码如下:
% 【PSINS】速度观测的153,UKF
% 2024-09-17/Ver1
% 如需付费咨询,可联系我
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数据来提高定位精度。