感谢严老师的无私奉献

test_SINS_GPS_153——15维状态和位置量测的SINS/GPS松组合

glvs——'全局变量,请参考glvf函数解读'
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误差，分别为陀螺常值零偏、加速度计常值零偏、角度随机游走和速度随机游走，请参考imuerrset函数解读'
imu = imuadderr(trj.imu, imuerr);——'对参考imu数据添加误差生成实际imu测量值，请参考imuadderr解读'
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]);——'初始姿态、速度和位置误差，注意姿态的单位为：分，请参考avperrset函数解读'
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+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)==0——'判断是否有GNSS位置量测，此处认为GNSS为整秒量测，mod为求余数函数'posGPS = trj.avp(k1,7:9)’ + davp0(7:9).*randn(3,1);——'模拟GNSS位置量测值'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;endtimebar;——'进度条的计数器'
endavp(ki:end,:) = [];  xkpk(ki:end,:) = [];——'清除多余的内存'
% show results
insplot(avp);——'画姿态、速度和位置'
avperr = avpcmpplot(trj.avp, avp);——'组合导航计算的avp与参考avp的差值'
kfplot(xkpk, avperr, imuerr);——'画avp误差、imu误差、反馈后的剩余状态和方差阵'

‘glvf’函数解读；‘trjfile’函数解读；‘imuerrset’函数解读；‘imuadderr’函数解读；‘avperrset’函数解读

严恭敏老师PSINS工具箱解读——test_SINS_GPS_153相关推荐

PSINS工具箱学习（一）下载安装初始化、SINS-GPS组合导航仿真、习惯约定与常用变量符号、数据导入转换、绘图显示
文章目录一.前言二.相关资源三.下载安装初始化 1.下载PSINSyymmdd.rar工具箱文件 2.解压文件 3.初始化 4.启动工具箱导览四.习惯约定与常用变量符号 1.PSINS全局变量 ...
PSINS工具箱编写C语言版应用
目标: 基于PSINS工具箱中的惯性系粗对准,编写C语言程序. 声明: 程序文件基于严恭敏老师PSINS开源工具箱中相应算法由个人编写完成,程序能够正常执行,可用于算法验证及相关实验,但并不具备应用于 ...
PSINS工具箱text_SINS_GPS_153.m
% SINS/GPS intergrated navigation simulation using kalman filter.基于卡尔曼滤波的SINS / GPS组合导航仿真. % Please ...
Bailian2855 计算概论（刘志敏老师班）ACM测试题1【椭圆】
2855:计算概论(刘志敏老师班)ACM测试题1 总时间限制: 1000ms 内存限制: 65536kB 描述给定一个圆心在原点的椭圆,长轴跟短轴分别在x轴和y轴上,长度分别为8,6,写一个程序,判 ...
严恭敏 matlab,惯性仪器测试与数据分析 [严恭敏编] 2012年版
惯性仪器测试与数据分析作者:严恭敏编出版时间:2012年版内容简介 <惯性仪器测试与数据分析>比较系统和全面地介绍了陀螺仪.加速度计和惯导系统的测试原理以及典型的数据分析方法.全书 ...
阅读笔记-数据结构（C语言）-严蔚敏老师-单链表
这里创建单链表都是指定长度进行创建的,之后会补上动态创建. 单链表一.头插法创建单链表(p30) 二.尾插法创建单链表三.按索引查找和按值查找四.插入到第几个节点之后五.删除节点一.头插法创 ...
杨冬敏老师:老板懂点财务知识，看懂三张报表
杨冬敏老师:老板懂点财务知识,看懂三张报表很多老板问说开公司老板一定要懂财务相关的知识.如果想要把一家企业经营好,必须要懂得财务的相关知识.如果不懂财务知识的老板,在月末,你问财务说这个月我们公司赚 ...
严恭敏PSINS工具箱航位推算DR解读
DR中使用陀螺仪记录的角增量计算姿态,利用里程计代替加速度计计算位置增量,由于加速度计输出的比力进行积分时,误差会对结果造成较大的影响,利用里程计能够更准确的得到位置增量信息.里程计是一个与载体固连的 ...
PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-初始化设置
文章目录 test_SINS_GPS_153源码主函数 psinstypedef imuerrset imuadderr avperrset avpadderr insinit 流程 test_SI ...

严恭敏老师PSINS工具箱解读——test_SINS_GPS_153

感谢严老师的无私奉献

test_SINS_GPS_153——15维状态和位置量测的SINS/GPS松组合

严恭敏老师PSINS工具箱解读——test_SINS_GPS_153相关推荐

最新文章

热门文章