【基于PSINS】UKF/SSUKF对比的MATLAB程序

请添加图片描述

UKF与SSUKF

UKF是:无迹卡尔滤波
SSUKF是:简化超球面无迹卡尔曼滤波

UKF

相较于传统的KF算法,UKF能够更好地处理非线性系统,并且具有更高的估计精度。它适用于多种应用场景,如机器人定位导航、目标跟踪、信号处理等。

SSUKF

简化超球面无迹卡尔曼滤波是一种用于状态估计的滤波算法。它是卡尔曼滤波(Kalman Filter)的一种扩展形式,用于解决非线性系统的状态估计问题。
SUKF的核心思想是将非线性函数通过高斯-埃尔米特(Gaussian-Hermite)积分转化成线性函数,并利用无迹变换(Unscented Transform)对状态空间进行采样。在SUKF中,状态空间被视为超球面,每个采样点代表一个可能的状态。通过对超球面上的采样点进行加权求和,可以得到状态的估计值和协方差矩阵。
简化超球面无迹卡尔曼滤波是一种常用的状态估计算法,但在实际应用中需要根据具体问题进行参数调整和优化,以获得最佳的估计结果。

例程

例程从PSINS的UKF153和SSUKF153的程序修改而来,如果有工具箱(版本不限),即可直接运行这个对比的例程。
例程将两者算法放在一个m文件里面,重新设计了一个带有加速、减速、爬升、下降、转弯的三维运动轨迹,让两个程序在同样的轨迹和相同的传感器误差特性下运行,结果可靠。
在工具箱原有的单个算法

运行结果

运行结果分成两部分:

  • 第一部分是借用了工具箱的绘图命令,绘制的UKF误差曲线,此部分误差是 真值 − U K F 真值-UKF 真值UKF 真值 − S S U K F 真值-SSUKF 真值SSUKF
  • 第二部分是我自己原创的三维轨迹图和三轴位置误差对比曲线
    UKF误差曲线:
    请添加图片描述
    SSUKF误差曲线:
    请添加图片描述
    三维轨迹图:
    请添加图片描述
    三轴的误差对比图:
    请添加图片描述

源代码

代码如下:【需要PSINS工具箱】

