四元数约束的容积卡尔曼滤波及其应用

钱华明，黄 蔚，孙 龙

(哈尔滨工程大学自动化学院，150001哈尔滨)

针对非线性系统状态估计问题，文献［1］提出了一种新的非线性滤波，容积卡尔曼滤波(cubature kalman filter，CKF).其核心思想是针对带高斯白噪声的非线性系统，利用三阶球面-相径容积规则来求取非线性函数的统计特性，即均值和方差，估计精度在理论上能够达到泰勒展开的三阶精度，算法优点为实现简单、收敛性好以及精度高等.然而CKF的理论推导中，状态变量都是假设不存在约束条件，但在实际应用中，由于存在几何约束、运动约束等问题，使得状态变量可能存在某些线性或非线性的约束条件，如车载导航系统可能受道路条件的约束，四元数存在规范化等.现有关于 CKF的文献［2-8］，并未考虑状态存在约束的情况.因此，讨论状态约束下的CKF设计方法，在实际应用中具有重要意义.

在姿态估计领域，姿态描述参数主要有四元数、欧拉角、方向余弦以及修正罗德里格斯参数等.其中，四元数在计算量上相对较小，不存在三角函数以及超越函数方面的运算，同时也没有欧拉角描述姿态时存在的奇异性问题，因此，以四元数作为姿态描述参数的非线性姿态估计滤波问题在国内外已成为热点问题［9-13］.由于四元数作为状态变量时存在归一化的约束，如果不考虑约束直接作滤波处理，滤波精度差，甚至会导致协方差奇异.文献［14］提出重构系统状态参数，从而降低系统维数，避免了四元数约束.文献［15］提出将罗德里格斯参数与四元数相切换，避免了四元数的规范化问题.这些方法并没有将约束条件与滤波相结合，而文献［16］提出了约束下的卡尔曼滤波，将线性等式状态约束与卡尔曼滤波相结合.由于四元数约束本质上属于非线性约束，因此，文献［17］将其线性化作为线性等式约束与滤波结合，但是这种线性化引入了截断误差，影响滤波精度.文献［18］提出两步投影理论来解决非线性状态等式约束情况.文献［19］提出增益修正理论与扩展卡尔曼滤波结合来解决非线性状态约束问题，但它是以扩展卡尔曼滤波为主要框架.而本文从最优滤波本质出发，将CKF与增益修正理论相结合，利用四元数约束条件与最小均方误差估计准则构造最小约束代价函数，从而能够保证四元数满足归一化的条件.

因此，本文采用四元数作为姿态描述参数，针对其构成的非线性姿态估计模型，提出了一种四元数约束下的CKF算法(quaternion constrained cubature kalman filter，QCCKF).利用三阶球面 -相径容积规则来近似计算系统状态的后验均值和协方差，针对状态变量存在四元数约束的问题，采用最小约束代价函数来修正滤波增益，推导出了QCCKF算法的递推公式.然后通过仿真实验验证了本文提出的QCCKF算法的有效性和正确性.

1 问题描述

考虑非线性系统为

式中:xk和zk分别为系统n维状态向量和m维量测向量;f(·)为系统方程中的非线性状态函数;h(·)则为量测方程中的非线性量测函数;系统噪声wk是高斯白噪声，其均值为零，方差为Qk-1，而vk则为量测噪声，其均值为零，方差为Rk，系统噪声和量测噪声互不相关．

上式为在量测数据z1，z2，…，zk已知的情况下状态向量xk的概率密度函数．针对式(1)的状态假设，使得J(xk)最大的状态向量值就是系统(1)的最优状态估计［20］．为解决这个问题，可以采用一种次优的方法去近似非线性函数的均值和方差，如无迹卡尔曼滤波(UKF)、中心差分滤波、CKF等.

但是由于状态变量xk中存在四元数约束qTq=1．因此，需要对无约束条件下的滤波进行

kk修正，使得在有四元数约束的情况下式(2)最大，从而得到最优的状态估计．问题是在四元数约束下，如何采用基于最小均方误差估计准则来修正滤波增益，求四元数约束下的容积卡尔曼滤波公式．

2 QCCKF算法

