首页 > 其他分享 >基于【PSINS工具箱】提供一个MATLAB例程,仅以速度为观测量的SINS/GNSS组合导航

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

时间:2024-11-06 09:46:38浏览次数:6  
标签:imuerr CKF kf 例程 SINS 观测 GNSS 速度

在这里插入图片描述

基于【PSINS工具箱】,提供一个MATLAB例程,仅以速度为观测量的SINS/GNSS组合导航(滤波方式可选EKF/UKF/CKF),无需下载,订阅专栏后可直接复制

文章目录

工具箱

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

程序简述

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

运行结果

  • 三轴AVP曲线:
    在这里插入图片描述

  • 三轴速度误差曲线:
    在这里插入图片描述

  • 滤波后 X X X轴速度累积概率分布函数:
    在这里插入图片描述

程序源代码

代码如下:

% 【PSINS】速度观测的153,EKF
% 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];
kf = kfinit(ins, davp0, imuerr, rk);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;
kf.Rk = diag(rk)^2;
kf.Pxk = diag([davp0; imuerr.eb; imuerr.db]*1.0)^2;
kf.Hk = [zeros(3,3), eye(3), zeros(3,9)]; %修改观测方程
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1;
len = length(imu); [avp_EKF1, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '15-state SINS/GPS UKF 观测量为速度.'); 
ki = 1;
for k=1:nn:len-nn+1
    k1 = k+nn-1;  
    wvm = imu(k:k1,1:6);  t = imu(k1,end);
    ins = insupdate(ins, wvm);
    kf.Phikk_1 = kffk(ins);
    kf = kfupdate(kf);

% 完整代码详见:https://gf.bilibili.com/item/detail/1106602012
% 可选EKF、UKF、CKF三种不同的滤波方法

程序讲解

这段 MATLAB 代码实现了基于 CKF(Cubic Kalman Filter,容积卡尔曼滤波)的速度观测处理,主要用于模拟和分析惯性导航系统(INS)与全球导航卫星系统(GNSS)结合的情况。以下是对代码的详细介绍:

代码功能概述

  1. 背景与应用

    • 本代码旨在通过 CKF 方法优化速度观测,使用 GNSS 数据来提高惯性导航系统的定位精度。CKF 是一种改进的卡尔曼滤波器,适用于处理非线性系统的状态估计问题。
  2. 输入数据

    • 代码从名为 trj10ms.mat 的文件中读取轨迹数据,该数据包含了 IMU(惯性测量单元)和 GNSS 的观测信息。
    • 初始的 IMU 数据通过 imuadderr 函数添加了噪声,模拟现实环境中传感器的误差。

代码结构与关键步骤

  1. 环境初始化

    • 使用 clear; clc; close all; 清除工作空间、命令窗口和关闭所有图形窗口,以确保代码运行时环境的干净。
    • 使用 rng(0); 设置随机数生成器的种子,以便于结果的可重复性。
  2. 参数设置

    • 通过 nnts 函数提取时间序列参数,确定数据处理的步长和时间间隔。
    • 使用 imuerrset 函数初始化 IMU 的误差参数,定义加速度计和陀螺仪的误差特性。
    • davp0 设置了初始的姿态误差,这对后续的 INS 初始化至关重要。
  3. INS 初始化

    • 通过 insinit 函数结合姿态误差和时间序列,初始化惯性导航系统的状态。
  4. CKF滤波器设置

    • 初始化 CKF 的相关参数,包括观测噪声权重和状态协方差矩阵。
    • 使用 prealloc 函数为结果数据预分配内存,优化性能。
  5. 主循环

    • 使用一个循环遍历所有的 IMU 数据,进行状态更新和滤波处理:
      • 从 IMU 数据中提取当前时间窗口的观测值,调用 insupdate 更新 INS 状态。
      • 计算当前状态的 CKF 更新,并将更新后的状态与 GNSS 数据进行融合。
      • 每当时间达到整数秒时,生成带噪声的 GNSS 速度观测,并使用 CKF 更新滤波器状态。
      • 保存每次迭代的估计结果和状态协方差,用于后续分析。
  6. 结果处理与绘图

    • 使用 avpcmpplot 函数比较真实的 GNSS 姿态与 CKF 估计的速度。
    • 绘制速度误差,展示在 X、Y、Z 轴的速度估计与真实值之间的差异。
    • 最后,绘制速度误差的累积概率分布函数(CDF),直观展示滤波结果的统计特性。

结论

这段代码通过 CKF 方法有效地处理了 IMU 和 GNSS 的融合问题,展示了如何在动态环境中利用不同传感器的数据提高系统的定位精度。其结构清晰,便于理解和扩展,适合在实际应用中进行进一步的研究和优化。

