卡尔曼滤波算法研究

毛秀华，吴 健

(中国电子科技集团公司第五十一研究所，上海 201802)



卡尔曼滤波算法研究

毛秀华，吴 健

(中国电子科技集团公司第五十一研究所，上海 201802)

对卡尔曼滤波的起源和发展进行了简述，然后对标准卡尔曼滤波的定义和模型进行了回顾，重点对近似二阶扩展卡尔曼滤波、扩维无迹卡尔曼滤波和自适应卡尔曼滤波等3种最新改进型的卡尔曼滤波算法进行了详细阐述，最后对这3种新改进型的卡尔曼滤波算法的优缺点进行了对比分析，对各自的适用领域和场景进行了说明。

卡尔曼滤波；近似二阶扩展卡尔曼滤波；无迹卡尔曼滤波；自适应卡尔曼滤波

0 引 言

卡尔曼滤波器最早要追寻到1960年卡尔曼先生发表的关于利用递归算法来求解离散线性滤波器问题的学术论文——《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预测问题的新方法)，在卡尔曼先生的这篇学术论文中首次提出了针对维纳滤波器缺点的全新解决方案，这种方案就是时至今日被人们所熟知的卡尔曼滤波方法。卡尔曼滤波方法功能非常强大，应用也十分广泛，利用卡尔曼滤波方法不仅可以预测信号当前、过去的状态，还可以预测信号的下一步即将来的状态，这种预测和估计可以在不知道系统确切模型的条件下完成[1]。

最小均方误差被用来作为卡尔曼滤波器估计的准则，在完成对信号状态变量的估计时，信号与噪声的状态空间模型是卡尔曼滤波算法首先使用的一个模型，在进行状态变量的估计时，利用的是当前时刻的估计值和当前时刻的观测值。

卡尔曼滤波算法在各行业中的应用已经有30多年的历史了，实践证明卡尔曼滤波器在解决相关问题时，它是最优的解决方案，有时甚至是效率最高最可行的解决方案。卡尔曼滤波算法现在应用得十分广泛，在电子、控制、导航、无线传感器网络等多个领域均有应用，在雷达跟踪、无源定位跟踪、导弹追踪等军事方面，卡尔曼滤波算法均有不错的应用并取得了不俗的成绩。

文章首先对标准卡尔曼滤波的定义、数学模型进行了简单介绍，然后重点对3种最新改进型的卡尔曼滤波算法进行了阐述，对近似二阶扩展卡尔曼滤波、扩维无迹卡尔曼滤波和自适应卡尔曼滤波这3种算法的优缺点进行了分析和说明，对各自的应用场景进行了阐述。

1 标准卡尔曼滤波算法

为了说明卡尔曼滤波算法的基本原理，首先假设物理系统的状态更新过程为一个离散时间的随机过程。被控制对象的输入对物理系统的状态会产生影响，噪声对物理系统的观测过程产生影响，假定物理系统的状态是非直接可观测的。在以上假设前提下，得到系统的状体方程和观测方程[2]：

(1)

式中:Xk为状态向量;Lk为观测向量;Φk,k-1为状态转移矩阵;Uk-1为控制向量,一般不考虑;Γk,k-1、Bk为系数矩阵;Ωk-1为系统动态噪声向量;Δk为观测噪声向量，其随机模型为:

(2)

(3)

(4)

卡尔曼滤波递推公式为:

(5)

(6)

(7)

(8)

(9)

2 几种最新改进型的卡尔曼滤波算法

(1) 近似二阶扩展卡尔曼滤波算法

从前面的介绍和阐述可知，标准卡尔曼滤波算法主要针对的是线性系统，而在实际的工程运用中需要解决的问题多数是非线性系统，这时候标准卡尔曼滤波算法就失去了效能，在这种背景下20世纪70年代扩展卡尔曼滤波算法(EKF)被提出来了，扩展卡尔曼滤波算法针对的是非线性系统，目前在非线性系统估计方面已经有了十分广泛的应用。近似二阶扩展卡尔曼滤波方法(AS-EKF)采用的框架是线性最小方差框架，采用均值的二阶近似变换来获取非线性系统的递推滤波框架。该滤波基于线性最小方差递推框架，状态X的最小方差估计为[3]：

(10)

(11)

得到最优估计和估计误差方差阵的递推方程分别为：

E(X(k+1)/L1k+1)=E(X(k+1)/L(k+1),L1k)

(12)

var(X(k+1)/L1k+1)=var(X(k+1)/L(k+1),L1k)

(13)

在EKF中,假设非线性函数y=f(X)在状态X的最优估计(预测)值处线性化,即:

(14)

y的均值、方差和协方差的近似估计:

(15)

对均值进行二阶近似,而对方差和协方差进行一阶近似,即可得:

(16)

式中：Λ为梯度函数。

考虑如下带加性噪声的非线性离散系统:

X(k+1)=f(X(k),k)+Γ(X(k),k)V(k)

(17)

L(k+1)=h(X(k+1),k+1)+W(k+1)

