【PSINS工具箱】EKF与UKF滤波

描述

对工具箱SINS/GPS，153例程的修改，将EKF和UKF放在一个文件里面，一次运行可以得到两个滤波的结果。

片段

在这里插入图片描述

运行截图

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

程序完整源代码

在有工具箱的情况下，直接运行此代码，即可得到结果

% 基于PSINS工具箱的IMU数据生成与滤波
% date:2024-2-15
% Evand（evandworld@qq.com）
% Ver1
clear;clc;close all;
glvs
psinstypedef(153);
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);
% trjfile('trj10ms.mat', trj);
%% 初始化
% 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);
% KF filter
rk = poserrset([1;1;3]);
kf = kfinit(ins, davp0, imuerr, rk);
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1;
len = length(imu); [avp_ekf, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, 'KF'); 
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.Phikk_1 = kffk(ins);kf = kfupdate(kf);if mod(t,1)==0posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);  % GPS pos simulation with some white noisekf = kfupdate(kf, ins.pos-posGPS, 'M');[kf, ins] = kffeedback(kf, ins, 1, 'avp');avp_ekf(ki,:) = [ins.avp', t];xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;endtimebar;
end
avp_ekf(ki:end,:) = [];  xkpk(ki:end,:) = []; 
%% EKF show results
insplot(avp_ekf);
avperr = avpcmpplot(trj.avp, avp_ekf);
kfplot(xkpk, avperr, imuerr);%% UKF filter
glvs
% psinstypedef('test_SINS_GPS_UKF_153_def');
[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);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,:) = []; %% results
insplot(trj.avp);
imuplot(trj.imu);
figure;
plot3(trj.avp(:,7),trj.avp(:,8),trj.avp(:,9));
hold on
plot3(trj.avp(1,7),trj.avp(1,8),trj.avp(1,9),'*');
plot3(avp_ekf(:,7),avp_ekf(:,8),avp_ekf(:,9));
plot3(avp_ukf(:,7),avp_ukf(:,8),avp_ukf(:,9));
title('原创——3D轨迹图');
legend('真实值','起点','EKF滤波值','UKF滤波值');
% 误差绘图
% 误差曲线图与累积分布函数图，两种情况、三轴
figure;
subplot(3,2,1);
plot(1:10:len,avp_ekf(:,7)-trj.avp(1:10:len,7),1:10:len,avp_ukf(:,7)-trj.avp(1:10:len,7));
title('原创——X轴位置误差对比');legend('EKF滤波值','UKF滤波值');
subplot(3,2,3);
plot(1:10:len,avp_ekf(:,8)-trj.avp(1:10:len,8),1:10:len,avp_ukf(:,8)-trj.avp(1:10:len,8));
subplot(3,2,5);
plot(1:10:len,avp_ekf(:,9)-trj.avp(1:10:len,9),1:10:len,avp_ukf(:,9)-trj.avp(1:10:len,9));
subplot(3,2,2);
cdfplot(abs(avp_ekf(:,7)-trj.avp(1:10:len,7)));
hold on
cdfplot(abs(avp_ukf(:,7)-trj.avp(1:10:len,7)));
subplot(3,2,4);
cdfplot(abs(avp_ekf(:,8)-trj.avp(1:10:len,8)));
hold on
cdfplot(abs(avp_ukf(:,8)-trj.avp(1:10:len,8)));
subplot(3,2,6);
cdfplot(abs(avp_ekf(:,9)-trj.avp(1:10:len,9)));
hold on
cdfplot(abs(avp_ukf(:,9)-trj.avp(1:10:len,9)));
%% 误差输出
fprintf('ekf X轴位置误差最大值%d\n',max(abs(avp_ekf(:,7)-trj.avp(1:10:len,7))));
fprintf('ekf Y轴位置误差最大值%d\n',max(abs(avp_ekf(:,8)-trj.avp(1:10:len,8))));
fprintf('ekf Z轴位置误差最大值%d\n',max(abs(avp_ekf(:,9)-trj.avp(1:10:len,9))));
fprintf('ukf X轴位置误差最大值%d\n',max(abs(avp_ukf(:,7)-trj.avp(1:10:len,7))));

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

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

相关文章

腾讯云轻量2核2G3M云服务器优惠价格61元一年，限制200GB月流量

腾讯云轻量2核2G3M云服务器优惠价格61元一年，限制200GB月流量

