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;
数据存储
·······································································································
理解不足之处,还请赐教!
·······································································································