(18)

将式(16)所使用的近似二阶方法代入式(12)和(13)，可得如下近似二阶卡尔曼滤波递推公式。

预测阶段：

(19)

(20)

其中:

(21)

(22)

更新阶段：

K(k)=

(23)

(24)

P(k+1)= (I-K(k+1)Hk+1)P(k+1|k)·

(I-K(k+1)Hk+1)T+

K(k+1)R(k+1)KT(k+1)

(25)

(2) 扩维无迹卡尔曼滤波算法

利用无迹变换和卡尔曼滤波算法的线性滤波框架建立起来的一种改进型卡尔曼滤波算法叫无迹卡尔曼滤波(UKF)。UKF的状态向量概率密度函数是通过非线性状态方程来估算得到，无迹卡尔曼滤波存在的不足是当系统的噪声较大时，无迹卡尔曼滤波并不能得到精确的卡尔曼滤波结果。

为了克服无迹卡尔曼滤波的这一缺点，改进型无迹卡尔曼滤波在初始状态中添加过程和测量噪声，这样采样点就包含了这些噪声，从而保证了噪声能够在非线性系统中传输和估计，从而使得无迹卡尔曼滤波的结果能够更好地接近真实值，在系统噪声和观测噪声较大的条件下改进型无迹卡尔曼滤波应用更加有效[4]。无迹卡尔曼滤波算法如下：

设非线性系统模型为:

(26)

式中：X(k)为系统k时刻的n维状态向量；L(k)为系统k时刻的测量向量；f(·),h(·)为非线性变换；过程噪声V(k)和测量噪声W(k)是零均值。

初始状态为:

(27)

(28)

状态变量为:

(29)

状态方差为:

(30)

此时的采样点集变为{Xai(k|k),i=0,1,…,2N,N=(n+q+m)}。

时间更新过程变为:

xx(k/k-1)=f(xx(k-1/k-1),k-1)+

xx(k-1)

(31)

(32)

(33)

(34)

(35)

测量更新过程变为：

(36)

(37)

(38)

(39)

(40)

在本算法中因为状态按照扩展维数来计算的，Sigma采样点的个数为2(n+q+m) +1,维数为N×(2N+1)；当不扩维时,Sigma采样点的个数为2n+1,维数为n×(2n+1)[5]。

(3) 自适应卡尔曼滤波算法

从前面的介绍可见，传统卡尔曼滤波算法的限制条件较多，对系统模型有一定的依赖性，当系统模型存在误差时，直接影响到系统的滤波效果。针对上述问题，很多学者提出了不同的改进方案，其中自适应卡尔曼滤波算法是最有效的解决方案之一[6]。

(a) 多模型自适应卡尔曼滤波算法

(41)

(b) 基于信息的自适应卡尔曼滤波算法

多模型自适应卡尔曼滤波和基于信息的卡尔曼滤波是自适应卡尔曼滤波的2个最主要的研究方向[8]。基于信息的卡尔曼滤波的研究方法主要分为2类:Q(k)和R(k)是通过信息估计得到的,进而完成对增益矩阵J(k)的控制，J(k)的自适应估计是通过信息来实现。

(42)

(43)

(44)

多模型自适应卡尔曼滤波算法估计了噪声的方差矩阵和噪声的均值，该方法适用于观测噪声为有色噪声的高动态系统。多模型自适应卡尔曼滤波的最大缺点是计算复杂，计算量偏大，不利于实时处理。基于信息的自适应卡尔曼滤克服了第1种方法的不足，计算量较小，但是计算过程中涉及到矩阵求逆，操作也不是十分方便，但是这种方法适用于噪声统计特性不明确或噪声发生变换的场景[9]。

3 几种卡尔曼滤波算法的比较与分析

(1) 近似二阶卡尔曼滤波算法

近似二阶卡尔曼滤波算法是一种典型的递推非线性滤波算法，参阅相关文献可知，近似二阶卡尔曼滤波算法在计算量稍微大于扩展卡尔曼滤波算法的基础上，近似二阶卡尔曼滤波算法的滤波效果明显强于扩展卡尔曼滤波算法，而且此时的近似二阶卡尔曼滤波算法的计算量明显低于无迹卡尔曼滤波算法。综上所述，近似二阶卡尔曼滤波算法适用于滤波精度和实时性要求较高的场合。

近似二阶卡尔曼滤波算法也并非绝对完美，它也存在有许多的不足，近似二阶卡尔曼滤波算法要利用雅克比矩阵来完成对系统的线性化处理,但是在实际应用中非线性函数的雅克比矩阵确实非常难得到，在无法得到雅克比矩阵的情况下，系统误差就会很大，而且还会导致滤波器的不稳定。近似二阶卡尔曼滤波算法适用于对估计精度和计算量都有所要求的非线性系统滤波器设计。

(2) 扩维无迹卡尔曼滤波算法

