PSINS中19维组合导航模块sinsgps详解(初始赋值部分)

初始赋值部分

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;

上述代码为设置相关参数

时间同步初始化部分

15. imugpssyn(imu(:,end), gps(:,end));时间同步部分会在下一篇详细介绍