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'):
时间: 2023-08-22 14:04:39 浏览: 99
NTS4001NT1G-VB一种N沟道SC70-3封装MOS管
这段代码是对KF滤波器进行更新和状态估计的部分。具体的步骤和功能如下:
1. `kf = kfupdate(kf)`:调用kfupdate函数对KF滤波器进行更新,根据当前的测量值和噪声参数来更新滤波器的状态。
2. `if mod(t,1)<nts`:如果当前时间t与采样时间nts的模小于1,即达到了一个新的采样周期。
3. `gps = [vn0; pos0] + rk.*randn(6,1)`:根据预设的GPS测量噪声参数rk,生成一个随机的GPS测量值。
4. `kf = kfupdate(kf, [vn;pos]-gps, 'M')`:调用kfupdate函数对KF滤波器进行再次更新,根据GPS测量值与真实值之间的差异来更新滤波器的状态。
5. `vn(3) = vn(3)- kf.Xk(6)`:根据KF滤波器的估计结果,对速度进行修正。
6. `Kt.XK(6) = O`:将KF滤波器估计结果中的第6个状态量设为0。
7. `avp(kk,:) = [qq2phi(qnb,qnb0); vn; pos; t]'`:将姿态、速度、位置和时间存储在avp矩阵中。
8. `xkpk(kk,:) = [kf.Xk; diag(kf.Pk); t]`:将KF滤波器的状态量、协方差矩阵的对角线和时间存储在xkpk矩阵中。
9. `if mod(t,100)<nts disp(fix(t)); end`:每过100个时间单位,显示当前进度。
以上是对代码的解释,该部分主要完成KF滤波器的更新和状态估计,并将结果存储在相应的矩阵中。后面的代码是对状态估计结果进行可视化和均方差收敛的分析。具体的绘图和计算部分需要进一步了解代码的上下文和具体实现。
阅读全文