汤霞清， 黄湘远， 武 萌， 张 环

(装甲兵工程学院控制工程系，北京 100072)

平方根形式的CKF/后向平滑非线性滤波研究

汤霞清， 黄湘远， 武 萌， 张 环

(装甲兵工程学院控制工程系，北京 100072)

为了提高非线性滤波的精度、数值稳定性并保证计算速度，将非线性平方根后向平滑 (Square Root Backward Smoothing, SR-BS) 算法和平方根容积卡尔曼滤波(Square Root Cubature Kalman Filter, SR-CKF)引入到前向滤波-后向平滑-前向滤波的滤波框架中，形成平方根形式的CKF/后向平滑(SR-BS-CKF)非线性滤波算法。该算法在首次前向滤波的基础上利用后向平滑-前向滤波的思想来提高滤波精度，利用平方根更新来保证滤波的数值稳定性；该算法包含2次SR-CKF和1次SR-BS运算，由于SR-BS计算量较小，因此该算法计算量为SR-CKF的2倍左右，增加不大。仿真结果表明：SR-BS-CKF算法滤波精度高，数值稳定性强，计算速度较快，滤波性能优于CKF。

容积卡尔曼滤波；非线性后向平滑；平方根滤波；滤波性能

基于量测信息和系统方程的最优估计理论在导航、信息处理、金融等领域获得了大量的应用。根据利用的量测信息的时间区别，可分为预测、滤波和平滑3种[1]。

滤波是利用当前时刻以及以前的量测信息对当前状态进行估计，典型算法有卡尔曼滤波及其非线性形式，如:扩展卡尔曼滤波(Extended Kalman Filter,EKF)、无损卡尔曼滤波(Unscented Kalman Filter,UKF)、容积卡尔曼滤波(Cubature Kalman Fiter, CKF)等[2]。CKF是一种计算量小、精度高的非线性滤波算法，能够实现任意高斯非线性系统的状态估计[3-4]。平方根CKF (Square Root CKF, SR-CKF)利用平方根矩阵代替协方差矩阵，降低计算机字长限制对数据处理精度的影响，避免协方差矩阵Cholesky分解，提高了算法的数值稳定性[5]。

平滑是一种离线处理算法，其利用以前、当前、以后的量测信息进行状态估计，估计精度理论上优于滤波。基于线性状态的平滑算法[6-7]已经获得了大量应用，而基于非线性状态的平滑算法[8-9]应用还较少。

为了提高滤波精度，曹意等[10]将非线性滤波和线性平滑结合起来，形成多次迭代和窗口平滑算法；霍光等[11]在此基础上，从计算量和滤波精度考虑，提出了后向平滑CKF (Backward Smoothing CKF, BS-CKF)算法。以上2种算法的本质依然是滤波算法，而不是平滑算法，只是借用了平滑算法的思想；而且2种算法均基于线性状态方程推导，具有一定的局限性。

为了提高非线性状态的滤波精度，本文重新推导了非线性状态的后向平滑算法，提出了平方根形式的后向平滑算法(Square Root Backward Smoothing，SR-BS)，将SR-CKF和SR-BS算法引入到前向滤波-后向平滑-前向滤波框架[10]中，形成SR-BS-CKF非线性滤波算法，最后通过仿真试验对该算法进行验证。

1 平方根容积卡尔曼滤波

非线性滤波就是基于非线性系统方程，根据当前及此前的观测量序列估计出当前系统状态。考虑如下离散非线性系统：

(1)

式中：xk和zk分别为k时刻系统的状态量和观测量；f(·)和h(·)为已知任意函数；系统噪声wk～N(wk;0,Qk)，其中Qk为系统噪声方差矩阵；观测噪声vk～N(vk;0,Rk)，其中Rk为观测噪声方差矩阵，且wk和vk互相独立。

1.1 球面径向容积规则

I(f)=∫Rnf(x)exp(-xTx)dx。

(2)

式中：Rn为积分区域。

取x=ry，yTy=1，r∈[0,∞)，积分式I(f)在Spherical-radial坐标系可写成

(3)

式中：Un为n维单位球面；σ(·)为Un上的元素。

积分I(f)可分离为球面积分S(r)和径向积分R，其中:

S(r)=∫Unf(ry)dσ(y);

三阶球面径向容积规则由三阶球面规则和一阶径向规则组成[3]，基本思路是分别对S(r)和R进行多项式逼近，利用两者的逼近方式对积分I(f)进行逼近。对于高斯分布x～N(x;μ,Σ)，I(f)的三阶近似规则为

(4)