2.1 三阶球面-相径容积规则

由于上面3个多元积分很难求得解析解，所以通常需要进行近似计算.而对形如:Ⅰ=∫g(x)N(x;，Px)dx的积分函数，通常无法准确求出其积分值，只能采用一些近似的方法，本文则采用三阶球面 -相径容积规则对该积分值进行近似求取．主要的求取方法就是利用状态变量x的先验均值和方差信息，利用容积规则得到一系列带权值的容积点，再将求得的容积点通过非线性函数f(·)进行传播得到相应结果，最后利用得出的结果计算近似的积分值．

1)对Px做乔列斯基分解

2)求取容积点 (i=1，2，…，m)

式中:m表示容积点总数，其为状态维数的2倍，即m=2n;第i个容积点用ξi表示，该容积点的产生可以采用如下方式，假设 e= ［1，0，…，0］T为单位向量，其维数为n，符号［1］被采用来表示对单位向量e中的元素实行全排列或是将其中的元素符号进行改变所得到的点集，该点集是完整全对称的，符号［1］i则被描述为点集［1］中的第i个点;ωi为对应点的权值．

3)利用非线性函数f(·)对容积点αi进行传播，可得 γi=f(αi)．

2.2 QCCKF算法实现

引理1对于非线性系统(1)，基于贝叶斯估计的状态估计以及状态误差协方差阵为

式中Kk∈Rn×m为滤波状态增益阵，

该引理的详细证明见参考文献［1］.从引理1可以看出，求得状态估计值以及状态误差协方差阵需要知道滤波增益Kk，在传统的线性卡尔曼滤波以及非线性滤波如UKF、CKF中，利用最小均方误差估计可以求出Kk=PxzP-1zz，从而得到状态估计，但是这都未考虑到状态变量存在约束的情况.现将状态估计方程(3)分为四元数部分与非四元数部分:

式中 Kq∈Rnq×m，Kβ∈Rnβ×m，rk|k-1=zk-k|k-1．由于状态变量中的四元数部分存在归一化，即qTkqk=1，就会使得滤波增益中四元数部分Kq也存在约束条件，如:

滤波增益Kq存在约束条件，会影响滤波增益Kk的结果．因此，由最小均方误差估计准则，定义最小约束代价函数为

约束条件为

为求解上述问题，给出引理2以及定理3.

引理2假设矩阵A、B、C、D中，A与D为可逆的方阵，则有

该引理的证明见参考文献［21］.

式中:

证 明

式中:

构造拉格朗日代价函数:

式中λk为拉格朗日乘子，为使上式最小，对其求导可得

由式(12)、(13)可得

为了求出λk，利用引理2，将式(15)变形为

将式(17)代入到式(14)中得

求解可以得出

针对上述符号的选取，考虑到滤波的稳定性以及(Pzz+λkrk|k-1rTk|k-1)的可逆性，λk取一个正数，即上述公式中取负号:

利用定理3求得的滤波增益Kk带入到式(3)和式(4)中，从而得到在四元数约束条件下的基于贝叶斯估计的状态估计及状态误差协方差阵.

对于式(5)～(9)的积分，采用三阶球面-相径容积规则求取近似解，同时根据上述的两个定理，得到四元数约束下的CKF滤波公式.滤波递推公式如下:

1)已知k-1时刻状态xk-1的统计特性为，求取容积点为

式中Pk-1=

2)容积点αi，k-1进过非线性函数f(·)传递得

4)根据状态xk的一步预测统计特性N(xk;k|k-1，Pk|k-1)，求取相应的容积点为

式中Pk|k-1=

5)量测更新，经过非线性函数h(·)传递得

7)根据式(18)～(20)，利用定理3求得滤波增益Kk，然后代入到引理1的式(3)和式(4)中求出状态估计k|k和状态误差协方差阵Pk．

3 姿态估计模型

3.1 陀螺量测模型

若飞行器的质心本体坐标系与陀螺测量坐标系相重合，则陀螺模型可以表示为

3.2 系统状态方程

四元数乘法有不同的定义准则，本文采用文献［14］的四元数乘法定义准则，则飞行器姿态运动学方程可表示为

