当前位置:网站首页>PSINS中19维组合导航模块sinsgps详解(初始赋值部分)
PSINS中19维组合导航模块sinsgps详解(初始赋值部分)
2022-07-06 19:13:00 【python小白(下阶段小黑)】
初始赋值部分
1.[nn, ts, nts] = nnts(2, diff(imu(1:2,end)));
用于设置nn
子样数、ts
采样间隔以及前面两者相乘nts
2.if size(gps,2)<=5, gpspos_only=1; pos0=gps(1,1:3)'; else, gpspos_only=0; pos0=gps(1,4:6)'; end
判断传入的GPS数据是否有速度结果,只有位置的话,gpspos_only=1
,还有速度的话,gpspos_only=0
3.
if ~exist('rk', 'var'),
if gpspos_only==1, rk=poserrset([10,30]);
else, rk=vperrset([0.1;0.3],[10,30]); end
end
上述代码块是用于给测量值赋初始方差的,只有位置观测值的话,只需要位置误差;还有速度观测值的话,那速度位置的方差都要设置;注意这里,位置误差的值输入的是米,速度误差的值单位是米每秒
4.
if ~exist('dT', 'var'), dT = 0.01; end; if length(dT)==1, dT(2,1)=1; end
if ~exist('lever', 'var'), lever = rep3(1); else, lever=rep3(lever); end; if length(lever)==3, lever(4)=1; end
if ~exist('imuerr', 'var'), imuerr = imuerrset(0.05, 500, 0.001, [10;10;100]); end
if ~exist('davp', 'var'), davp = avperrset([10;300], 1, [10;30]); end
if ~exist('ins', 'var'), ins=100; end
if ~isstruct(ins) % sinsgps(imu, gps, T); T=ins align time
[~, res0] = aligni0(imu(1:fix(ins/ts),:), pos0); imu(1:fix(ins/ts),:)=[];
ins = insinit([res0.attk(1,1:3)'; 0;0;0; pos0], ts); ins.nts=nts;
end
上述代码块是为没有提供的变量赋初值的:dT
默认(0.01,1)s; lever
默认[1;1;1]m;其他的如默认设置;
最重要的是ins
默认为100,是用于后面判断是否要进行ins
的初始化的
5. ins.lever = lever(1:3); ins.tDelay = dT(1);
设置杆臂和时间延迟参数
6. ins = inslever(ins, -ins.lever); ins.vn = ins.vnL; ins.pos = ins.posL;
对速度和位置进行杆臂误差的补偿
7. psinstypedef(196-gpspos_only*3);
为组合导航维度类型做准备的
8.
kf = [];
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1); 0])^2;
上述代码块为系统噪声方差阵的构建。imuerr.web为角度随机游走;imuerr.wdb为速度随机游走; imuerr.sqg为角速率随机游走相关偏差;imuerr.sqa为比力随机游走相关偏差。
9.kf.Rk = diag(rk)^2;
为设置测量噪声矩阵。
10. kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever(1:3)*lever(4); dT(1)*dT(2)]*1.0)^2;
为设置状态方差矩阵
11. kf.Hk = zeros(length(rk),19);
为设置系数矩阵
12. kf = kfinit0(kf, nts);
为卡尔曼滤波器初始化
13.
if exist('Pmin', 'var'),
if sum(Pmin)<=0, kf.pconstrain=0;
else kf.Pmin = Pmin; kf.pconstrain = 1; end
end
kf.adaptive = 1;
if exist('Rmin', 'var'),
if sum(Rmin)<=0, kf.adaptive=0; end
if kf.adaptive==1,
if length(Rmin)==1, kf.Rmin = kf.Rk*Rmin;
else kf.Rmin = diag(Rmin); end
end
end
上述代码为设置限制状态方差下限以及自适应滤波的测量噪声矩阵下限设置
14.
if exist('fbstr', 'var'), kf.fbstr=fbstr; end
kf.xtau = [ [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; 1]*1;
上述代码为设置相关参数
时间同步初始化部分
imugpssyn(imu(:,end), gps(:,end));
时间同步部分会在下一篇详细介绍
边栏推荐
- wzoi 1~200
- Gee upgrade can realize one piece of run tasks
- MySQL - common functions - string functions
- The third season of ape table school is about to launch, opening a new vision for developers under the wave of going to sea
- 数论 --- 快速幂、快速幂求逆元
- What management points should be paid attention to when implementing MES management system
- QT common Concepts-1
- 【Node学习笔记】chokidar模块实现文件监听
- 基于ensp防火墙双击热备二层网络规划与设计
- 安全巡检的工作
猜你喜欢
Statistics of radar data in nuscenes data set
Huitong programming introductory course - 2A breakthrough
你不可不知道的Selenium 8种元素定位方法,简单且实用
Use of fiddler
Read fast RCNN in one article
Django database (SQLite) basic introductory tutorial
C#/VB.NET 删除Word文檔中的水印
HAVE FUN | “飞船计划”活动最新进展
leetcode:736. LISP syntax parsing [flowery + stack + status enumaotu + slots]
Classify the features of pictures with full connection +softmax
随机推荐
[leetcode]Search for a Range
Real project, realized by wechat applet opening code (end)
【Socket】①Socket技术概述
Examples of how to use dates in Oracle
导数、偏导数、方向导数
AWS learning notes (I)
哈希表及完整注释
[node learning notes] the chokidar module realizes file monitoring
QT常见概念-1
Code line breaking problem of untiy text box
Pgadmin4 of PostgreSQL graphical interface tool
Difference and the difference between array and array structure and linked list
一文读懂Faster RCNN
[Mori city] random talk on GIS data (II)
Qpushbutton- "function refinement"
4 -- Xintang nuc980 mount initramfs NFS file system
C # / vb. Net supprime le filigrane d'un document word
Redis入门完整教程:客户端案例分析
Use of pgpool II and pgpooladmin
The 8 element positioning methods of selenium that you have to know are simple and practical