标签:imuerr,CKF,kf,例程,SINS,观测,GNSS,速度
From: https://blog.csdn.net/callmeup/article/details/143502119

相关文章

  • 【MATLAB代码】三个CT模型的IMM例程,各CT旋转速率不同,适用于定位、导航、目标跟踪
    三个CT模型,各CT模型下的运动旋转速率不同,适用于定位、导航、目标跟踪文章目录代码构成运行结果源代码代码讲解概述代码结构1.初始化2.仿真参数设置3.生成量测数据4.IMM迭代5.绘图主要功能函数部分1.卡尔曼滤波函数2.模型综合函数3.模型概率更新函数总结......
  • 【正点原子K210连载】第四十九章 综合例程摘自【正点原子】DNK210使用指南-CanMV版指
    第四十九章综合例程本章,简单介绍一下DNK210开发板的CanMV综合例程。本章分为如下几个小节:49.1综合例程运行说明49.2综合例程展示49.1综合例程运行说明本综合例程旨在演示利用CanMV在DNK210开发板上实现的各种功能,包括板载LED控制、板载蜂鸣器控制、相机、相册、音乐播放器、......
  • IC-GVINS: A Robust, Real-Time, INS-Centric GNSS-Visual-Inertial Navigation Syste
    IC-GVINS:ARobust,Real-Time,INS-CentricGNSS-Visual-InertialNavigationSystemIC-GVINS:一种稳健的实时以惯性导航系统为中心的全球导航卫星系统视觉惯性导航系统XiaojiNiu${}^{\circledR}$,HailiangTang${}^{\circledR}$,TishengZhang${}^{\circledR......
  • 【matlab代码】3个模型的IMM例程(匀速、左转、右转),附源代码(可复制粘贴)
    文章目录3个模型的IMM源代码运行结果代码介绍总结3个模型的IMM代码实现了基于IMMIMMIMM(InteractingMultipleModel)算法的目标跟踪。它使用三种不同的运动模型(匀速直线运动、左转弯和右转弯)来预测目标的位置,并通过卡尔曼滤波进行状......
  • 【基于PSINS工具箱的程序设计】专栏目录,持续更新……
    文章目录重要的文章后续更新方向专栏介绍在进行上层设计时,借助工具箱完成底层的复杂计算是很有必要的。也能利用工具箱学习惯导解算。本专栏分享若干自己基于PSINS编写的滤波、导航程序。如果没有工具箱,自己网上面下载或私信我,我给你啊-......
  • 【MATLAB】无迹粒子滤波(Unscented Particle Filter)例程
    文章目录运行结果位置曲线和速度曲线位置误差曲线和速度误差曲线源代码代码结构源代码目的作者信息代码结构与功能详细说明修改建议总结运行结果位置曲线和速度曲线位置误差曲线和速度误差曲线源代码代码结构......
  • C语言 队列例程
    队列的数据成员为结构体或数组#include<stdio.h>#include<stdlib.h>#include<assert.h>typedefstructnodeArr{ intdat[2]; structnodeArr*next;}NodeArr;NodeArr*initQueueAr();intis_emeptyAr(NodeArr*Q);voidenQueueAr(NodeArr*Q,int*arr);......
  • 带I2C的LCD12864使用经验总结和例程(RSCG12864B)
    搬运说明:本文是本人在24年4月CSDN上发布的,现全文搬运至博客园,图片水印暂未去除,并删除原CSDN博文。本文简要说明了带I2C协议的LCD12864屏幕的使用经验与教训,特别是厂家说明书说法有误或未提及的内容。文末也提供一个本人编写的STC89C52单片机读取SHT30温湿度传感器并显示在LCD1286......
  • gnss信号转发器 北斗卫星转发器 gps信号转发器
    在现代社会的工作中,我们都会使用到卫星信号。但存在一个现象,那就是卫星信号不能够穿透建筑物,生产车间、实验室等室内环境下的测试工作就会遇到困难。SYN2309型GNSS信号转发器是由西安同步电子科技有限公司精心设计、自行研发生产的一款增益可调的GNSS全频段卫星信号转发系统,主要功......
  • ATGM331C-5T杭州中科微BDS/GNSS全星座定位授时模块种类及特点
    ATGM331C-5T系列模块通过UART作为主要输出通道,按照NMEA0183的协议格式输出。产品选型:性能指标:出色的定位导航功能,支持BDS/GPS卫星导航系统的单系统授时,以及任意组合的多系统联合定位,并支持QZSS和SBAS系统。支持A-GNSS冷启动捕获灵敏度:-148dBm跟踪灵敏度:-16......