由四元数表示的载体姿态矩阵为

A(q)=(q24- ρTρ)Ⅰ3×3+2ρρT-2q4［ρ ×］．将姿态四元数q(t)与陀螺漂移β(t)组成状态向量，x(t)=［q(t)Tβ(t)T］T，建立四元数的非线性状态方程为

式中

将式(23)进行离散化，可得

式中:qk为k时刻的姿态四元数;k为k时刻的陀螺量测输出值;βk为k时刻的陀螺漂移值;Δt为采

样周期;wk为零均值的高斯白噪声序列，其方差根据文献［9］为

3.3 系统量测方程

星敏感器的测量模型为

式中:zik为星敏感器的输出;A(q(tk))为在tk时刻真实的姿态矩阵→■;ri为星敏感器的参考向量;vik为零均值的高斯白噪声;i为取不同参考矢量所对应的数字;为获得姿态信息，至少需要2个不平行参考矢量的观测值，则非线性量测模型为

式中:Zk为扩维的量测向量;h(xk)为与状态有关的非线性函数;vk为零均值的高斯白噪声，其方差为σ2sⅠ6×6．

4 仿真实验及分析

本文的姿态估计仿真平台是由捷联惯导和星敏感器组成，其主要模块有轨迹发生器模块、SINS模块、星敏感器模块以及滤波模块.仿真实验中的初始参数按如下条件设置:假设飞行器在地球表面上的初始地理位置为东经126°，北纬45°，飞行初始高度为0，初始航向角度为90°，初始速度为0，仿真实验时间设为600 s，陀螺量测噪声设为σv=0.05(°)/h，陀 螺 漂 移 噪 声 设 为 σu=星敏感器的测量噪声标准差为σs=15″，其输出频率为2 Hz;陀螺漂移的初始值设为β=［111］T(°)/h，姿态角的初始时刻误差设为［0.5 0.5 15］T(°)．同时，假设滤波模块中姿态角与陀螺漂移的初始估计值均为零，与之对应的初始方差阵分别设为 (0.2°)2Ⅰ3×3和(1.2(°)/h)2Ⅰ3×3．

为说明本文算法的有效性，将本文提出的QCCKF算法与传统CKF算法以及文献［15］提出的USQUE算法进行仿真对比与分析.首先，由轨迹发生器模块模拟出一条飞行器的飞行轨迹，如图1所示，滑跑、加速、爬高、转弯等是飞行器的主要飞行动作，然后利用SINS模块以及星敏感器模块产生飞行轨迹相应的仿真测量信号，再通过滤波模块进行姿态估计.图2～4为仿真结果.

图1 载体运动轨迹

图2 横滚角姿态误差

图3 俯仰角姿态误差

图4 偏航角姿态误差

从图中可以看出，QCCKF算法估计精度明显优于常规CKF以及USQUE算法.这是由于状态变量存在四元数约束条件，导致滤波增益也存在约束，从而影响常规CKF的估计效果，而USQUE算法虽然将四元数与三维的修正罗德里格参数进行来回切换，避开了四元数存在归一化约束的问题，但它是以UKF算法为基础.

表1显示的是通过100次的蒙特卡罗仿真，3种滤波算法的性能对比，可以看出QCCKF整体性能均优于CKF和USQUE算法，同时满足四元数约束条件.QCCKF算法相比较USQUE算法一次迭代时间减少约二分之一，虽然QCCKF算法状态维数为n=7，其采样点为14个，USQUE算法采用的是三维的误差四元素，其状态维数为n=6，则其采样点为13个，因此从采样点数目来看QCCKF算法的运算效率略有下降，但是由于USQUE算法频繁地在四元数与三维的修正罗德里格斯参数之间进行切换，对姿态估计系统的实时性造成了严重影响.

表1 三种滤波算法性能对比

5 结语

常规CKF算法未考虑状态变量存在状态约束的情况，这样就限制了常规CKF的应用.针对其在状态约束下存在滤波精度差的问题，将四元数约束与常规CKF相结合，提出了一种四元数约束下的CKF算法.该算法基于贝叶斯估计理论，采用三阶球面-相径容积规则来近似计算系统状态的后验均值和协方差，并将四元数约束转化为增益约束，利用最小约束代价函数来修正滤波增益，从而得到了四元数约束下的CKF滤波公式.仿真结果表明，相比于CKF和USQUE算法，该算法具有更好的估计性能和更高的估计精度.