% 【PSINS】UKF/SSUKF对比
% 无迹卡尔滤波和简化超球面无迹卡尔曼滤波
% 使用三维轨迹(加速、减速、旋转、爬升下降)
% 对比滤波后的姿态、速度、位置
% Evand
% 2024-7-25/Ver1
clear;clc;close all;
glvs
psinstypedef('test_SINS_GPS_UKF_153_def');
ts = 0.1;       % sampling interval
%% 轨迹设置
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % init avp
% trajectory segment setting
traj_ = [];
seg = trjsegment(traj_, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   10, traj_, 1);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'climb',        10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'descent',      10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'deaccelerate', 5,  traj_, 2); %2
seg = trjsegment(seg, 'uniform',      100);
% generate, save & plot
trj = trjsimu(avp0, seg.wat, ts, 1);
%% 初始化
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
imuerr = imuerrset(0.03, 100, 0.001, 5);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]);
ins = insinit(avpadderr(trj.avp0,davp0), ts);
%% UKF
rk = poserrset([1;1;3]);
kf = kfinit(ins, davp0, imuerr, rk);
kf.fx = @largephiu15ukf;
len = length(imu); [avp_ukf, 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+1k1 = k+nn-1;  wvm = imu(k:k1,1:6);  t = imu(k1,end);ins = insupdate(ins, wvm);kf.px = ins;kf = ukf(kf);if mod(t,1)==0posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);  % GPS pos simulation with some white noisekf = ukf(kf, ins.pos-posGPS, 'M');  % UKF filter[kf, ins] = kffeedback(kf, ins, 1, 'avp');avp_ukf(ki,:) = [ins.avp', t];xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;endtimebar;
end
avp_ukf(ki:end,:) = [];  xkpk(ki:end,:) = []; fprintf('完整代码下载链接:https://gf.bilibili.com/item/detail/1105881012');
%% SSUKF filter
ins = insinit(avpadderr(trj.avp0,davp0), ts);

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/876898.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Git入门与实战:版本控制的艺术

🍁 作者:知识浅谈,CSDN签约讲师,CSDN博客专家,华为云云享专家,阿里云专家博主 📌 擅长领域:全栈工程师、爬虫、ACM算法 🔥 微信:zsqtcyw 联系我领取学习资料 …

【人工智能】深度剖析:Midjourney与Stable Diffusion的全面对比

文章目录 🍊1 如何选择合适的AI绘画工具1.1 个人需求选择1.2 比较工具特点1.3 社区和资源 🍊2 Midjourney VS Stable Diffusion:深度对比与剖析 2.1 使用费用对比 2.2 使用便捷性与系统兼容性对比 2.3 开源与闭源对比 2.4 图片质量对比 2.5 上…

19145 最长无重复子数组

这个问题可以使用滑动窗口的方法来解决。我们可以使用两个指针,一个指向子数组的开始,一个指向子数组的结束。然后我们使用一个哈希表来记录每个元素最后出现的位置。当我们遇到一个已经在子数组中出现过的元素时,我们就将开始指针移动到这个…

Python面试题:结合Python技术,如何使用NetworkX进行复杂网络分析

NetworkX 是一个强大的 Python 库,用于创建、操作和研究复杂网络的结构、动力学和功能。它提供了丰富的功能来处理图和网络数据,适合用于复杂网络分析。以下是使用 NetworkX 进行复杂网络分析的基本步骤: 安装 NetworkX: pip inst…

Mac文件拷贝到移动硬盘怎么做Mac拷贝之后Win电脑里看不到

在日常使用mac电脑的过程中,我们经常需要将一些重要的文件备份到外部硬盘上,以防止数据丢失或电脑故障。传输文件到硬盘可以通过多种方法实现,比如拖拽或者复制至移动硬盘,但有时也会遇到移动硬盘无法粘贴,或拷贝后无法…

SSRF (服务端请求伪造)

🎼个人主页:金灰 😎作者简介:一名简单的大一学生;易编橙终身成长社群的嘉宾.✨ 专注网络空间安全服务,期待与您的交流分享~ 感谢您的点赞、关注、评论、收藏、是对我最大的认可和支持!❤️ 🍊易编橙终身成长社群&#…

Pycharm + Pyside6

1. 使用 Qt designer 创建 UI 文件 2. 使用 UIC 工具生成 ui_.py 文件 3. 自定义类导入ui.py 文件的窗口类 4.自定义窗口继承UI窗体类 5. self.setupUi(self) from PySide6.QtWidgets import QApplication, QWidget, QComboBox, QVBoxLayout from ui_test import Ui_Formc…

图像生成中图像质量评估指标—PSNR的详细介绍

文章目录 1. 背景介绍2. 实际应用3. 总结和讨论 1. 背景介绍 峰值信噪比(Peak Signal-to-Noise Ratio,简称PSNR)是一种广泛应用于图像和视频处理领域的客观图像质量评价指标。它主要用于衡量图像的噪声水平和图像质量,可以用来评…

Python酷库之旅-第三方库Pandas(051)

目录 一、用法精讲 186、pandas.Series.is_monotonic_increasing属性 186-1、语法 186-2、参数 186-3、功能 186-4、返回值 186-5、说明 186-6、用法 186-6-1、数据准备 186-6-2、代码示例 186-6-3、结果输出 187、pandas.Series.is_monotonic_decreasing属性 187…

CSS基础入门 —— 打造网页的华丽外衣

在Web开发的广阔世界里,CSS(Cascading Style Sheets,层叠样式表)是不可或缺的一部分。它如同网页的化妆师和服装设计师,负责为HTML(HyperText Markup Language,超文本标记语言)构建的…

嵌入式人工智能(34-基于树莓派4B的红外传感器、紫外传感器、激光传感器)

这三种光传感器都是不可见光传感器,光是由电场和磁场交替传播而形成的波动现象。光是一种电磁辐射,属于电磁波的一种。下图是电磁波的频谱范围,生活中多数光是看不到的,但是确真实存在,本文介绍几种光传感器&#xff0…

C++从入门到起飞之——友元内部类匿名对象对象拷贝时的编译器优化 全方位剖析!

🌈个人主页:秋风起,再归来~🔥系列专栏:C从入门到起飞 🔖克心守己,律己则安 目录 1、友元 2、内部类 3、 匿名对象 4、对象拷⻉时的编译器优化 5、完结散花 1、友元 • 友元提供…

基于爬虫和机器学习的招聘数据分析与可视化系统,python django框架,前端bootstrap,机器学习有八种带有可视化大屏和后台

在现代招聘领域,数据驱动的决策已成为提升招聘效率和质量的关键因素。基于爬虫技术和机器学习算法,结合Django框架和Bootstrap前端技术,我们开发了一套完整的招聘数据分析与可视化系统。该系统旨在帮助企业从海量招聘信息中提取有价值的数据&…

学习Numpy的奇思妙想

学习Numpy的奇思妙想 本文主要想记录一下,学习 numpy 过程中的偶然的灵感,并记录一下知识框架。 推荐资源:https://numpy.org/doc/stable/user/absolute_beginners.html 💡灵感 为什么 numpy 数组的 shape 和 pytorch 是 tensor 是…

等保2.0测评 — 容器测评对象选取

之前有小伙伴提问到,关于容器到底要测评哪些内容,也就是测评对象的选取。 首先要区分的是容器与容器集群这两个概念。容器集群概念可参考该篇文章。 不使用容器扩展要求情况 当仅使用容器技术时,采用安全通用要求,无需使用容器…

昇思25天学习打卡营第15天|探索 Diffusion 扩散模型:从构建到应用的全过程

目录 环境配置 构建Diffusion模型 位置向量 ResNet/ConvNeXT块 Attention模块 组规一化 条件U-Net 正向扩散 数据准备与处理 训练过程 推理过程 环境配置 首先进行环境配置、库的导入和一些设置操作,具体代码如下: %%capture captured_output …

土体的有效应力原理

土体的有效应力原理 有效应力原则1. 总应力的测定2. 孔隙水压力的测定3. 有效应力的确定 有效应力的重要性 土体中的有效应力原理是卡尔太沙基在1936年提出的重要理论之一。它是总应力和孔隙水压力之间的差值。下面简要说明土壤中有效应力的更多特征和测定。 有效应力原则 有…

python中kerea库的使用方法

Keras是一个高层神经网络API,用于简化构建和训练深度学习模型的过程。它能够在TensorFlow、Theano和CNTK之上运行。Keras的主要目标是使深度学习更容易上手,并加速实验进程。以下是关于Keras库的详细介绍及其使用方法。 安装Keras 在开始使用Keras之前&…

基于城市感知的公共交通多源异构数据融合的应用研究申请书

基于城市感知的公共交通多源异构数据融合的应用研究 研究意义 基于城市感知的公共交通多源异构数据融合的应用研究具有广泛的研究意义, 提升交通效率:利用多源数据,可以为公共交通管理提供更为全面和准确的信息,从而优化线路设…

pytorch-openpose代码笔记

1.cv2.imread(filename, flags) 参数: filepath:读入imge的完整路径 flags:标志位,{cv2.IMREAD_COLOR,cv2.IMREAD_GRAYSCALE,cv2.IMREAD_UNCHANGED} cv2.IMREAD_COLOR:默认参数,读入…