卡尔曼滤波研究综述
1 卡尔曼滤波简介
1.1卡尔曼滤波的由来
1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文-《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。
其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。
对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
1.2标准卡尔曼滤波-离散线性卡尔曼滤波
为了描述方便我们作以下假设：物理系统的状态转换过程可以描述为一个离散时间的随机过程；系统状态受控制输入的影响；系统状态及观测过程都不可避免受噪声影响；对系统状态是非直接可观测的。在以上假设前提下，得到系统的状体方程和观测方程。
X ⎧⎨ 1-1
式中:X k 为状态向量,L k 为观测向量,Φk,k-1为状态转移矩阵,U k-1为控制向量,一
般不考虑,Γk,k-1,B k 为系数矩阵,Ωk-1为系统动态噪声向量,Δk 为观测噪声向量,其
随机模型为
E(Ωk ) =0;E(Δk ) =0;cov(Ωk ,Ωj ) = D Ω(k )δkj ,
cov(Δk ,Δj ) = D k (k )δkj ;cov(Ωk ,Δj ) =0;E(X 0) =μx(0)
var(X 0) = D(X 0);cov(X 0,Ωk ) =0;cov(X 0,Δk ) =0. 1-2
卡尔曼滤波递推公式为
X ∧(k/k) = X ∧(k/k-1)+J k (L k -B k X ∧(k/k-1)),
D(k/k) = (E-J k B k )D x (k/k-1),
J k = D x (k/k-1)BT k [B k D x (k/k-1)]B T k +D Δ(k)]-1,
X ∧
(k/k-1) =Φk ,k-1X ∧
(k-1/k-1), D x (k/k-1) =Φk ,k-1D x (k-1/k-1)ΦT k ,k-1+Γk ,k-1D Δ(k-1)ΓT k ,k-1. 1-3
2 几种最新改进型的卡尔曼滤波算法。
2.1 近似二阶扩展卡尔曼滤波
标准的卡尔曼滤波只适用于线性系统，而工程实际问题涉及的又大多是非
线性系统，于是基于非线性系统线性化的扩展卡尔曼滤波(EKF)在上世纪70年代
被提出,目前已经成为非线性系统中广泛应用的估计方法。近似二阶扩展卡尔曼
滤 波方法(AS-EKF)基于线性最小方差递推滤波框架,应用均值变换的二阶近似从
而得到非线性系统的递推滤波滤波框架
该滤波基于线性最小方差递推框架，状态X 的最小方差估计为
(/)X E X L Λ
= 2-1-1 L 是观测矩阵，假定状态X 的估计值X Λ
是观测矩阵L 的线性函数，即
()X L AL b Λ=+ 2-1-2 得到最优估计和估计误差方差阵的递推方程分别为：
11111111((1)/)((1)/(1),)
((1)/)((1),(1)/)(((1)/))((1)((1)/))k k k k k K E X k L E X k L k L E X k L COV X k L k L Var L k L L k E L k L +-+=++=++++++-+ 2-1-2 11111111((1)/)((1)/(1),)
((1)/)((1),(1)/)(((1)/))((1),((1)/)k k k k k T K Var X k L Var X k L k L Var X k L COV X k L k L Var L k L Cov X k L k L +-+=++=+-+++++ 2-1-3
2.1.2近似二阶扩展卡尔曼滤波器的设计
在EKF 中,假设非线性函数y=f(X)在状态X 的最优估计(预测)值处线性化,即
()y f X D f α-
≈+ 2-1-4 y 的均值、方差和协方差的近似估计
()T yy X XX X T Xy XX X y f X P A P A P P A -
-⎧≈⎪⎪≈⎨⎪≈⎪⎩
2-1-5
对均值进行二阶近似,而对方差和协方差进行一阶近似,即可得
1()[)()]2T XX X X T yy X XX X T Xy XX X
y f X P f X P A P A P P A ---=⎧⎪≈+∇∇⎪⎪≈⎨⎪≈⎪⎪⎩
2-1-6 考虑如下带加性噪声的非线性离散系统
X(k+1) =f(X(k),k) +Γ(X(k),k)V(k)
L(k+1) =h(X(k+1),k+1) +W(k+1)
将式2-1-6 所使用的近似二阶方法代入2-1-2，和2-1-3，可得如下近似二阶卡尔曼滤波递推公式。
预测阶段：
X Λ (k+1|k)=f(X Λ (k),k)+1/2[( ▽T P(k) )f(X,k)] | (X= X
Λ (k)) 2-1-7 P(k+1|k)=Φk/k-1P(k)ΦT k/k-1+Γ(X(k),k)Q(k)ΓT (X(k),k)
L Λ (k+1|k)=h(X Λ (k+1|k),k+1)+1/2(▽ T P(k+1|k) ▽)h(X,k+1) |X=X
Λ (k+1|k)
更新阶段：
K(k+1) =P(k+1|k)H T k+1(H k+1P(k+1|k)H T k+1+R(k+1))-1 2-1-8 X Λ (k+1) =X Λ (k+1|k)+K(k+1)[L(k+1)- L Λ (k+1k)]
P(k+1) = (I-K(k+1)H k+1)P(k+1|k)(I-K(k+1)H k+1)T +K(k+1)R(k+1)K T (k+1) 其中1[(),]()
k f X k k X h -∂Φ=∂|X(k)= X Λ， 1[(1),1](1)k h X k k H X k +∂++=∂+|X(k+1)= X Λ(k+1|k) Q(k)为系统噪声序列V(k)的方差阵,R(k)为测量噪声序列W(k)的方差阵。
2.2扩维无迹卡尔曼滤波
无迹卡尔曼滤波(Unscented Kalman Filter,UKF),它是在以无迹变换(Unscented Transformation,UT)为基础,借用卡尔曼线性滤波框架而建立起来的。它直接利用非线性状态方程来估算状态向量的概率密度函数。但是,简单的UKF 在面对系统中的噪声影响较大时不能得到精确的滤波结果, 改进的无迹卡尔曼滤波算法则是在初始状态中引入过程噪声和测量噪声,使得采样点也包括了
