PSINS工具箱text_SINS_GPS_153.m
% SINS/GPS intergrated navigation simulation using kalman filter.基于卡尔曼滤波的SINS / GPS组合导航仿真。
% Please run 'test_SINS_trj.m' to generate 'trj10ms.mat' beforehand!请预先运行' test_SINS_trj.m '生成' trj10ms.mat ' !
% See also test_SINS_trj, test_SINS, test_SINS_GPS_186, test_SINS_GPS_193.
glvs %全局变量
psinstypedef(153); %全局变量,主要设置状态转移矩阵的维数、量测矩阵的维数和画图
trj = trjfile('trj10ms.mat'); %加载轨迹数据，请参考trjfile函数解读
% initial settings初始设置
[nn, ts, nts] = nnts(2, trj.ts); %子样数和采样间隔
imuerr = imuerrset(0.03, 100, 0.001, 5); %imu误差，分别为陀螺常值零偏、加速度计常值零偏、角度随机游走和速度随机游走
imu = imuadderr(trj.imu, imuerr); %对参考imu数据添加误差生成实际imu测量值
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; %方差阵上下限约束条件，1：表示约束
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1); %对变量预先分配内存
timebar(nn, len, '15-state SINS/GPS Simulation.'); %程序运行的进度条
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); %卡尔曼滤波器预测更新
if mod(t,1)==0 %判断是否有GNSS位置量测，此处认为GNSS为整秒量测，mod为求余数函数
posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1); %模拟GNSS位置量测值 % GPS pos simulation with some white noise
kf = kfupdate(kf, ins.pos-posGPS, 'M'); %卡尔曼滤波器量测更新
[kf, ins] = kffeedback(kf, ins, 1, 'avp'); %卡尔曼滤波状态反馈
avp(ki,:) = [ins.avp', t]; %姿态、速度、位置和时间
xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'; ki = ki+1; %反馈后的剩余状态、方差阵和时间
end
timebar; %进度条的计数器
end
avp(ki:end,:) = []; xkpk(ki:end,:) = []; %清除多余的内存
% show results
insplot(avp); %绘制姿态、速度和位置图
avperr = avpcmpplot(trj.avp, avp); %组合导航计算的avp与参考avp的差值
kfplot(xkpk, avperr, imuerr); %绘制avp误差、imu误差、反馈后的剩余状态和方差阵
自用，水平较低，小白刚入门，写的代码注释，也参考了网友和西工大严恭敏老师PPT，可能有错误，如有错误请指出谢谢
感谢西工大严恭敏老师为大家提供的免费工具箱
PSINS工具箱text_SINS_GPS_153.m相关推荐
- PSINS工具箱学习（一）下载安装初始化、SINS-GPS组合导航仿真、习惯约定与常用变量符号、数据导入转换、绘图显示
文章目录 一.前言 二.相关资源 三.下载安装初始化 1.下载PSINSyymmdd.rar工具箱文件 2.解压文件 3.初始化 4.启动工具箱导览 四.习惯约定与常用变量符号 1.PSINS全局变量 ...
- PSINS工具箱 编写C语言版应用
目标: 基于PSINS工具箱中的惯性系粗对准,编写C语言程序. 声明: 程序文件基于严恭敏老师PSINS开源工具箱中相应算法由个人编写完成,程序能够正常执行,可用于算法验证及相关实验,但并不具备应用于 ...
- PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-初始化设置
文章目录 test_SINS_GPS_153源码 主函数 psinstypedef imuerrset imuadderr avperrset avpadderr insinit 流程 test_SI ...
- 基于PSINS工具箱的卡尔曼滤波与SINS/GNSS组合导航
文章目录 卡尔曼滤波与SINS/GNSS组合导航 典型的SINS/GNSS组合滤波 POS处理 卡尔曼滤波与SINS/GNSS组合导航 用于Kalman滤波的函数有: psinstypedef(nnm ...
- SINS/GNSS组合导航仿真应用详细版(基于PSINS工具箱 )
文章目录 轨迹仿真 生成轨迹数据 绘制仿真数据 惯导仿真 纯惯导仿真 SINS/GPS组合导航 总结 1 2 3 4 5 轨迹仿真 生成轨迹数据 首先,打开demos\test_trj.m文件,运行仿 ...
- PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-卡尔曼滤波设置+导航解算
文章目录 test_SINS_GPS_153源码 poserrset kfinit gabias kfupdate 流程 test_SINS_GPS_153源码 poserrset function ...
- 严恭敏老师PSINS工具箱解读——test_SINS_GPS_153
感谢严老师的无私奉献 test_SINS_GPS_153--15维状态和位置量测的SINS/GPS松组合 glvs--'全局变量,请参考glvf函数解读' psinstypedef(153);--'全 ...
- 严恭敏PSINS工具箱航位推算DR解读
DR中使用陀螺仪记录的角增量计算姿态,利用里程计代替加速度计计算位置增量,由于加速度计输出的比力进行积分时,误差会对结果造成较大的影响,利用里程计能够更准确的得到位置增量信息.里程计是一个与载体固连的 ...
- 关于PSINS运动轨迹仿真模块的理解和思考
文章目录 坐标系定义 飞行轨迹设计 1. 飞行轨迹类型 2. 相关代码 (1) test_SINS_trj.m (2) trjsegement.m (3) trjsimu.m 疑问与思考 本篇博客主要 ...
最新文章
- 计算 webView 显示内容后实际高度
- np.asarray和np.array、np.nanmean和np.mean、np.diff、
- == ===区别，利用!= !==判空。
- 视觉直观感受7种常用排序算法
- 方案一TCP 完成聊天室的编写
- 苏大微型计算机原理与应用题库,苏州大学计算机原理及应用考研复习题.pdf
- Sublime中查找重复行的正则表达式
- （96）分频器设计（任意分频器）
- 苹果修复 Gatekeeper 绕过漏洞
- 7-2 组织星期信息 (10 分)
- 集成Android SlidingMenu（SlideMenu）
- rs232接口_串口，COM口，TTL，RS232，RS485，UART的区别详解
- 如何利用福禄克OFP2-100-Q CH测试光纤的熔接损耗
- SharePoint 模拟审批Nintex工作流
- 什么是语法糖（syntactic suger）
- OBD数据过滤之：变速箱的里程能不能改
- vue电商实战-黑马-01
- win10笔记本合盖无法睡眠（风扇依然在转动）(开盖后不是锁屏状态)
- 第6章第1节：基本设计理论：幻灯片设计过程中的七个常见技巧 [PowerPoint精美幻灯片实战教程]
- 这些年，我与Google不得不说的那些事儿