第7卷第4期 智能系统学报 Vol.7 No.4 2012年8月 CAAI Transactions on Intelligent Systems Aug.2012 D0I:10.3969/j.issn.1673-4785.201203001 网络出版地址：htp:/nw.cmki.net/kcms/detail/23.1538.TP.20120712.1052.007.html 自适应扩维UKF算法在SINS/GPS 组合导航系统中的应用 孙尧，马涛，高延滨，王璐2 (1.哈尔滨工程大学自动化学院，黑龙江哈尔滨150001；2.上海交通大学航空航天学院，上海200240) 摘要：针对自适应渐消因子卡尔曼滤波无法应用于非线性系统的问题以及自适应渐消因子的局限性，提出了带自 适应渐消矩阵的扩维UKF(adaptive fading matrix augmented UKF,AFM-AUKF)算法.该算法针对含有非加性白噪声 的非线性系统，引入了一种新的自适应渐消矩阵计算方法，并用Unscented变换逼近系统的后验均值和协方差，有效 解决了此类系统的滤波问题.针对SNS/GPS组合导航系统的非线性状态估计问题，分别设计了滤波器容错试验和 系统噪声突变试验，试验结果证明了该算法的有效性。 关键词：扩维UKF;自适应渐消矩阵；组合导航；非线性滤波 中图分类号：TP18;U666.11文献标志码：A文章编号：16734785(2012)04-034507 An adaptive augmented unscented Kalman filter with applications in a SINS/GPS integrated navigation system SUN Yao',MA Tao',GAO Yanbin',WANG Lu2 (1.College of Automation,Harbin Engineering University,Harbin 150001,China;2.School of Aeronautics and Astronautics,Shang- hai JiaoTong University,Shanghai 200240,China) Abstract:Because an adaptive fading Kalman filter cannot be applied to nonlinear systems,an augmented unscent- ed Kalman filter(AUKF)based on an adaptive fading matrix(AFM)was proposed in this paper.The AFM-AUKF algorithm was implemented by first calculating the adaptive fading matrix,and then using the unscented transforma- tion to estimate the posterior mean and covariance of the state of a nonlinear system,so as to effectively solve the filtering problem.In order to solve the problem of nonlinear state estimation in a low-cost integrated navigation sys- tem,a filter fault-tolerant experiment and a system noise mutation experiment were designed and implemented,re- spectively.The experimental results prove that the algorithm enhances the robustness of the filter when the system model is uncertain,improves the accuracy of the filter,and has a strong fault-tolerant ability. Keywords:augmented unscented Kalman filter;adaptive fading matrix;integrated navigation;nonlinear filter 卡尔曼滤波是一种递推的线性无偏最小方差估 态估计过分依赖于陈旧数据，导致滤波发散3) 计，当理论模型与实际系统完全匹配时，卡尔曼滤波 衰减记忆滤波方法能够通过自适应渐消因子限 器输出的残差序列是互不相关的高斯白噪声序 制滤波器的记忆长度，从而抑制了滤波发散4.但 列口.然而，其前提是必须精确已知系统模型和噪 是，对高阶系统进行滤波时，由于系统各状态量估计 声的统计特性，否则将会出现滤波器精度下降甚至 精度的差异，通过简单渐消因子的计算，很难达到理 发散等问题2].这是由于当系统噪声统计特性未知 想的效果.针对这一问题，文献[3,5]通过自适应渐 时，估计误差的协方差矩阵随时间逐渐收敛于零，状 消矩阵限制滤波器的记忆长度，但其前提条件为系 收稿日期：20120301.网络出版日期：201207-12， 统状态量是直接可观测的，即系统量测矩阵中对应 基金项目：国家自然科学基金资助项目(50909025/E091002):国际科 部分为对角阵，这极大地限制了自适应渐消矩阵在 技合作基金资助项目(2010DFR80140). 通信作者：马涛.E-mail:mt_0606@yahoo.com.cn. 具有复杂量测方程系统中的应用
·346 智能系统学报 第7卷 传统渐消因子的计算方法以卡尔曼滤波为基 的滤波问题在高阶系统中，由于各状态量估计精度的 础，仅适用于线性系统状态估计，而组合导航系统是 差异，使用自适应渐消因子的滤波效果并不理想 典型的非线性系统，由于忽略了高阶误差项，基于线 文献[5]通过自适应渐消矩阵对卡尔曼滤波的 性误差模型的组合导航系统滤波精度将会下降.Ju 记忆长度进行限制，记： lier提出了基于Unscented变换的UKF(nscented 「M41=H电Pp+nHg, Kalman filter)算法[s,不论系统非线性程度如何， N =Cie -H2H-Rxe. Unscented变换都能以三阶泰勒精度逼近非线性系 1 统状态的后验分布.然而，传统UKF算法假定系统 式中：C+1=k+1召y,y1=21-A1玉w为 噪声为加性噪声，不参与系统非线性传递，Mewe等 新息矢量， 针对含有非加性噪声的非线性系统，提出了扩维 自适应渐消矩阵为 UKF(augmented UKF,AUKF)算法[1，文献[8]从理 m 论上给出了AUKF算法可以提高滤波精度的证明. Sk+1=diag(s1,s2,…,sm,1,…,1) 但UKF和AUKF算法与经典卡尔曼滤波器一样，以 最小方差估计原理为理论基础，都需要精确已知系 式中+》却小=1.2-a 统的模型和噪声统计特性. n为系统状态量维数，m为系统观测量维数.从而得 针对以上分析，提出了带自适应渐消矩阵的 到带自适应渐消因子的误差协方差矩阵一步预测过 AUKF(adaptive fading matrix augmented UKF,AFM- 程如式(2)： (2) AUKF)算法.为了使渐消矩阵适用于具有复杂量测方 Pi+k Sk+P+2k 程的系统，提出一种新的自适应渐消矩阵的计算方 该方法的前提条件为系统量测矩阵中对应部分 法，并以Unscented变换对系统的后验均值和协方差 为对角阵，当系统状态量存在耦合关系，或系统状态 进行近似，从而将自适应渐消矩阵的使用扩展到含有 量不是直接可观测的时候，该计算方法无法使用.且 非加性白噪声的非线性系统状态估计领域。 随着时间的推移，C+1的计算量会逐渐增大，保存从 初始时刻开始的新息矢量对于长时间工作的滤波器 1渐消矩阵的分析与计算 来说也是很难实现的. 考虑如下线性离散系统： 针对以上问题，本文根据滤波最优的条件，提出 ∫下k+l=中+lkk+Wk, 一种自适应渐消矩阵的计算方法. 若k时刻滤波最优，根据卡尔曼滤波的无偏性和 LZ+=Hi+1+Vel 文献[3]提出了一种自适应渐消因子卡尔曼滤波 正交性原理，残差应服从均值为0的高斯分布，即 (adaptive fading factor Kalman filter,.AFF-KF)算法， y+~N(0,H+P+Rk). 记为 由式(3)构造残差的加权平方和： rAk+1=Hg+1中+kPEHE+1, Yk+1=y+1(H+1Pk+zHE+1+Rk+i）-y+:(3) (1) Bi+t =H-QH+R+ 根据x2分布的定义，Yk+1服从m自由度的中心X分 布，通过自适应渐消矩阵对一步预测误差协方差矩 式中：Q和R分别为系统噪声和量测噪声的协方差 阵的修正过程同式(1)，根据式(1)可以看出，Sk+ 矩阵，且Q和R正定， 自适应渐消因子为 无法通过直接计算获得.假设2k+1Hs+1=Hk+S+1, 则有 Ak+1=max(1,s1,s2,…,sm）. 式中： Yk=yi(2+HPH+ Y+1(i)Bk+1(i,i) Hk+12H+l+Rk+1）y… (4) > A(,08A1（,i=1,2,…,m, 根据X检验准则，得到2+1的计算方法： 8k Zk-Zw/k-1. e=diag(01,02,,0m), (5) 从而得到带自适应渐消因子的误差协方差矩阵 yk(i)B(i,i) 一步预测过程为 0:>=A(i,i)EA（,),i=1,2,…,m Pk+lk=入s+H④x+山P④+k+Qk (6) 这种计算渐消因子的方法简单，适用于低阶系统 式中：Ak+1和Bk+1的计算方法同式(1)
第4期 孙尧，等：自适应扩维UKF算法在SNS/GPS组合导航系统中的应用 ·347· 假设Hk+1是行满秩的.当H+1不存在全零列 针对自适应渐消矩阵的计算，当H+,不存在全 时，自适应渐消矩阵为 零列时，可由式(5)~(7)获得；否则，采用式(5)、 Sk+1=（H+1Hk+1）-H+2k+1Hk+1. (7) (6)、(8)~(10).在此基础上，可以得到经自适应渐 从而通过式(7)得到经自适应渐消矩阵修正后的一 消矩阵修正后的一步预测协方差矩阵为 步预测误差协方差矩阵. P+=S+IP++P+P (13) 在组合导航系统中H+1存在全零列的情况十 3AFM-AUKF算法 分常见，此时假设系统状态维数为，系统观测量维 数为m,H+1中全零列的维数为P,将H+1表达为如 相对于经典卡尔曼滤波，UKF主要解决的是随 式(8)： 机向量沿非线性函数的传播问题.UKF算法用一组 Hk+1=[H-P）0m9]. (8) 确定的Sigma点来近似状态分布，通过Unscented变 渐消因子矩阵的计算过程为： 换来逼近系统状态的后验均值和协方差].使用 S=(HH)-HH, (9) AFM-AUKF算法时，首先以对称采样策略获得Sig rSP)x(a-p） ma点，通过时间更新和量测更新，得到系统一步预 Sk+1= 0px(n-p) (10) 0(m-p)9 测的误差协方差矩阵P+k和后验协方差矩阵 式中：=a-m. PP进而通过式(3)~(6)、(11)（当 H1存在全零列时，通过(3)~(4)、(6)~(9)、 2AFM在UKF算法中的等价描述 (11))得到自适应渐消矩阵Sk+1和经修正后的一步 通过以上分析可以看出，计算自适应渐消因子 预测误差协方差矩阵P+1k·此时，一步预测误差协 和渐消矩阵的方法都是基于卡尔曼滤波框架的，需 方差矩阵Pk+1被修正为P+1k,则后验协方差矩阵 要获得系统的状态转移矩阵和量测矩阵等信息.然 Pnn应随之被修正为珍成n 而，在AUKF算法中，状态转移矩阵和量测矩阵等系 而此时的Sigma点无法完成这一修正过程，故需要 统信息无法直接获得；因此，无法通过卡尔曼滤波框 利用+k和P+,重新生成服从均值为无+1k、方 架下的计算方法得到AUKF滤波器的自适应渐消矩 差为P+k的高斯分布的Sigma点，并重新进行量测 阵.由于AUKF与UKF算法在原理上是相同的，因 更新，进而得到P函P1,最后根据系统状 此，通过UKF的等价描述，可以得到AUKF滤波器 态一步预测+和后验协方差矩阵P 自适应渐消矩阵的计算方法. P完成对系统状态的点估计. 根据卡尔曼滤波和UKF算法的等价描述]： 考虑如下非加性噪声的非线性离散系统： (Pi=HPR xk1=f（k,u,,g), P=P lzk+1=h(k+1,x+1,wk1) (11) Ki=PP 式中：k≥0为离散时间变量；x∈R”为状态向量； 4∈R”为输入向量；z∈R为输出向量；非线性函数 =PP ∫∈R×R"→R",heR”→Rm.系统噪声yk、量测噪 可以得到计算自适应渐消矩阵过程中各变量的 声w:分别为q维和m维的高斯白噪声，并具有以 表达式为 下统计特性： E[v]=0,E[y]=O6, Ak+=P E[w]=0,E[ww]=R6, B Pi-Ake E[yw]=0. 式中：Pk+为系统误差的协方差矩阵沿非线性函数 式中：Q和R为正定对称阵，6，为kronecker-8函数， 的传播结果，计算P%1k的步骤与UKF算法中计算 初始化系统状态和误差的协方差矩阵为 Pk+uk的步骤相似，只是计算Sigma点沿非线性函数 o=E[x],P=E[(x-o)(x0-)] 的传播过程的时候，不考虑系统噪声的作用即可.显 根据自适应渐消矩阵在UKF算法中的等价计 然，对于系统噪声为加性白噪声的非线性系统，有 算方法，得到AFM-AUKF算法的步骤： Qk=P+k-Pk+ 1)计算Sigma点
·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算法
第4期 孙尧，等：自适应扩维UKF算法在SNS/CPS组合导航系统中的应用 ·349. 相比，姿态误差的均值分别降低了44.6%和83， UKF.---AFF-UKK----AFM-UKE 2%,姿态误差的标准差分别降低了32.4%和54. 压 9%. 12 24364860 ts 。0 10 一UKF-AFF-UKF--AM-UK 0 。 -UKF----AFF-UKF----AFM-UKF -10 nnt户o的 12 24 364860 12 24 36 4860 10[-UKF----AFF-UKF ----AFM-UKF 。 -UKF----AFF-UKF ----AFM-UKK -10 0P4nwww4用 12 24 3648 60 钩州 0 12 24 364860 tis IOf—UKF---AFF-UKF…AFM-UKF 0 图4 系统噪声不准确时的位置误差 -10 0 12 24364860 Fig.4 Attitude error while the statistics of system noise t/s is inaccurate 图1 系统状态突变时的姿态误差 4.3加速度计噪声统计特性不准确 Fig.1 Attitude error of state mutation 使用摇摆数据进行试验，假设在t=308时，系 —K-·AFF-UKF--AFM-UKF 统噪声的统计特性发生突变，但仅有加速度计噪声 的方差变为原来的100倍，而陀螺的噪声统计特性 12 24 36 48 60 t/s 不发生改变（如图5所示）.此时，AFF-UKF和AFM- -UKF---AFF_UKF----AFM-UKF UKF算法与传统UKF算法相比，姿态误差的均值分 别降低了7.0%和78.3%，姿态误差的标准差分别 0 12 24 36 48 60 降低了2.6%和88.1%， —KF…AF-UKF--AFM-UKF —UKE--AFE-UKF---AFM-UKF 0 AW 12 24364860 -20 0 12 24364860 图2 系统状态突变时的速度误差 -UKF----AFF-UKF----AFM-UKF 2 Fig.2 Velocity error of state mutation 5 -UKF.--AFF-UKF----AFM-UKF 饰nw So 0 12 24.364860 12 24 364860 tls -UKF---AFF-UKF----AFM-UKF -UKF----AFF-UKF ----AFM-UKF 性型随 12 24 乐- 0 36 48 60 0 12 24 36 4860 t/s 图5加速度计噪声统计特性不准确时的位置误差 53e —UKF…AFF-UKF---AF'M-UKF Fig.5 Attitude error while the noise's statistics of ac- 50 12 24 36 48 60 celerometer is inaccurate I/s 4.4结果分析 图3系统状态突变时的位置误差 从以上实验结果可以看出，由于系统的速度和 Fig.3 Position error of state mutation 位置状态量是直接可观测的，所以3种方法在位置 4.2系统噪声统计特性不准确 和速度估计效果上的差异并不明显，只是AFM-UKF 使用摇摆数据进行试验，假设在t=30s时，系 算法的位置估计精度略高于AFP-UKF算法和传统 统噪声的统计特性发生突变，陀螺仪和加速度计噪 UKF算法.但是通过表1中数据的比较能够看出： 声的方差均变为原来的100倍（如图4）.此时，AF℉- 在系统状态突变时，2种衰减记忆方法对突变状态 UKF和AFM-UKF算法与传统UKF算法相比，姿态 的跟踪能力均强于传统UKF算法，AFM-UKF算法 误差的均值分别降低了54.4%和71.6%，姿态误差 的跟踪速度又略高于AFT-UKF算法；在系统噪声全 的标准差分别降低了59.6%和90.6%. 部发生改变且变化幅度一致时，AFF-UKF和AFM