留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

有偏卡尔曼滤波及其算法

李永明 归庆明 顾勇为 韩松辉

李永明, 归庆明, 顾勇为, 韩松辉. 有偏卡尔曼滤波及其算法[J]. 武汉大学学报 ● 信息科学版, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072
引用本文: 李永明, 归庆明, 顾勇为, 韩松辉. 有偏卡尔曼滤波及其算法[J]. 武汉大学学报 ● 信息科学版, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072
LI Yongming, GUI Qingming, GU Yongwei, HAN Songhui. The Biased Kalman Filter and Algorithm[J]. Geomatics and Information Science of Wuhan University, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072
Citation: LI Yongming, GUI Qingming, GU Yongwei, HAN Songhui. The Biased Kalman Filter and Algorithm[J]. Geomatics and Information Science of Wuhan University, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072

有偏卡尔曼滤波及其算法

doi: 10.13203/j.whugis20140072
基金项目: 

国家自然科学基金 Nos. 40974009

国家自然科学基金 41174005

国家自然科学基金 41474009

详细信息
    作者简介:

    李永明,硕士,研究方向为动态测量数据处理。keepming@163.com

  • 中图分类号: P207

The Biased Kalman Filter and Algorithm

Funds: 

The National Natural Science Foundation of China Nos. 40974009

The National Natural Science Foundation of China 41174005

The National Natural Science Foundation of China 41474009

  • 摘要: 从理论角度分析了观测矩阵的复共线性对卡尔曼滤波的影响,并在均方误差最小意义下,给出了一种有偏卡尔曼滤波算法。分别对观测矩阵和观测量施加扰动进行了试验和分析,证明观测矩阵的病态性会对卡尔曼滤波估计造成严重危害。数值模拟结果表明,本文算法能够有效改善观测矩阵病态性对卡尔曼滤波估计的影响,提高解算质量。
  • 图  1  卡尔曼滤波估值 ${{\hat X}_k}$ (1)在扰动前后的比较

    Figure  1.  Comparison of Kalman Filter Estimate ${{\hat X}_k}$ (1)Before and After the Disturbance

    图  2  有偏卡尔曼滤波与卡尔曼滤波均方误差的比较

    Figure  2.  Comparison of Mean Square Error of Biased Kalman Filte and Kalman Filter

    图  3  有偏卡尔曼滤波和卡尔曼滤波

    中文注解

    Figure  3.  Comparison of Difference Value Between Biased Kalman Filter and Kalman Filter

    英文注解

    图  4  有偏卡尔曼滤波和卡尔曼滤波结果

    Figure  4.  Comparison of Euclidean Distance Between Biased Kalman Filter and 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
  • [1] 肖元弼, 彭认灿, 暴景阳, 董箭, 吕程.  利用卡尔曼滤波改正多波束数据声速整体误差 . 武汉大学学报 ● 信息科学版, 2020, 45(9): 1461-1468. doi: 10.13203/j.whugis20180261
    [2] 郭士荦, 吴苗, 许江宁, 李京书.  自适应渐消卡尔曼滤波及其在SINS初始对准中的应用 . 武汉大学学报 ● 信息科学版, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
    [3] 姜兆英, 刘国林, 于胜文.  病态方程基于Liu估计的一种迭代估计新方法 . 武汉大学学报 ● 信息科学版, 2017, 42(8): 1172-1178. doi: 10.13203/j.whugis20150218
    [4] 张炳先, 王密, 潘俊.  基于卡尔曼滤波的光学遥感影像高精度复原处理 . 武汉大学学报 ● 信息科学版, 2015, 40(7): 964-970. doi: 10.13203/j.whugis20130123
    [5] 郭杰, 归庆明, 郭淑妹, 张宁.  利用复共线性诊断确定偏差矫正项的截断型岭估计 . 武汉大学学报 ● 信息科学版, 2015, 40(6): 785-789. doi: 10.13203/j.whugis20130465
    [6] 柴华, 王勇, 许大欣, 王虎彪.  地固系下四元数和卡尔曼滤波方法的惯导初始精对准研究 . 武汉大学学报 ● 信息科学版, 2012, 37(1): 68-72.
    [7] 马洋, 欧吉坤, 袁运斌, 王海涛.  采用联合平差法处理附有病态等式约束的反演问题 . 武汉大学学报 ● 信息科学版, 2011, 36(7): 816-819.
    [8] 乐洋, 何秀凤.  具有区间隶属函数的模糊卡尔曼滤波在组合导航系统中的应用 . 武汉大学学报 ● 信息科学版, 2011, 36(4): 450-452.
    [9] 陆付民, 王尚庆, 李劲, 严学清.  顾及地下水位因子的卡尔曼滤波模型在滑坡变形预测中的应用 . 武汉大学学报 ● 信息科学版, 2010, 35(10): 1184-1187.
    [10] 高书亮, 李锐, 黄智刚.  利用卡尔曼滤波估计导航信号电离层差分改正 . 武汉大学学报 ● 信息科学版, 2010, 35(9): 1021-1023.
    [11] 刘繁明, 钱东, 郭静.  卡尔曼滤波地形反演算法的系统方程建模 . 武汉大学学报 ● 信息科学版, 2010, 35(10): 1179-1183.
    [12] 刘国林, 郝华东, 陶秋香.  卡尔曼滤波相位解缠及其与其他方法的对比分析 . 武汉大学学报 ● 信息科学版, 2010, 35(10): 1174-1178.
    [13] 孙红星, 付建红, 袁修孝, 唐卫明.  基于多历元递推最小二乘卡尔曼滤波方法的模糊度解算 . 武汉大学学报 ● 信息科学版, 2008, 33(7): 735-739.
    [14] 赵长胜, 陶本藻.  有色噪声作用下的卡尔曼滤波 . 武汉大学学报 ● 信息科学版, 2008, 33(2): 180-182.
    [15] 赵长胜, 陶本藻.  有色噪声作用下的抗差卡尔曼滤波 . 武汉大学学报 ● 信息科学版, 2007, 32(10): 880-882.
    [16] 徐天河, 杨元喜.  均方误差意义下正则化解优于最小二乘解的条件 . 武汉大学学报 ● 信息科学版, 2004, 29(3): 223-226.
    [17] 孙红星, 李德仁.  非线性系统中卡尔曼滤波的一种新线性化方法 . 武汉大学学报 ● 信息科学版, 2004, 29(4): 346-348,358.
    [18] 郭杭.  迭代扩展卡尔曼滤波用于实时GPS数据处理 . 武汉大学学报 ● 信息科学版, 1999, 24(2): 112-114,123.
    [19] 柳响林, 刘经南, 杜道生.  卡尔曼滤波可靠性分析及其在动态GPS定位中的应用 . 武汉大学学报 ● 信息科学版, 1997, 22(3): 234-236,244.
    [20] 邵建平, 沈鹤鸣, 宋光普.  抗SA影响的卡尔曼滤波技术 . 武汉大学学报 ● 信息科学版, 1994, 19(3): 274-279.
  • 加载中