CKF采用球面径向容积规则和Guass-Bayes滤波框架，利用2n个等权值的容积点来实现非线性积分的多项式逼近，避免了非线性模型的线性化，能够实现任意高斯非线性系统的状态估计，滤波精度达到三阶。

1.2 平方根滤波算法

常规CKF需对Pk-1和Pk|k-1进行Cholesky分解，这是一项计算量大且非常危险的工作。因为受计算机字长有限、初始矩阵P0设置不当等因素的影响，Pk-1和Pk|k-1易失去正定性，Cholesky分解无法进行，导致滤波精度急剧下降甚至滤波发散。SR-CKF是一种有效的解决方法，其降低了计算机字长限制对数据处理精度的影响，提高了算法的数值稳定性[4-5]。

1.2.1 时间更新

1.2.2 量测更新

1) 重采样及观测传播：

zi,k|k-1=h(xi,k|k-1)。

3) 计算Pzz的均方根Szz和Pxz：

4) 估计滤波增益阵，最优估计及协方差阵：

2 非线性平方根后向平滑算法

2.1 非线性状态后向平滑算法

后向平滑算法根据当前及此前的观测量序列及前向滤波获得的最优状态估计序列进行后向递推，来获得此前任意时刻状态的最优估计。后向平滑分为固定区间平滑、固定点平滑和固定滞后平滑[10]。三者之间没有本质的区别，主要区别在于针对的对象不同。基于线性系统的平滑算法结构简单、易于实现，获得了较多的应用。

首先，本文不加证明地给出定理1[9]。

定理1:设x、a、b是3个具有前二阶矩的随机矢量，令z=[ab]，则x基于z的最优估计为

(5)

(6)

(7)

将式(6)、(7)分别改写为如下形式：

(8)

将式(10)代入式(8)，由于Pk|k-1和Ck|k-1理论上均为可逆正定矩阵，则有

(11)

则k到k-1时刻的一步后向平滑状态为

(12)

(13)

协方差阵Pk-1|k为

(14)

(15)

由式(12)、(15)构成了非线性系统的一步后向平滑算法。

2.2 平方根后向平滑算法(SR-BS)

前文推导了非线性状态前向滤波的平方根算法和后向一步平滑算法，为了将两者结合起来，需推导后向平滑的平方根算法。

首先考虑滤波增益阵Wk的平方根算法，即

(16)

考虑到Pk|k-1为对称正定阵，则有

(17)

从而获得

(18)

将式(18)代入式(15)，可得

(19)

可得Pk-1|k的平方根矩阵Sk-1|k为

(20)

3 CKF/后向平滑非线性滤波

3.1 前向滤波-后向平滑-前向滤波

前向滤波是一种实时状态估计方法，后向平滑过程是一种精度优于前向滤波的事后状态估计方法，因为前向滤波能够消除大部分误差，经后向平滑后再次前向滤波可进一步消除误差，两者结合起来可获得一种高精度的实时滤波方法。曹意等[10]推导了UKF和线性平滑的多次迭代和窗口平滑滤波算法，证明了2种方案均能够提高估计精度，但是滤波精度并不一定随着迭代次数或窗口长度的增加而增加。

图1 滤波算法基本框架

3.2 平方根形式的CKF/后向平滑滤波算法

霍光等[11]基于线性状态方程设计了CKF/后向平滑滤波器，本文在此基础上考虑到SR-CKF和SR-BS的强数值稳定性，以及对非线性状态的适用性，将两者引入到前向滤波-后向平滑-前向滤波框架中，形成平方根形式的CKF/后向平滑非线性滤波器(SR-BS-CKF)，实现任意高斯非线性模型的高精度实时滤波。基本滤波框架如下：

4 仿真实验

考虑一个典型的飞行器控制问题[4]，其非线性离散方程为

(21)

x0=(1 000 m,300 m/s,1 000 m,0 m/s,-3°/s)T,

10 mrad/s)]2。

(22)

图2给出了200次仿真实验的位置、速度、角速度的RMSE曲线，考虑到滤波器稳定时间，只给出了50～100 s的误差曲线。图2表明：2种算法均能对状态进行有效估计，SR-BS-CKF的估计精度高于SR-CKF，其中位置估计体现得最明显，速度估计次之，角速度估计相差不大。表1给出了2种算法后50 s位置、速度和角速度的RMSE平均值，经计算可知：SR-BS-CKF算法的位置、速度和角速度估计精度比SR-CKF算法分别提高了10%、4%、1%左右，表明SR-BS-CKF算法对提高滤波精度有效。

图2 2种滤波算法的RMSE曲线对比