UKF是通过无迹变换而来，是通过线性KF框架得到的。无迹卡尔曼滤波是通过非线性状态方程去估计状态向量的概率密度函数，相比扩展卡尔曼滤波，无迹卡尔曼滤波的精度提高了许多，而且无迹卡尔曼滤波对参数不敏感，无迹卡尔曼滤波的鲁棒性非常好，无迹卡尔曼滤波非常适用于非线性系统。

无迹卡尔曼滤波算法也有其不足，那就是当系统中噪声较大时，滤波结果并不是十分的精确，而改进无迹卡尔曼滤波算法对系统噪声和过程噪声进行了采集，将采集到的噪声和采样点进行集中，从而使得滤波精度和灵敏度都得到了提高。扩维无迹卡尔曼滤波是无损变换和标准卡尔曼滤波的有机结合体，它适用于导航、目标跟踪、信号处理和神经网络学习等多种场合。

(3) 自适应卡尔曼滤波

由自适应卡尔曼滤波公式可以看到，滤波方程递推公式的计算过程是一个连续的预测和修正过程。在计算过程中，可以计算速度参数，校正由干扰引起的突变，与实际变形趋势更加吻合。当得到新的观测数据时，可以计算出实时处理观测结果的新的滤波值，它将参数估计和预测有机地结合起来，因此，自适应卡尔曼滤波特别适合于变形监测的动态数据处理。

自适应卡尔曼滤波也有缺点，它是基于过去所有观测值的条件期望，这些观测值的权值被处理成一致的，因此随着时间的推移，当采集到的数据越来越多，从新的数据中获得的信息量将相对下降，算法逐渐失去了修正的能力，参数估计可能偏离真实值较远而不能更新[10]。自适应卡尔曼滤波将系统辨识与滤波估计有机地结合到一起，它对动态系统的滤波非常有效。

4 结束语

卡尔曼滤波算法作为研究热点已经持续多年，对卡尔曼滤波算法的改进也非常多，除本文介绍的几种方法以外，还有基于小波变换的卡尔曼滤波算法、基于自适应采样平方根的无迹卡尔曼滤波算法、双无迹卡尔曼滤波算法等。这些改进型卡尔曼滤波算法适用场景各不相同，适用条件也千变万化，一种完美、通用的卡尔曼滤波算法是不存在的，很多的卡尔曼滤波算法都是在彼此基础上改进而来，大家是相辅相成，相互支撑的，具体选用那种算法，需要由实际应用场景来决定。

[1] 黄小平，王岩．卡尔曼滤波原理及应用[M]．北京:电子工业出版社，2015．

[2] GAO S S，HU G G，ZHONG Y M.Windowing and random weighting-based adaptive unscented Kalman filter[J].International Journal of Adaptive Control and Signal Processing，2014，29(2):201-223．

[3] GUSTAFSSON F,HENDEBY G.Some relations between extended and unscented Kalman filters[J]．IEEE Transactions on Signal Process,2012,60(2):545- 555.

[4] MEDEIROS C B,WANDERLEY M M.Multiple-model linear Kalman filter framework for unpredictable signals[J].IEEE Sensors Journal,2014,14(4):979-991.

[5] CHANG G.Kalman filter with both adaptivity and robustness[J].Process Control,2014,24(3):81-87.

[6] CHANG L,HU B,LI A,QIN F.Transformed unscented Kalman filter[J]．IEEE Transactions on Automatic Control,2013,58(1):252-257.

[7] HORWOOD J T,POORE A B.Adaptive Gaussian sum filtersfor space surveillance[J]．IEEE Transactions on Automatic Control,2011,56(8):1777-1790.

[8] HUR H,AHN H S.Discrete-time H∞ filtering for mobile robotlocalization using wireless sensor network[J]．IEEE Sensors Journal,2013,13(1):245-252.

[9] KARLGAARD C D.Nonlinear regression Huber-Kalman filtering andfixed-interval smoothing[J]．Journal of Guidance,Control and Dynamics,2014,38(2):322- 330.

[10]MBALAWATA I S,SRKKS,VIHOLA M,HAARIO H.Adaptive Metropolis algorithm using variational Bayesian adaptive Kalman filter[J].Computer Statist and Data Analysis,2015,83(1):101-115.

ResearchintoKalmanFilteringAlgorithm

MAO Xiu-hua,WU Jian
(51st Research Institute of CETC,Shanghai 201802,China)

This paper expatiates the origin and development of Kalman filtering briefly,then reviews the definition and model of the standard Kalman filtering,focuses on expatiating three new improved Kalman fitering algorithms in detail:approximate two order extended Kalman filtering,augmented unscented Kalman filtering,adaptive Kalman filtering,finally compares and analyzes the advantages and disadvantages of three new improved Kalman filtering algorithms,performs illumination to respective application fields and scene.

Kalman filtering；approximate two order extended Kalman filtering；unscented Kalman filtering；adaptive Kalman filtering

2017-03-30

TN713

：A

：CN32-1413(2017)03-0064-05

10.16426/j.cnki.jcdzdk.2017.03.015