卡尔曼滤波研究综述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-32 几种最新改进型的卡尔曼滤波算法。
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-32.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 Xy 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 在面对系统中的噪声影响较大时不能得到精确的滤波结果, 改进的无迹卡尔曼滤波算法则是在初始状态中引入过程噪声和测量噪声,使得采样点也包括了这些噪声,这样在状态预测和更新过程中,噪声的影响就能够在非线性系统中进行传输和估计,使得滤波信号更好地接近真实值,尤其是当信号的系统噪声和观测噪声影响较大时。
其算法如下：设非线性系统模型为:X(k) =f(X(k-1)) +V(k-1) 2-2-1L(k) =h(X(k)) +W(k)其中,X(k)是系统k 时刻的n 维状态向量,L(k)是系统k 时刻的测量向量,f(·),h(·)为非线性变换,过程噪声V(k)和测量噪声W(k)是零均值。
方差阵各为Q 和R 的不相关的高斯白噪声,其统计特性满足:E[W(k)] =0,E[V(k)] =0 2-2-3E[W(i)W(j)] =Rδij , ,i j ∀E[V(i)V(j)] =Qδij , ,i j ∀E[W(i)V(j)T ] =0初始状态为X ∧a (0|0) = [X ∧ (0|0)T 0 0]T 2-2-4(0/0)00(0/0)0(0/0)000(0/0)XX a T XX P P Q R ⎤⎥=⎥⎥⎦状态变量为 X ∧a (k |k) = [X ∧ (k |k)T V(k |k)T W(k |k)T ]T 2-2-5状态方差为 (/)00(/)0(/)000(/)XX a T XXP k k P k k Q k k R k k ⎤⎥=⎥⎥⎦ 2-2-6此时的采样点集变为{χa i (k |k), i =0,1,…,2N,N = (n +q+m)},q 为Q 的维数,m 为R 的维数,采样点的状态维数变为n+q+m,χx i 为χa i 的前n 维组成的列向量,χv i为χa i 的n+1维到n+q 维组成的列向量,χw i 为χa i 的n+q+1维到n+q+m 维组成的列向量。
时间更新过程变为(/1)((1/1),1)(1)x x x x k k f x k k k x k -=---+- 2-2-72()0(/1)(/1)Vi x m i i X k k W x k k ∧=-=-∑(/1)((/1),1)a x i k k h x k k k μ-=--2()0(/1)(/1),1)Vi m i i L k k W k k k μ∧=-=--∑2()0(/1)[((/1),1)(/1))((/1)(/1))]V i x x T XX m i ii P k k W x k k k X k k x k k X k k ∧∧=-=----⨯---∑测量更新过程变为：2()0(/1)[((/1)(/1))((/1)(/1))]V i x T XX m ii i P k k W x k k X k k k k L k k μ∧∧=-=---⨯---∑ 2()0(/1)[((/1)(/1))((/1)(/1))]V i T YY ci i i P k k W k k L k k k k L k k μμ∧∧=-=---⨯---∑ K(k) =P XL (k |k-1)P -1LL (k |k-1)X ∧ (k |k) =X ∧ (k |k-1) +K(k)(L(k) -Y ∧(k |k-1)) 2-2-8P XX (k |k) =P XX (k |k-1) -K(k)P LL (k |k-1)K T (k)由于状态是按扩维处理的,Sigma 采样点的个数为2(n+q+m) +1,维数为N ×(2N+1),而不扩维时,Sigma 采样点的个数为2n+1,维数为n ×(2n+1),所以,使用扩维计算量上升得比较快,当数据量过大时,耗费的计算时间长。