腾讯云轻量2核2G3M云服务器优惠价格61元一年，配置为轻量2核2G、3M带宽、200GB月流量、40GB SSD盘，腾讯云优惠活动 yunfuwuqiba.com/go/txy 活动链接打开如下图： 腾讯云轻量2核2G云服务器优惠价格 腾讯云：轻量应用服务器100%CPU性能…
阅读更多...
pyqt 创建右键菜单栏

pyqt 创建右键菜单栏

class MainModule(QMainWindow, Ui_MainWindow):def __init__(self):super().__init__(parentNone)self.setupUi(self)# 允许出现菜单栏self.tableWidget.setContextMenuPolicy(Qt.CustomContextMenu)# 对空间添加右键菜单栏处理 self.tableWidget.customContextMenuRequested.…
阅读更多...
Java设计模式详解：单例模式

Java设计模式详解：单例模式

设计模式详解：单例模式 文章目录 设计模式详解：单例模式一、单例模式的原理二、单例模式的实现推荐1、饿汉模式2、静态内部类 三、单例模式的案例四、单例模式的使用场景推荐总结 一、单例模式的原理 单例模式听起来很高大上，但其实它的核心…
阅读更多...
速盾：cdn可以加速接口吗？

速盾：cdn可以加速接口吗？

随着互联网应用的不断发展，网站的访问速度成为了用户体验的重要指标之一。为了解决网站访问速度过慢的问题，CDN（内容分发网络）技术被广泛应用。CDN作为一种分布式的网络架构，可以将静态资源如图片、视频、JS、CSS等内容…
阅读更多...
Mac 版 IDEA 中配置 GitLab

Mac 版 IDEA 中配置 GitLab

一、安装Git 在mac终端输入Git检测指令&#xff0c;可以通过git命令查看Git是否安装过&#xff0c;如果没有则会弹出安装按钮&#xff0c;如果安装过则会输出如下信息。 WMBdeMacBook-Pro:~ WENBO$ git usage: git [--version] [--help] [-C <path>] [-c namevalue][--…
阅读更多...
6.5物联网RK3399项目开发实录-驱动开发之LCD显示屏使用（wulianjishu666）

6.5物联网RK3399项目开发实录-驱动开发之LCD显示屏使用（wulianjishu666）

90款行业常用传感器单片机程序及资料【stm32,stc89c52,arduino适用】 链接&#xff1a;https://pan.baidu.com/s/1M3u8lcznKuXfN8NRoLYtTA?pwdc53f LCD使用 简介 AIO-3399J开发板外置了两个LCD屏接口&#xff0c;一个是EDP&#xff0c;一个是LVDS&#xff0c;接口对应板…
阅读更多...
拿到运营商给的IP池

拿到运营商给的IP池

如果你想要从checkip.amazonaws.com获取IP地址&#xff0c;然后统计不同IP地址的个数&#xff08;去除空格&#xff09;&#xff0c;以下是一个使用requests库的示例代码。你可以在你自己的环境中运行这个示例&#xff1a; import requests from collections import Counter# …
阅读更多...
1.语言环境安装【go】

1.语言环境安装【go】

Go 语言支持以下系统： LinuxFreeBSDMac OS X（也称为 Darwin）Windows安装包下载地址为：All releases - The Go Programming Language。 如果打不开可以使用这个地址：All releases - The Go Programming Language。 UNIX/Linux/Mac OS X, 和 FreeBSD 安装 以下介绍了在UN…
阅读更多...
算法编程：计算斐波那契数列

算法编程：计算斐波那契数列

实现代码&#xff1a;C 实现方法&#xff1a;通过递推法、递归法、矩阵快速幂方法 适用&#xff1a; 范围小且单次查询时&#xff0c;可以不用记忆化处理。 范围大或多次查询时&#xff0c;应使用记忆化处理。 时间复杂度&#xff1a; 递归法&#xff1a;O(n^2)-->递推法(…
阅读更多...
Meme币如何赋能Web3社交？

Meme币如何赋能Web3社交？

Meme 币正在迎来它的高光时刻。然而究其本质&#xff0c;在过去四年中&#xff0c;我们在 Crytpo 世界中所见证的只不过是一些相同的理念在不同的新媒介中的实现。例如社交代币、NFT 或者 Meme 币。它们的共同特征都是让那些共享的想法、资产或信息的金融化成为可能。参与这些市…
阅读更多...
Conda 常用命令及Pycharm项目应用

