Detailed explanation of 19 dimensional integrated navigation module sinsgps in psins (filtering part)

Partial filtering

 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);

The above code first updates the inertial navigation algorithm
2.kf.Phikk_1 = kffk(ins); To create the state transition matrix of Kalman filter
3.kf = kfupdate(kf); Time update of Kalman filter
4. [kgps, dt] = imugpssyn(k, k1, 'F'); It's calculation imu and gps The corresponding time difference dt, and gps Number of rows of data kgps
5. measflag = 0; Initialization of measurement update method identification
6. ins = inslever(ins); For lever arm compensation
7.

if kgps>0
     dtpos=+vn2dpos(ins.eth,ins.vnL,ins.tDelay);

The above code block is represented as , Calculation ins.tDelay Position increment in time
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

The above code is based on gps The dimension of observation provided , Design the observed value of Kalman filter zk And coefficient matrix Hk
9. kf = kfupdate(kf, zk, 'M'); For the measurement update of Kalman filter
10. 10.zkrk(kiz,:) = [zk; diag(kf.Rk); t]; kiz = kiz+1; data storage
11. [kf, ins] = kffeedback(kf, ins, nts); Feedback correction of Kalman filter
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;

data storage

·······································································································
Understanding deficiencies , Please give me some advice ！
·······································································································