·348· 智能系统学报 第7卷 扩维系统状态变量为x=[xwv]T,则有 diag(Picut,R). Pg =E[(()]diag(P:,2,R) 式中：L'=n+m为扩维后系统状态维数. 利用均值和方差P,得到2L+1个Siga点： Y+e=[装+kAB] X=[联+√(L+A)Pg-√(L+A)P]. A=2t+√(D+A)Pk· 式中：L=n+q+m为扩维后的系统状态维数 B=+e-√(E+入)P+: 2)权值计算. 6)新的Sigma点权值计算. w%=A/(L+A), [Wom=A/(L'+'), W=/(L+A)+(1-a2+B). Wm）=1/[2(L+入')]，i=1,2,…,2L'. W）=w=1/[2(L+A)],i=1,2,…,2L. 式中：X'=a2(L'+)-L' 式中：入=a(L+k)-L. 7)经渐消矩阵修正后的量测更新。 3)时间更新过程. 忘.k+nk=h(.k+,y,k+),i=0,1,…,2L', Xi=fi,),i=0,1,,n, 2L 2L 名+=∑W回·专4， 主t=∑W·Xi4k, 0 2L' 2L Pn=含Wo(uu）· P4k=∑W时9X-玉4)Xk-五)， (专+E-之k+E)T， X'k+nk=fk,4sX),i=0,1,…,2n, 2L i=宫时0Xiu 2双o(5e-2u）·(5a4-iw) P=名°08a-ira-月 8)最小方差估计结果. 4)量测更新过程. =P片A()， 5,k+k=h(Xi+"kK,k）,i=0,1,…,2n, 王1=无4+K(Z41-k）, 2L Pxel Picus -KPK. 0 P生wAIt9 4实验与分析 o(Xa-Gd 为了验证AFM-UKF算法的性能，本文以SNS/ GPS组合导航系统作为平台，对提出的方法进行验 证.选取北、东、地坐标系作为导航坐标系，取姿态四 ∑WgO(Gkt-元kn)(g:+s-t)月 元数、速度、位置和陀螺零偏作为系统状态向量，建 立滤波器状态方程，选取位置、速度为系统观测向 当H+1不存在全零列时，根据式(5)~(7)、 量，建立滤波器量测方程，具体模型参考文献[11]. (12)(当H+1存在全零列时，通过式(5)~(6)、 分别采用UKF、自适应渐消因子的扩维UKF (8)~(10)、(12))计算得到自适应渐消矩阵Sk+1, (adaptive fading factor augmented UKF,AFF-AUKF) 进而通过式(13)修正系统一步预测的误差协方差 和AFM-AUKF作为滤波器对组合导航系统的姿态、 矩阵P+ 速度和位置信息进行估计.通过系统状态突变、系统 5)重新计算Sigma点. 噪声统计特性不准确和加速度计噪声统计特性不准 由于P+1,是系统经修正后一步预测的误差协 确3个试验验证3种滤波方法对系统模型不确定的 方差矩阵，它已经包含了系统噪声沿非线性函数的 鲁棒性和容错能力. 传播结果，因此重新计算Sigma点时只需要考虑量 4.1系统状态突变 测噪声的作用，这样减少了Sigma点的数量，从而降 采用静态数据进行试验，并在t=30s时，设定 低了计算量. 系统的俯仰角和横滚角误差突变为-10°，检验滤波 扩维系统状态变量+1=[x1v]T,则有 器的容错能力（如图1~3）.通过试验结果可以看 Pa=E[(41-)(1-弟+e)]= 出，AFF-AUKF和AFM-UKF算法与传统UKF算法