图(4)
计量
  • 文章访问数:  1549
  • HTML全文浏览量:  60
  • PDF下载量:  261
  • 被引次数: 0
出版历程
  • 收稿日期:  2014-10-13
  • 刊出日期:  2016-07-05

有偏卡尔曼滤波及其算法

doi: 10.13203/j.whugis20140072
    基金项目:

    国家自然科学基金 Nos. 40974009

    国家自然科学基金 41174005

    国家自然科学基金 41474009

    作者简介:

    李永明,硕士,研究方向为动态测量数据处理。keepming@163.com

  • 中图分类号: P207

摘要: 从理论角度分析了观测矩阵的复共线性对卡尔曼滤波的影响,并在均方误差最小意义下,给出了一种有偏卡尔曼滤波算法。分别对观测矩阵和观测量施加扰动进行了试验和分析,证明观测矩阵的病态性会对卡尔曼滤波估计造成严重危害。数值模拟结果表明,本文算法能够有效改善观测矩阵病态性对卡尔曼滤波估计的影响,提高解算质量。

English Abstract

李永明, 归庆明, 顾勇为, 韩松辉. 有偏卡尔曼滤波及其算法[J]. 武汉大学学报 ● 信息科学版, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072
引用本文: 李永明, 归庆明, 顾勇为, 韩松辉. 有偏卡尔曼滤波及其算法[J]. 武汉大学学报 ● 信息科学版, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072
LI Yongming, GUI Qingming, GU Yongwei, HAN Songhui. The Biased Kalman Filter and Algorithm[J]. Geomatics and Information Science of Wuhan University, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072
Citation: LI Yongming, GUI Qingming, GU Yongwei, HAN Songhui. The Biased Kalman Filter and Algorithm[J]. Geomatics and Information Science of Wuhan University, 2016, 41(7): 946-951. doi: 10.13203/j.whugis20140072
  • 卡尔曼滤波自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是观测噪声序列。

      同时,WkVk满足

      (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)求导并令导数为零,得:

      当式中‖Xk2未知时,用 ${\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+12未知时,用 ${\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

      分析图 2~图 4可得出以下结论。

      1) 当观测矩阵的病态性不受控制时,卡尔曼滤波估计的均方误差和与真值之间的欧氏距离都很大,其解算质量很不理想。

      2) 在本文设计的算例中,由于所用模型为定常线性系统,所以卡尔曼滤波估计的均方误差是收敛的,并且收敛速度很快,大概在递推了15次左右就收敛了。

      3) 本文给出的有偏卡尔曼滤波算法在均方误差最小意义下,对标准的卡尔曼滤波作出了较大的改善,并且其状态估计与真值的差异性更小。所以新算法能够有效地克服病态性对状态估计的影响,较好地提高解算质量。

    • 目前,针对离散动态系统病态性的相关研究和文献较少。相对于静态的Gauss-Markov模型,离散动态系统中观测矩阵的病态性对卡尔曼滤波的影响机理更加复杂,找到合适的应对策略也更加艰难。本文给出了分析离散动态系统中观测矩阵的病态性影响的一种方法,并结合有偏估计给出了一种应对策略。Gauss-Markov模型中病态性问题的相关研究已经较为成熟,因此,可借鉴其相关理论和方法,从多角度分析并解决离散动态系统中的病态性问题。

参考文献 (13)

目录

    /

    返回文章
    返回