Conda 常用命令及Pycharm项目应用

阅读更多...
【AI学习指南】十、PyTorch-简洁、高效、易用的深度学习框架和简单使用

【AI学习指南】十、PyTorch-简洁、高效、易用的深度学习框架和简单使用

目录 简介 为什么说PyTorch节省算力 简单使用 安装 创建模型实例
阅读更多...
【保姆级教程】YOLOv3图像目标检测：训练自己的数据集

【保姆级教程】YOLOv3图像目标检测：训练自己的数据集

一、YOLOv3图像目标检测原理 二、YOLOv3代码及预训练权重下载 2.1 下载yolov3代码 这里使用的是B站大佬Bubbliiiing复现的yolov3代码 仓库地址&#xff1a; https://github.com/bubbliiiing/yolo3-pytorch 2.2 下载模型预训练权重unet_resnet_medical.pth 链接&#xff1a…
阅读更多...
【博弈论3——二人博弈的纳什均衡】

【博弈论3——二人博弈的纳什均衡】

1.俾斯麦海之战 2. 零和博弈的定义 零和博弈&#xff08;Zero-Sum Game&#xff09;是一种博弈论的基本概念&#xff0c;指的是在博弈过程中&#xff0c;博弈参与者之间的收益和损失之和总是一个常数&#xff0c;特别是总和为零。即博弈一方的收益必然等于另一方的损失&#x…
阅读更多...
贪吃蛇：从零开始搭建一个完整的小游戏

贪吃蛇：从零开始搭建一个完整的小游戏

目录 导语&#xff1a; 一、游戏框架 二、蛇的实现 三、绘制游戏界面 四、食物 五、移动蛇 六.得分系统&#xff0c;是否吃到食物 七、检查碰撞 八、处理按键事件 九、得分系统 十、游戏状态管理 导语&#xff1a; 贪吃蛇这个经典的小游戏&#xff0c;我上学的时候就…
阅读更多...
QT记事本

QT记事本

QT记事本 1.概述 2.界面  2.1 界面布局  2.2 UI美化stylesheet   2.2.1 准备   2.2.2 stylesheet   2.2.3 效果 2.3 窗口大小调整与子控件自适应 3.信号与槽  3.1 简述  3.2 信号与槽设置   3.2.1 UI控件设置   3.2.2 UI转到槽&#xff08;自动连接&am…
阅读更多...
Jamba LLM模型：破解大型上下文窗口挑战的AI新星

Jamba LLM模型：破解大型上下文窗口挑战的AI新星

每周跟踪AI热点新闻动向和震撼发展 想要探索生成式人工智能的前沿进展吗&#xff1f;订阅我们的简报&#xff0c;深入解析最新的技术突破、实际应用案例和未来的趋势。与全球数同行一同&#xff0c;从行业内部的深度分析和实用指南中受益。不要错过这个机会&#xff0c;成为AI领…
阅读更多...
JBPM学习(三)：管理流程定义，java集合面试题

JBPM学习(三)：管理流程定义，java集合面试题

Test public void findAll() { // 查询 List list processEngine.getRepositoryService()// .createProcessDefinitionQuery()// .orderAsc(ProcessDefinitionQuery.PROPERTY_KEY)//排序条件 //.count()查询出总数量 //.page(0, 100)分页&#xff0c;取出前100条记录 …
阅读更多...
flask-（数据连接池的使用，定制命令，信号的使用，表关系的建立和查询）

flask-（数据连接池的使用，定制命令，信号的使用，表关系的建立和查询）

文章目录 连接池实例flask定制命令flask 缓存的使用flask信号的使用sqlalchemy原生操作sqlalchemy操作表flask orm操作表一对多的增加和跨表查询 &#xff08;一对一只需要关联字段加上 ,uniqueTrue&#xff09;多对多关系的增加和查询多对多基本的增删改查 连接池 import pymy…
阅读更多...
axios封装，请求取消和重试，请求头公共参数传递

axios封装，请求取消和重试，请求头公共参数传递

axios本身功能已经很强大了&#xff0c;封装也无需过度&#xff0c;只要能满足自己项目的需求即可。 常规axios封装&#xff0c;只需要设置&#xff1a; 实现请求拦截实现响应拦截常见错误信息处理请求头设置 import axios from axios;// 创建axios实例 const service axios…
阅读更多...
最新文章

Copyright @ 2022~2023