当前位置:网站首页>PSINS中19维组合导航模块sinsgps详解(滤波部分)
PSINS中19维组合导航模块sinsgps详解(滤波部分)
2022-07-06 19:13:00 【python小白(下阶段小黑)】
PSINS中19维组合导航模块sinsgps详解
滤波部分
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);
上述代码先进行的是惯导算法更新
2.kf.Phikk_1 = kffk(ins);
为创建卡尔曼滤波的状态转移矩阵
3.kf = kfupdate(kf);
卡尔曼滤波的时间更新
4. [kgps, dt] = imugpssyn(k, k1, 'F');
是计算imu
和gps
对应的时间差值dt
,和gps
数据所在的行数kgps
5. measflag = 0;
量测更新方法标识的初始化
6. ins = inslever(ins);
为进行杆臂补偿
7.
if kgps>0
dtpos=+vn2dpos(ins.eth,ins.vnL,ins.tDelay);
上述代码块表示为,计算ins.tDelay
时间内的位置增量
8.
if gpspos_only==1
measflag = 2;
zk = ins.posL+dtpos-gps(kgps,1:3)';
kf.Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb,-ins.Mpvvn];
else
measflag = 3;
zk = [ins.vnL+ins.tDelay*ins.anbar;ins.posL+dtpos]-gps(kgps,1:6)';
kf.Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW,-ins.anbar;-ins.MpvCnb,-ins.Mpvvn]];
end
上述代码为根据gps
提供的观测量的维度,设计卡尔曼滤波的观测值zk
和系数矩阵Hk
9. kf = kfupdate(kf, zk, 'M');
为进行卡尔曼滤波的量测更新
10. 10.zkrk(kiz,:) = [zk; diag(kf.Rk); t]; kiz = kiz+1;
数据存储
11. [kf, ins] = kffeedback(kf, ins, nts);
卡尔曼滤波的反馈校正
12.
avp(ki,:) = [ins.att; ins.vnL; ins.posL; ins.eb; ins.db; t]';
xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';
sk(ki,:) = [measflag, t]; ki = ki+1;
数据存储
·······································································································
理解不足之处,还请赐教!
·······································································································
边栏推荐
猜你喜欢
电气工程及其自动化
Planning and design of double click hot standby layer 2 network based on ENSP firewall
Huitong programming introductory course - 2A breakthrough
[Mori city] random talk on GIS data (II)
从零安装Redis
3 -- Xintang nuc980 kernel supports JFFS2, JFFS2 file system production, kernel mount JFFS2, uboot network port settings, and uboot supports TFTP
Django database (SQLite) basic introductory tutorial
Increase 900w+ playback in 1 month! Summarize 2 new trends of top flow qiafan in station B
What management points should be paid attention to when implementing MES management system
普通测试年薪15w,测试开发年薪30w+,二者差距在哪?
随机推荐
Use of fiddler
安全巡检的工作
MySQL - common functions - string functions
Increase 900w+ playback in 1 month! Summarize 2 new trends of top flow qiafan in station B
测试优惠券要怎么写测试用例?
[software test] the most complete interview questions and answers. I'm familiar with the full text. If I don't win the offer, I'll lose
[leetcode]Search for a Range
Redis入门完整教程:复制原理
Planning and design of double click hot standby layer 2 network based on ENSP firewall
3 -- Xintang nuc980 kernel supports JFFS2, JFFS2 file system production, kernel mount JFFS2, uboot network port settings, and uboot supports TFTP
差异与阵列和阵列结构和链表的区别
Real project, realized by wechat applet opening code (end)
Safety delivery engineer
Matlb| economic scheduling with energy storage, opportunity constraints and robust optimization
Fundamentals of process management
[Mori city] random talk on GIS data (II)
普通测试年薪15w,测试开发年薪30w+,二者差距在哪?
6-6漏洞利用-SSH安全防御
慧通编程入门课程 - 2A闯关
Convert widerperson dataset to Yolo format