-
卡尔曼滤波自1960年被提出以来,便迅速发展为动态数据处理常用的方法之一,特别是在大地测量以及GPS定位定轨等方面得到了广泛的应用[1]。卡尔曼滤波采用递推算法,即由参数的验前估计和新的观测数据进行状态参数的更新,如此卡尔曼滤波一般只需存储前一个历元的状态参数估计,无须存储所有的历史观测信息,显然卡尔曼滤波具有较高的计算效率,并可进行实时估计[2]。但当观测矩阵呈现复共线性时,卡尔曼滤波状态估计的精度会受到很大的影响[3, 4]。为了克服病态性影响,提高解的质量,一些学者给出了改进算法[5-9],但这些算法都受限于先验信息的确定或者某些特定模型的构建,不能有效解决标准卡尔曼滤波模型的病态性问题。文献[9]提出了一种卡尔曼滤波(biased Kalman filter,BKF)的改进算法。但这种算法仅仅针对观测矩阵是病态性的某一时刻对卡尔曼滤波状态估计进行了改进,而没有考虑到卡尔曼滤波是一种递推算法,它的每个递推周期中包含对被估计量的时间更新和量测更新两个过程,并包含增益计算回路和滤波计算回路两个计算回路[8],上一个递推周期中观测矩阵的病态性会对下一个周期的滤波值造成持续性的危害,所以BKF并不是一种克服病态性的完整滤波算法。本文给出了一种完整的有偏卡尔曼滤波算法,称之为有偏卡尔曼滤波,它在均方误差最小意义下优于卡尔曼滤波,并且在实际应用中取得了良好的效果。
-
卡尔曼滤波模型中有两个基本方程,称为状态方程和观测方程,分别为:
(1) 式中,Xk为p×1维向量,表示tk时刻的状态;Zk为n×1维向量,是tk时刻对状态Xk的观测;Фk,k-1为p×p维非奇异矩阵,是tk-1时刻至tk时刻的一步状态转移矩阵;Hk为n×p观测矩阵;Wk-1是系统噪声序列;Vk是观测噪声序列。
同时,Wk和Vk满足
(2) 式中,Qk为系统噪声序列的方差阵,假设为非负定阵;Rk为观测噪声序列的方差阵,假设为正定阵;δkj为Kronecher-δ函数。
-
按照卡尔曼滤波的基本原理[1, 2],在tk时刻Xk的估计 ${\hat X}$ k按下述方程求解:
状态一步预测
(3a) 状态估计
(3b) 滤波增益阵
(3c) 一步预测误差方差阵
(3d) 估计误差方差阵
(3e) 式(3(a)~3(e))即为卡尔曼滤波基本方程[10, 11]。只要给定初始值0和P0,根据tk时刻的观测量Zk,就可递推计算得tk时刻的状态估计 ${\hat X}$ k(k=1,2…)。
-
由卡尔曼滤波基本方程可知,tk时刻的状态估计又可表示为:
(4) 即 ${\hat X}$ k是式(5)的解:
(5) 如果在tk-1时刻的状态估计精度非常高,那么tk-1时刻的状态估计 ${\hat X}$ k-1就非常逼近于真值Xk-1,此时利用状态一步预测 ${\hat X}$ k/k-1就能更好地估计tk时刻的真值Xk,那么就有P-1k/k-1Xk≈Pk/k-1k/k-1-1。这样就能够把式(5)两边的第二项同时消去,得到一个约等式:
(6) 为了扰动性分析说明的方便,不妨假定式(6)为一个等式,即
(7) 称式(7)为法方程,HTkR-1kHk为法矩阵。若tk时刻的观测矩阵呈现病态性,且tk时刻的状态方程和tk-1时刻的状态估计不能对观测矩阵Hk的病态性施加有效控制,此时法矩阵的条件数很大[12],观测矩阵Hk或观测量Zk的微小扰动就会引起解的较大变化,使得状态估计严重失真[13]。
-
假设在时刻tk之前的观测矩阵都是良态的,tk时刻的观测矩阵Hk是病态的,此时的有偏卡尔曼滤波估计为:
(8) 式中, ${\hat X}$ Bk表示卡尔曼滤波估计经过有偏化处理后得到的状态估计,αk称为有偏系数(αk∈[0, 1])。
估计量Bk的均方误差为:
对均方误差MSE(Bk)求导并令导数为零,得:
当式中‖Xk‖2未知时,用 ${\left\| {{{\hat X}_k}} \right\|^2}$ 代替,其中‖·‖表示向量的欧氏范数。
估计量Bk的误差方差阵为:
(9) 式中,XkXTk如果是已知的,可以直接代入计算。但在大多数情况中XkXTk是未知的,此时用真值Xk的有偏估计 ${{\hat X}_{Bk}}$ 来代替,则式(9)可以写成:
tk+1时刻的卡尔曼滤波信息更新过程如下。
状态一步预测:
状态估计:
一步预测误差方差阵:
滤波增益阵:
估计误差方差阵:
在tk时刻对卡尔曼滤波进行有偏化处理后,使得新估计的解算质量更好,所以在求下一时刻的卡尔曼滤波时,利用有偏卡尔曼滤波估计 ${\hat X}$ Bk来代替卡尔曼滤波估计 ${\hat X}$ k,这样就使得在tk+1时刻的卡尔曼滤波估计 ${\hat X}$ B(k+1)更加逼近真值。如果在tk+1时刻观测矩阵依然呈现复共线性,那么此时得到的状态估计 ${\hat X}$ B(k+1)虽然比标准的卡尔曼滤波估计 ${\hat X}$ k+1的精度更高,但是观测矩阵的复共线性依然对卡尔曼滤波解 ${\hat X}$ B(k+1)的精度造成了较大影响。所以,为了克服或减轻此时病态性影响,需要再次对卡尔曼滤波进行有偏化处理
(10) 式中, ${\hat X}$ BB(k+1)表示在标准卡尔曼滤波递推结果 ${\hat X}$ B(k+1)的基础上又进行了有偏化处理。
在tk+1时刻估计的均方误差为:
(11) 其中,
(12) ${\hat X}$ B(k+1)按照标准的卡尔曼滤波方法迭代计算得到,而在计算的过程当中增益阵的选取准则是使估计的方差达到最小,并且增益阵的选取不受上一时刻偏差的影响,所以 ${\hat X}$ B(k+1)满足最小方差估计的无偏性,即
(13) 那么,式(12)等价于
(14) 且
(15) 故
(16) 对式(16)中αk+1求导并令导数为零,得:
(17) 当‖Xk+1‖2未知时,用 ${\left\| {{{\hat X}_{B\left( {k + 1} \right)}}} \right\|^2}$ 来代替。
把式(17)代入式(16)可得:
(18) 当选取合适的有偏系数时,新估计的均方误差优于卡尔曼滤波。如果在tk+1时刻观测矩阵是良态的,那么就没有必要对卡尔曼滤波进行有偏化处理,因为有偏卡尔曼滤波估计是在均方误差最小意义下的解,它是适当牺牲了估计的偏倚性来换取均方误差的减小[13],当观测矩阵呈现良态时对解的精度影响不大,不必牺牲解的精度。
有偏卡尔曼滤波是对标准卡尔曼滤波状态估计进行了一致均匀压缩,使得滤波结果得到了直接改善,但是并没有改变法矩阵的条件数,从而也没有改变其病态程度。
-
在本文的算例中,选用的一步状态转移矩阵Фk,k-1为:
观测矩阵Hk为:
设Qk=0.000 1×I4,Rk=0.000 1×I5。给定状态参数初始值 ${{\hat X}_0}$ =X0+0.001×[1 1 1 1]T,其中X0=[0.01 0.02 0.02 0.01]T,初始误差方差阵P0=I4。
-
情形1 若从第二个时刻开始,在每个递推周期内都对观测矩阵Hk的某个元素施以0.001×k的扰动,即
式中,k=2,…,20; ${{\tilde H}_k}$ 表示添加扰动后的观测矩阵。采用卡尔曼滤波算法,状态估计的第一个分量在施加扰动前后的变化情况如图 1(a)所示。
图 1 卡尔曼滤波估值 ${{\hat X}_k}$ (1)在扰动前后的比较
Figure 1. Comparison of Kalman Filter Estimate ${{\hat X}_k}$ (1)Before and After the Disturbance
情形2 若从第二个时刻开始,在每个递推周期内都对观测量Zk的某个分量(这里选取第二个分量)施以0.002×k的扰动,即 ${{\tilde Z}_k}$ =Zk+[0 0.002×k 0 0]T,其中k=2,…,20, ${{\tilde Z}_k}$ 表示添加扰动后的观测量。采用卡尔曼滤波算法,状态估计的第一个分量在施加扰动前后的变化情况如图 1(b)所示。
分析图 1可得出以下结论。
1) 当观测矩阵的病态性得不到有效控制时,病态性就会影响卡尔曼滤波估计的精度。例如在图 1(b)中,时刻tk=10时,卡尔曼滤波估计的第一个分量 ${{\hat X}_{10}}$ (1)为0.024 8,与真值的差别非常大。
2) 当病态性影响存在时,卡尔曼滤波解的抗干扰能力很不理想,即观测矩阵Hk或观测量Zk的微小扰动会引起解的较大变化。如在图 1(b)中,时刻tk=10时,在观测量Z10的第一个分量Z10(1)添加扰动0.02后,估值 ${{\hat X}_{10}}$ (1)变为0.048 9,可见状态估计的扰动非常大。
3) 在情形1中,当时刻tk从0~20变化时,观测矩阵 ${{\tilde H}_k}$ 数据列间的复共线性关系逐渐变强(即观测矩阵的病态性越来越严重)。由图 1(a)可以看出,病态性越严重,扰动对解的影响越大。
-
采用状态估计的均方误差作为评价算法优劣的准则,将本文给出的有偏卡尔曼滤波与卡尔曼滤波作比较,试验结果如图 2所示。
图 2 有偏卡尔曼滤波与卡尔曼滤波均方误差的比较
Figure 2. Comparison of Mean Square Error of Biased Kalman Filte and Kalman Filter
为了比较新算法和卡尔曼滤波算法估计各分量与真值的差异,将新算法和卡尔曼滤波算法状态估计的各分量分别与真值作差并取绝对值,得到结果如图 3所示。
图 3 有偏卡尔曼滤波和卡尔曼滤波
Figure 3. Comparison of Difference Value Between Biased Kalman Filter and Kalman Filter
为了比较新算法和卡尔曼滤波算法结果与真值的差异,将新算法和卡尔曼滤波算法状态参数估计与真值的欧氏距离作为评价准则,得到结果如图 4所示。
图 4 有偏卡尔曼滤波和卡尔曼滤波结果
Figure 4. Comparison of Euclidean Distance Between Biased Kalman Filter and Kalman Filter
1) 当观测矩阵的病态性不受控制时,卡尔曼滤波估计的均方误差和与真值之间的欧氏距离都很大,其解算质量很不理想。
2) 在本文设计的算例中,由于所用模型为定常线性系统,所以卡尔曼滤波估计的均方误差是收敛的,并且收敛速度很快,大概在递推了15次左右就收敛了。
3) 本文给出的有偏卡尔曼滤波算法在均方误差最小意义下,对标准的卡尔曼滤波作出了较大的改善,并且其状态估计与真值的差异性更小。所以新算法能够有效地克服病态性对状态估计的影响,较好地提高解算质量。
-
目前,针对离散动态系统病态性的相关研究和文献较少。相对于静态的Gauss-Markov模型,离散动态系统中观测矩阵的病态性对卡尔曼滤波的影响机理更加复杂,找到合适的应对策略也更加艰难。本文给出了分析离散动态系统中观测矩阵的病态性影响的一种方法,并结合有偏估计给出了一种应对策略。Gauss-Markov模型中病态性问题的相关研究已经较为成熟,因此,可借鉴其相关理论和方法,从多角度分析并解决离散动态系统中的病态性问题。
The Biased Kalman Filter and Algorithm
-
摘要: 从理论角度分析了观测矩阵的复共线性对卡尔曼滤波的影响,并在均方误差最小意义下,给出了一种有偏卡尔曼滤波算法。分别对观测矩阵和观测量施加扰动进行了试验和分析,证明观测矩阵的病态性会对卡尔曼滤波估计造成严重危害。数值模拟结果表明,本文算法能够有效改善观测矩阵病态性对卡尔曼滤波估计的影响,提高解算质量。Abstract: Kalman filter is one of the most common ways to deal with dynamic data and has been widely used in project fields. However, the accuracy of Kalman filter for discrete dynamic system is poor when the observation matrix is ill-conditioned. Therefore, the method for overcoming the harmful effect caused by ill-conditioned observation matrix in discrete dynamic system is studied in this paper.The causes of the ill-conditioned observation matrix and its effect on Kalman filter are analyzed. Biased Kalman filter and its algorithm are proposed by combining the biased estimation and Kalman filter in the sense of mean square error (MSE). The methods of choosing biased parameter in the new algorithm is proposed. By separately exerting some disturbance on the observation matrix and observation vector, two simulations are carried out. The experimental results show that the traditional Kalman filter is inaccurate when the observation matrix is ill-conditioned, and the biased Kalman filter is more accurate than the traditional one in terms of MSE.
-
Key words:
- Kalman filter /
- ill-conditioned /
- mean square error /
- biased Kalman filter
-
-
[1] 崔希璋,於宗俦,陶本藻.广义测量平差[M].武汉:武汉测绘科技大学出版社, 2001 Cui Xizhang,Yu Zongchou, Tao Benzao. Generalized Surveying Adjustment[M]. Wuhan: Wuhan Technical University of Surveying amd Mapping, 2001 [2] 杨元喜.自适应动态导航定位[M]. 北京:测绘出版社, 2006: 51-68 Yang Yuanxi. Adaptive Navigation and Kinematic Positioning[M]. Beijing:Surveying and Mapping Press, 2006:51-68 [3] Ou Jikun, Ding Wenwu, Liu Jihua, et al. An Improved Algorithm for Autonomous Orbit Determination of Navigation Satellite Constellation[J]. Survey Review,2011,43(9):361-369 [4] 韩松辉,归庆明,李建文,等.分布式自主定轨的岭型EKF算法[J].武汉大学学报·信息科学版, 2013,38(4):399-402 Han Songhui, Gui Qingming, Li Jianwen, et al. Ridge-Type EKF of Distributed Autonomous Orbit Determination[J]. Geomatics and Information Science of Wuhan University, 2013,38(4):399-402 [5] Kaipio J,Somersalo E. Nonstationary Inverse Problems and State Estimation[J]. Journal of Inverse Ill-Posed Problems, 1998, 7(3):273-282 [6] Baroudi D, Kaipio J, Somersalo E. Dynamical Electric Wire Tomography:Time Series Approach[J]. Inverse Problems, 1998, 14(4):799-813 [7] Berrier K L, Sorensen D C, Khoury D S. Solving the Inverse Problem of Electrocardiography Using a Duncan and Horn Formulation of the Kalman Filter[J]. IEEE Transactions on Biomedical Engineering, 2004, 51(3):507-515 [8] Derich R,Calder J,Descoteaux M. Optimal Real-Time Q-ball Imaging Using Regularized Kalman Filtering with Incremental Orientation Sets[J]. Medical Image Analysis, 2009, 13(4):564-579 [9] Tan Jiajia, Li Dan, Zhang Jianqing, et al. Biased Kalman Filter[C]. International Conference on Sensing Technology, Palmerston North, 2011 [10] 秦永元,张洪钺, 汪叔华.卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社, 201) Qin Yongyuan, Zhang Hongyue, Wang Shuhua. Theory of Kalman Filter and Integrated Navigation Principles[M]. Xi'an:Northwestern Polytechnical University Press, 2012 [11] 付梦印,邓志红,闫莉萍.Kalman滤波理论及其在导航系统中的应用[M].北京:科学出版社, 2010 Fu Mengyin, Deng Zhihong, Yan Liping. Theory of Kalman Filter and Its Application in Navigation System[M].Beijing:Science Press, 2010 [12] 王松桂.线性模型的理论及其应用[M].合肥:安徽教育出版社, 1987 Wang Songgui. The Theory of Linear Model and Its Application[M]. Hefei: Anhui Education Press, 1987 [13] 顾勇为.基于复共线性诊断的正则化方法及其在大地测量中的应用[D].郑州:信息工程大学, 2010 Gu Yongwei. Regularization Methods Based on Multicollinearity Diagnosis and Their Applications to Geodesy[D]. Zhengzhou:Information Engineering University, 2010 -