表1 2种滤波算法的RMSE均值

滤波算法位置/m速度/(m·s-1)角速度/((°)·s-1)SR-CKF24.978914.09391.4096SR-BS-CKF22.910913.61391.4018

表2给出了一次仿真实验(100 s)2种滤波算法的运算时间，可以看出：SR-BS-CKF所用时间为SR-CKF的2～3倍。因为每次滤波过程中，SR-BS-CKF需进行2次SR-CKF及1次SR-BS操作，而SR-BS的计算量相对SR-CKF较小，因此滤波运算主要集中在2次SR-CKF操作上。试验结果表明：SR-BS-CKF算法的计算量相对于SR-CKF算法增长不大，滤波实时性的实现并不是非常困难。

表2 2种滤波算法的运算时间

5 结论

为了提高CKF的滤波精度和数值稳定性并保证滤波实时性，本文将SR-BS、SR-CKF引入到前向滤波-后向平滑-前向滤波的框架中，形成了一种新的非线性滤波，能够适用于任何高斯非线性系统。为了进一步提高滤波精度，有以下3种可能的方式：1) 利用高阶非线性滤波代替常规CKF；2) 使用“多步后向平滑-前向滤波”的框架；3) 进行多次“一步后向平滑-前向滤波”，形成迭代算法。高精度的滤波方式必然带来较大的计算量，影响滤波的实时性，如何在两者之间找到一个平衡点需进一步研究。

[1] 宫晓琳，张蓉，房建成. 固定区间平滑算法及其在组合导航系统中的应用[J]. 中国惯性技术学报，2012，20(6)：687-693.

[2] 王小旭，潘泉，黄鹤，等. 非线性系统确定采样型滤波算法综述[J]. 控制与决策，2012，27(6)：801-812.

[3] 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.

[4] Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[5] 郝燕玲，杨峻巍，陈亮，等.基于SRCKF的水下航行器动基座初始对准技术[J]. 华中科技大学学报：自然科学版，2012，40(2)：123-127.

[6] 康泰钟，钟麦英，李建利. 基于时间双向解算融合的POS事后处理算法[J]. 仪器仪表学报，2012，33(9)：2067-2072.

[7] 秦显平，杨元喜.平方根滤波/平滑/双向滤波在LEO星载GPS定轨中的应用[J]. 武汉大学学报：信息科学版，2009，34(10)： 1176-1180.

[8] Simo S. Unscented Rauch-Tung-Striebel Smoother[J]. IEEE Transactions on Automatic Control, 2008,53(3):845-849.

[9] Wang X X, Pan Q, Liang Y, et al. Application of Unscented Transformation for Nonlinear State Smoothing [J]. Acta Automatica Sinica, 2012,38(7):1097-1101.

[10] 曹意，茅旭初.基于非线性滤波和平滑相迭代的GPS定位估计算法[J]. 上海交通大学学报，2009，43(7)：1108-1112.

[11] 霍光，李冬梅. 基于后向平滑容积卡尔曼滤波的单站无源定位算法[J]. 信号处理，2013，29(1)：68-74.

[12] Ito K, Xiong K Q. Gaussian Filter for Nonlinear Filtering Problem[J]. IEEE Transactions on Automatic Control, 2000, 45(5):910- 927.

(责任编辑:尚彩娟)

Research on Square Root CKF/Backward Smoothing Nonlinear Filter

TANG Xia-qing, HUANG Xiang-yuan, WU Meng, ZHANG Huan

(Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China)

In order to achieve higher precision and stronger numeric stability and an efficient computation, the paper introduces nonlinear Square Root Backward Smoothing (SR-BS) algorithm and Square Root Cubature Kalman Filter (SR-CKF) into the framework of forward filtering-backward smoothing-forward filtering, so as to form square root CKF/BS nonlinear filter (SR-BS-CKF) algorithm. The algorithm utilizes backward smoothing-forward filtering based on the first forward filtering to improve the accuracy. It ensures the stability of the filter values by using square root updating method. The algorithm contains two SR-CKF operations and one SR-BS operation, because SR-BS has the low calculating amount, the calculation amount of the algorithm approximately equals to two times of SR-CKF and increases slightly. The simulation results indicate that the algorithm SR-BS-CKF has high precision, stronger numerical stability and rapid calculating speed, which has more effective filter performance than CKF.

Cubature Kalman Filter (CKF); nonlinear backward smoothing; square root filter; filter performance

1672-1497(2015)02-0065-05

2014-11-20

军队科研计划项目

汤霞清(1965-)，男，教授，博士。

TN713

A

10.3969/j.issn.1672-1497.2015.02.013