当前位置: 首页 > article >正文

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

在这里插入图片描述

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

文章目录

  • 工具箱
  • 程序简述
    • 运行结果
  • 代码
  • 程序讲解
    • MATLAB 代码讲解:速度观测的 EKF 实现
      • 代码结构与功能
      • 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轴速度累积概率分布函数:
    请添加图片描述

代码

部分代码如下:

% 【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数据来提高定位精度。以下是对代码的详细讲解。

代码结构与功能

  1. 引言与准备工作

    • 代码开头部分包含了作者信息和版本控制,清晰地标明了代码的目的和更新记录。
    • 通过 clear; clc; close all; 清理工作环境,确保运行时不会受到之前运行的影响。
  2. 随机数种子的设置

    • rng(0); 用于设置随机数生成器的种子,以确保每次运行代码时结果的一致性。
  3. 全局变量和PSINS类型设置

    • glvs 函数调用全局变量设置,通常用于初始化一些全局参数。
    • psinstypedef(153); 定义了特定类型的PSINS(组合导航系统),为后续的导航计算做好准备。
  4. 数据读取

    • trj = trjfile('trj10ms.mat'); 读取包含IMU和GNSS数据的轨迹文件。
  5. 初始设置

    • [nn, ts, nts] = nnts(2, trj.ts); 获取时间序列的参数,nn 是时间步长,ts 是时间序列。
    • imuerr = imuerrset(8, 14, 0.18, 57); 设置IMU的误差参数,这些数值影响后续的滤波精度。
    • imu = imuadderr(trj.imu, imuerr); 向IMU数据添加误差,用于模拟真实环境中的噪声。
  6. 姿态误差设置

    • davp0 = avperrset([1; 1; 10]*60, 0.1, [1; 1; 3]); 初始化姿态误差参数,为后续的INS初始化提供基线。

EKF滤波过程

  1. INS初始化

    • ins = insinit(avpadderr(trj.avp0, davp0), ts); 初始化INS的状态,包括位置、速度和姿态。
  2. 卡尔曼滤波器初始化

    • kf = kfinit(ins, davp0, imuerr, rk); 初始化卡尔曼滤波器,设置状态、误差模型和观测噪声。
    • 之后设置了滤波器的过程噪声和观测噪声的协方差矩阵。
  3. 主循环

    • 代码使用循环遍历IMU数据,进行状态更新和滤波处理:
      • wvm = imu(k:k1, 1:6); 从IMU数据中提取当前时间窗口的观测值。
      • ins = insupdate(ins, wvm); 更新INS状态。
      • kf = kfupdate(kf); 更新卡尔曼滤波器状态。
      • 每到整秒,提取真实速度并加上噪声,更新滤波器并保存估计结果。

结果处理与可视化

  1. 清理数据

    • avp_EKF_vel(ki:end, :) = [];xkpk(ki:end, :) = []; 用于去除多余的预分配行,确保数据整洁。
  2. 结果绘图

    • 使用 avpcmpplot 函数绘制估计结果与真实值的比较。
    • 分别绘制X、Y、Z轴速度误差,帮助分析滤波效果。
    • 最后,绘制速度误差的累积概率分布函数(CDF),提供对滤波后速度估计准确性的统计分析。

总结

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


http://www.kler.cn/a/388483.html

相关文章:

  • 安卓录屏软件添加隐藏按钮
  • Mybatis 关联查询
  • 12月9日IO
  • React 组件中 State 的定义、使用及正确更新方式
  • Spire.doc 合并word,复制word
  • React废弃componentWillMount和componentWillReceiveProps这两个生命周期方法
  • jmeter常用配置元件介绍总结之分布式压测
  • Python | Leetcode Python题解之第557题反转字符串中的单词III
  • 团结引擎中直接出鸿蒙包hap app
  • 2024 年(第 7 届)“泰迪杯”数据分析技能赛B 题 特殊医学用途配方食品数据分析 完整代码 结果 可视化分享
  • Windows 结合 Docker 下使用 Django+Celery+Pool
  • [翻译]ANSI X9.24-3-2017
  • AI 刷题实践选题:精选真题功能的深度剖析与学习实践| 豆包MarsCode AI刷题
  • “双十一”电商狂欢进行时,在AI的加持下看网易云信IM、RTC如何助力商家!
  • UE5.4 PCG 执行后生成函数
  • Word大珩助手:超大数字怎么读?35位数字?69位数字?
  • 嵌入式学习-网络高级-Day01
  • 【青牛科技】应用方案 | D75xx-150mA三端稳压器
  • 如何在 HFSS 3D 布局中创建 cutout 子设计
  • goroutine 介绍
  • 我爸瘫痪,我们三兄弟每月出2500让嫂子伺候,得知我工资10000,嫂子打电话:你多给1000行吗?...
  • lua脚本调用 c/c++中的接口
  • Spring Shell——快速构建终端应用,自定义终端命令
  • Maven的依赖管理、传递、冲突、父子工程的继承和聚合
  • 题目讲解15 合并两个排序的链表
  • 易语言加载dll模拟windows鼠标轨迹移动