kf = kfupdate(kf)
if mod(t,1)<nts
gps = [vn0; pos0] + rk.*randn(6,1); % GPS速度位置仿真
kf = kfupdate(kf, [vn;pos]-gps, 'M');
vn(3) = vn(3)- kf.Xk(6); Kt.XK(6) = O % 反馈
end
avp(kk,:) = [qq2phi(qnb,qnb0); vn; pos; t]';
xkpk(kk,:) = [kf.Xk; diag(kf.Pk); t]; kk = kk+1;
if mod(t,100)<nts disp(fix(t)); end % 显示进度
end
avp(kk:end,:) = []; xkpk(kk:end,:) = []: tt = avp(:,end);
% 状态真值与估计效果对比佟
mysubplot(321, tt, [avp(:,1:2),xkpk(:,1:2)]/arcmin, '\phi_E,\phi_N /\prime');
mysubplot(322, tt, [avp(:,3),xkpk(:,3)]/arcmin, '\phi_U /\prime');
mysubplot(323, tt, [avp(:,4:6),xkpk(:,4:6)], '\deltav /n /m/s');
mysubplot(324,t,ideltapos(avp(:7:9)),[xkpk(:,7),xkpk(:,8).*cos(avp(:,7))]*Re,xkpk(:,9)],\DeltaP
m');
mysubplot(325, tt, xkpk(:,10:12)/dph, '\epsilon /\circ/h');
mysubplot(326, t, xkpk(:,13:15)/ug, '\nabla / ug');
% 均方差收敛佟
pk = sqrt(xkpk(:,16:end-1))
mysubplot(321, tt, pk(:,1:2)/arcmin, '\phi_E,\phi_N /\prime');
mysubplot(322, tt, pk(:,3)/arcmin, '\phi_U /\prime'):
mysubplot(323, tt, pk(:,4:6), '\deltav in / m/s');
mysubplot(324, t, [[pk(:,7),pk(:,8)*cos(avp(1,7))]*Re,pk(:,9)], \DeltaP /m');
mysubplot(325, tt, pk(:,10:12)/dph, '\epsilon /\circ/h');
mysubplot(326, tt, pk(:,13:15)/ua, "\nabla / ua'):