PSINS工具箱，MATLAB例程，仅以速度为观测量的SINS/GNSS组合导航（滤波方式为EKF）

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

文章目录

工具箱
程序简述
- 运行结果
代码
程序讲解
- [MATLAB 代码讲解：速度观测的 EKF 实现](#MATLAB 代码讲解：速度观测的 EKF 实现)
总结

工具箱

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

程序简述

原有例程的 153 153 153组合导航是 S I N S SINS SINS/ G P S GPS GPS下的位置观测或位置+速度观测，本文所述的代码是仅三轴位置观测的，使用EKF来滤波。

最后输出速度对比、速度误差、姿态对比、姿态误差、位置对比、位置误差等图片。如下：

运行结果

三轴AVP曲线：
三轴速度误差曲线：
滤波后 X X X轴速度累积概率分布函数：

代码

部分代码如下：

matlab 复制代码

% 【PSINS】速度观测的153，EKF
% Evand©2024
% 2024-4-2/Ver1
% 2024-6-3/Ver2：优化绘图标注
% 2024-11-05/Ver3：优化绘图标注、更新联系方式

% 清空工作空间，清除命令窗口，关闭所有图形窗口
clear; clc; close all;

rng(0); % 设置随机数种子为0，以确保结果可重复
glvs % 调用全局变量设置
psinstypedef(153); % 设置PSINS类型

% 读取轨迹文件
trj = trjfile('trj10ms.mat');

% 初始设置
[nn, ts, nts] = nnts(2, trj.ts); % 获取时间序列的参数
% imuerr = imuerrset(0.03, 100, 0.001, 5); % （注释掉的）设置IMU误差参数
imuerr = imuerrset(8, 14, 0.18, 57); % 设置IMU误差参数
imu = imuadderr(trj.imu, imuerr);  % 对IMU数据添加误差
% imuplot(imu); % （注释掉的）绘制IMU数据

% 设置初始姿态误差
davp0 = avperrset([1; 1; 10]*60, 0.1, [1; 1; 3]);

%% 速度观测EKF

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

程序讲解

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

MATLAB 代码讲解：速度观测的 EKF 实现

这段 MATLAB 代码实现了使用扩展卡尔曼滤波器（EKF）进行速度观测的功能，目的是通过融合惯性导航系统（INS）和GNSS数据来提高定位精度。以下是对代码的详细讲解。

代码结构与功能

引言与准备工作：
- 代码开头部分包含了作者信息和版本控制，清晰地标明了代码的目的和更新记录。
- 通过 clear; clc; close all; 清理工作环境，确保运行时不会受到之前运行的影响。
随机数种子的设置：
- rng(0); 用于设置随机数生成器的种子，以确保每次运行代码时结果的一致性。
全局变量和PSINS类型设置：
- glvs 函数调用全局变量设置，通常用于初始化一些全局参数。
- psinstypedef(153); 定义了特定类型的PSINS（组合导航系统），为后续的导航计算做好准备。
数据读取：
- trj = trjfile('trj10ms.mat'); 读取包含IMU和GNSS数据的轨迹文件。
初始设置：
- [nn, ts, nts] = nnts(2, trj.ts); 获取时间序列的参数，nn 是时间步长，ts 是时间序列。
- imuerr = imuerrset(8, 14, 0.18, 57); 设置IMU的误差参数，这些数值影响后续的滤波精度。
- imu = imuadderr(trj.imu, imuerr); 向IMU数据添加误差，用于模拟真实环境中的噪声。
姿态误差设置：
- davp0 = avperrset([1; 1; 10]*60, 0.1, [1; 1; 3]); 初始化姿态误差参数，为后续的INS初始化提供基线。

EKF滤波过程

INS初始化：
- ins = insinit(avpadderr(trj.avp0, davp0), ts); 初始化INS的状态，包括位置、速度和姿态。
卡尔曼滤波器初始化：
- kf = kfinit(ins, davp0, imuerr, rk); 初始化卡尔曼滤波器，设置状态、误差模型和观测噪声。
- 之后设置了滤波器的过程噪声和观测噪声的协方差矩阵。
主循环：
- 代码使用循环遍历IMU数据，进行状态更新和滤波处理：
  - wvm = imu(k:k1, 1:6); 从IMU数据中提取当前时间窗口的观测值。
  - ins = insupdate(ins, wvm); 更新INS状态。
  - kf = kfupdate(kf); 更新卡尔曼滤波器状态。
  - 每到整秒，提取真实速度并加上噪声，更新滤波器并保存估计结果。

结果处理与可视化

清理数据：
- avp_EKF_vel(ki:end, :) = []; 和 xkpk(ki:end, :) = []; 用于去除多余的预分配行，确保数据整洁。
结果绘图：
- 使用 avpcmpplot 函数绘制估计结果与真实值的比较。
- 分别绘制X、Y、Z轴速度误差，帮助分析滤波效果。
- 最后，绘制速度误差的累积概率分布函数（CDF），提供对滤波后速度估计准确性的统计分析。

总结

该代码展示了如何通过EKF方法结合IMU和GNSS数据实现高精度的速度观测。通过设置合理的初始参数和噪声模型，代码能够有效地更新状态并优化定位精度。通过可视化结果，用户可以直观地了解滤波效果，为后续的研究和应用提供数据支持。这种方法在自动驾驶、无人机导航等领域具有广泛的应用前景。

如需付费咨询，可联系我