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;

数据存储

·······································································································
理解不足之处，还请赐教！
·······································································································