［1］ARASARATNAM I，HAYKIN S.Cubature Kalman filter［J］.IEEE Transactions on Automatic Control，2009，54(6):1254-1269.

［2］ARASARATNAM I，HAYKIN S，HURD T R.Cubature Kalman filtering for continuous-discrete systems:theory and simulations［J］.IEEE Transactions on Signal Processing，2010，58(10):4977-4993.

［3］PAKKI K，CHANDRA B，GU D W.Cubature information filter and its applications［C］//Proceedings of the 2011 American Control Conference. Piscataway:IEEE，2011:3609-3614.

［4］穆静，蔡远利.迭代容积卡尔曼滤波算法及其应用［J］.系统工程与电子技术，2011，33(7):1454-1457.

［5］穆静，蔡远利.平方根容积卡尔曼滤波算法及其应用［J］.兵工自动化，2011，30(6):11-13.

［6］孙枫，唐李军.Cubature粒子滤波［J］.系统工程与电子技术，2011，33(11):2554-2557.

［7］PESONEN H，PICHE R.Cubature-based Kalman filter for positioning［C］//Proceedings of the 7th Workshop on Positioning，Navigation and Communication.Dresden:WPNC，2010:45-49.

［8］钱华明，葛磊，黄蔚，等.基于贝叶斯估计噪声相关下的CKF设计［J］.系统工程与电子技术，2012，34(11):2214-2218.

［9］CHOUKROUN D，BAR-ITZHACK I Y，OSHMAN Y.Novel quaternion Kalman filter［J］.IEEE Transactions on Aerospace and Electronic Systems，2006，42(1):174-190.

［10］CRASSIDIS J L，MARKLEY F L，CHENG Yang.Survey of nonlinear attitude estimation methods［J］.Journal of Guidance，Control，and Dynamics，2007，30(1):12-28.

［11］AHMADI M，KHAYATIAN A，KARIMAGHAEE P.Attitude estimation by divided difference filter in quaternion space ［J］.Acta Astronautica，2012，75(1):95-107.

［12］钱华明，黄蔚，孙龙，等.基于多重次渐消因子的强跟踪UKF姿态估计［J］.系统工程与电子技术，2013，35(3):580-585.

［13］钱华明，黄蔚，葛磊，等.基于四元数平方根容积卡尔曼滤波的姿态估计［J］.北京航空航天大学学报，2013，39(5):645-649.

［14］MARKLEY F L.Attitude error representations for Kalman filtering ［J］.Journal of Guidance，Control，and Dynamics，2003，26(2):311-317.

［15］CRASSIDIS J L，MARKLEY F L.Unscented filtering for spacecraft attitude estimation ［J］.Journal of Guidance，Control，and Dynamics，2003，26(4):536-542.

［16］SIMON D，CHIA T.Kalman filtering with state equality constraints［J］.IEEE Transactions on Aerospace and Electronic Systems，2002，38(1):128-136.

［17］TEIXEIRA B O S，CHANDRASEKAR J，TORRES L，et al.State estimation for linear and nonlinear equality-constrained systems［J］.International Journal of Control，2009，82(5):918-936.

［18］JULIER S J，Jr LAVIOLA J J.On Kalman filtering with nonlinear equality constraints［J］.IEEE Transactions on Signal Processing，2007，55(6):2774-2784.

［19］ZANETTI R，MAJJI M，BISHOP R H，et al.Normconstrained Kalman filtering［J］.Journal of Guidance，Control，and Dynamics，2009，32(5):1458-1465.

［20］RAWLINGS J B，BAKSHI B R.Particle filtering and moving horizon estimation ［J］. Computers and Chemical Engineering，2006，30(10/11/12):1529-1541.

［21］SIMON D.Optimal state estimation:Kalman，H infinity，and nonlinear approaches［M］.New York:Wiley-Interscience，2006:11-12.