当前位置:网站首页>Detailed explanation of 19 dimensional integrated navigation module sinsgps in psins (initial assignment part)
Detailed explanation of 19 dimensional integrated navigation module sinsgps in psins (initial assignment part)
2022-07-07 02:48:00 【Python Xiaobai (Xiaohei in the next stage)】
19 Dimensional integrated navigation module sinsgps
Initial assignment part
1.[nn, ts, nts] = nnts(2, diff(imu(1:2,end)));
Used for setting up nn
Number of samples 、ts
The sampling interval and the previous two are multiplied 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
Judge the incoming GPS Whether the data has speed results , Only position ,gpspos_only=1
, And speed ,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
The above code block is used to assign the initial variance to the measured value , If there are only position observations , Only position error is needed ; If there are velocity observations , The variance of speed position should be set ; Note that there , The value of position error is entered in meters , The unit of value of speed error is meters per second
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
The above code block is to assign initial values to variables that are not provided :dT
Default (0.01,1)s; lever
Default [1;1;1]m; Others, such as default settings ;
most important of all ins
The default is 100, It is used to judge whether to carry out ins
Initialization of
5. ins.lever = lever(1:3); ins.tDelay = dT(1);
Set the lever arm and time delay parameters
6. ins = inslever(ins, -ins.lever); ins.vn = ins.vnL; ins.pos = ins.posL;
Compensation of lever arm error for speed and position
7. psinstypedef(196-gpspos_only*3);
Preparing for the integrated navigation dimension type
8.
kf = [];
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1); 0])^2;
The above code block is the construction of system noise variance matrix .imuerr.web Random walk for angle ;imuerr.wdb Random walk for speed ; imuerr.sqg Is the angular rate random walk correlation deviation ;imuerr.sqa Is the correlation deviation of specific random walk .
9.kf.Rk = diag(rk)^2;
To set the measurement noise matrix .
10. kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever(1:3)*lever(4); dT(1)*dT(2)]*1.0)^2;
To set the state variance matrix
11. kf.Hk = zeros(length(rk),19);
Set the coefficient matrix for
12. kf = kfinit0(kf, nts);
Initialize the Kalman filter
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
The above code is to set the lower limit of the limit state variance and the lower limit of the measurement noise matrix of the adaptive filter
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;
The above code is to set relevant parameters
Time synchronization initialization part
imugpssyn(imu(:,end), gps(:,end));
Time synchronization will be introduced in detail in the next article
边栏推荐
- KYSL 海康摄像头 8247 h9 isapi测试
- Wireshark installation
- 6-6漏洞利用-SSH安全防御
- Unity custom webgl packaging template
- widerperson数据集转化为YOLO格式
- 一文读懂Faster RCNN
- Oracle中日期的使用方法实例
- What management points should be paid attention to when implementing MES management system
- What are the applications and benefits of MES management system
- leetcode:736. LISP syntax parsing [flowery + stack + status enumaotu + slots]
猜你喜欢
Niuke programming problem -- double pointer of 101 must be brushed
【Socket】①Socket技术概述
Google Earth Engine(GEE)——Landsat 全球土地调查 1975年数据集
Redis入门完整教程:客户端案例分析
The 8 element positioning methods of selenium that you have to know are simple and practical
wireshark安装
Increase 900w+ playback in 1 month! Summarize 2 new trends of top flow qiafan in station B
导数、偏导数、方向导数
Douban average 9 x. Five God books in the distributed field!
Fundamentals of process management
随机推荐
Difference and the difference between array and array structure and linked list
安德鲁斯—-多媒体编程
记一次JAP查询导致OOM的问题分析
Redis入門完整教程:問題定比特與優化
Dotconnect for DB2 Data Provider
慧通编程入门课程 - 2A闯关
牛客编程题--必刷101之双指针篇
Mmdetection3d loads millimeter wave radar data
Derivative, partial derivative, directional derivative
[leetcode]Search for a Range
The third season of ape table school is about to launch, opening a new vision for developers under the wave of going to sea
Untiy文本框的代码换行问题
What are the applications and benefits of MES management system
Redis入门完整教程:RDB持久化
Use of fiddler
Redis入门完整教程:客户端案例分析
AWS learning notes (I)
Hash table and full comments
Compress JS code with terser
MySQL