卡尔曼滤波(KF),扩展卡尔曼滤波(EKF)

卡尔曼滤波(KF),扩展卡尔曼滤波(EKF)

声明:本文仅作为个人复习与分享,里面很多内容是对其他博文的整理,在文章中均有注明。本人小白一枚,文章中如有错误,希望各位提醒!

1.卡尔曼滤波(KF)

1.1 卡尔曼滤波的模型假设
Kalman滤波所解决的问题,是对一个动态变化的系统的状态跟踪的问题,基本的模型假设包括:
1.系统的状态方程是线性
2.观测方程是线性
3.过程噪声符合零均值高斯分布
4.观测噪声符合零均值高斯分布

状态方程
x t = A t x t − 1 + B t u t + W t x_{t}=A_{t}x_{t-1}+B_{t}u_{t}+W _{t} xt=Atxt1+Btut+Wt W t ∼ N ( 0 , Q t ) W_{t} \sim N(0,Q_{t}) WtN(0,Qt)
观测方程
z t = H t x t + V t z_{t}=H_{t}x_{t}+V{t} zt=Htxt+Vt V t ∼ N ( 0 , R t ) V_{t} \sim N(0,R_{t}) VtN(0,Rt)

1.2 卡尔曼滤波算法

预测过程
x ^ t ′ = A t x ^ t − 1 + B t u t \hat x_{t}^{'}=A_{t}\hat x_{t-1}+B_{t}u_{t} x^t=Atx^t1+Btut 预测t时刻的状态
P t ′ = A t P t − 1 A t T + Q t P_{t}^{'}=A_{t}P_{t-1}A_{t}^{T}+Q_{t} Pt=AtPt1AtT+Qt 预测方差矩阵
更新过程
K t = P t ′ H t T ( H t P t ′ H t T + R t ) − 1 K_{t}=P_{t}^{'}H_{t}^{T}(H_{t}P_{t}^{'}H_{t}^{T}+R_{t})^{-1} Kt=PtHtT(HtPtHtT+Rt)1 计算卡尔曼增益
x ^ t = x ^ t ′ + K t ( z t − H t x ^ t ′ ) \hat x_{t}=\hat x_{t}^{'}+K_{t}(z_{t}-H_{t}\hat x_{t}^{'}) x^t=x^t+Kt(ztHtx^t) 基于观测进行状态更新
P t = ( I − K t H t ) P t ′ P_{t}=(I-K_{t}H_{t})P_{t}^{'} Pt=(IKtHt)Pt 计算更新状态的方差矩阵

其中,
x ^ t \hat x_{t} x^t, n维向量,表示t时刻卡尔曼估计值
x ^ t ′ \hat x_{t}^{'} x^t, n维向量,表示t时刻预测值
P t P_{t} Pt, n ∗ n n*n nn方差矩阵,表示t时刻被观测的n个状态的方差
P t ′ P_{t}^{'} Pt , n ∗ n n*n nn方差矩阵,表示t时刻预测的n个状态的方差
u t u_{t} ut, l 维向量,表示t时刻的输入
z t z_{t} zt, m维向量,表示t时刻的观测
A t A_{t} At, m ∗ m m*m mm矩阵,表示状态转移矩阵
B t B_{t} Bt, m ∗ n m*n mn矩阵,表示输入增益矩阵
H t H_{t} Ht, m ∗ n m*n mn矩阵,表示观测矩阵
Q t Q_{t} Qt, n ∗ n n*n nn矩阵,表示过程噪音 W t W _{t} Wt的方差矩阵
R t R_{t} Rt, m ∗ m m*m mm矩阵,表示观测噪音 V t V_{t} Vt的方差矩阵

注:本节引用了 http://www.cnblogs.com/ycwang16/p/5999034.html
1.3 推导过程
真实值与预测值之间的误差为
e t ′ = x t − x ^ t ′ e_{t}^{'}=x_{t}- \hat x_{t}^{'} et=xtx^t
预测误差协方差矩阵为
P t ′ = E [ e t ′ e t ′ T ] = E [ ( x t − x ^ t ′ ) ( x t − x ^ t ′ ) T ] P_{t}^{'}=E[e_{t}^{'}e_{t}^{'T}]=E[(x_{t}- \hat x_{t}^{'})(x_{t}- \hat x_{t}^{'})^{T}] Pt=E[etetT]=E[(xtx^t)(xtx^t)T]
真实值与卡尔曼估计值之间的误差为:
e t = x t − x ^ t = x t − ( x ^ t ′ + K t ( H x t + V t − H x ^ t ′ ) ) = ( I − K t H ) ( x t − x ^ t ′ ) − K t V t e_{t}=x_{t} - \hat x_{t}=x_{t}-(\hat x_{t}^{'}+K_{t}(Hx_{t}+V_{t}-H\hat x_{t}^{'})) \\=(I-K_{t}H)(x_{t} - \hat x_{t}^{'})-K_{t}V_{t} et=xtx^t=xt(x^t+Kt(Hxt+VtHx^t))=(IKtH)(xtx^t)KtVt
e t e_{t} et代入卡尔曼估计误差协方差矩阵 P t = E [ e t e t T ] P_{t}=E[e_{t}e_{t}^{T}] Pt=E[etetT]
P t = E [ [ ( I − K t H ) ( x t − x ^ t ′ ) − K t V t ] [ ( I − K t H ) ( x t − x ^ t ′ ) − K t V t ] T ] = ( I − K t H ) E [ ( x t − x ^ t ′ ) ( x t − x ^ t ′ ) T ] ( I − K t H ) T + K t E [ V t V t T ] K t T P_{t}=E[[(I-K_{t}H)(x_{t} - \hat x_{t}^{'})-K_{t}V_{t}][(I-K_{t}H)(x_{t} - \hat x_{t}^{'})-K_{t}V_{t}]^{T}] \\ =(I-K_{t}H)E[(x_{t} - \hat x_{t}^{'})(x_{t} - \hat x_{t}^{'})^{T}](I-K_{t}H)^{T}+K_{t}E[V_{t}V_{t}^{T}]K_{t}^{T} Pt=E[[(IKtH)(xtx^t)KtVt][(IKtH)(xtx^t)KtVt]T]=(IKtH)E[(xtx^t)(xtx^t)T](IKtH)T+KtE[VtVtT]KtT
其中 E [ V t V t T ] = R t E[V_{t}V_{t}^{T}]=R_{t} E[VtVtT]=Rt,将预测误差协方差矩阵代入,得到
P t = ( I − K t H ) P t ′ ( I − K t H ) T + K t R t K t T P_{t}=(I-K_{t}H)P_{t}^{'}(I-K_{t}H)^{T}+K_{t}R_{t}K_{t}^{T} Pt=(IKtH)Pt(IKtH)T+KtRtKtT

卡尔曼滤波本质是最小均方差估计,而均方差是 P t P_{t} Pt的迹,于是我们有
t r ( P t ) = t r ( P t ′ ) − 2 t r ( K t H P t ′ ) + t r ( K t ( H P t ′ H T + R t ) K t T ) tr(P_{t})=tr(P_{t}^{'})-2tr(K_{t}HP_{t}^{'})+tr(K_{t}(HP_{t}^{'}H^{T}+R_{t})K_{t}^{T}) tr(Pt)=tr(Pt)2tr(KtHPt)+tr(Kt(HPtHT+Rt)KtT)
最优估计 K t K_{t} Kt使 t r ( P t ) tr(P_{t}) tr(Pt)最小,所以上式两边对 K t K_{t} Kt求导
t r ( P t ) ∂ K t = − ∂ 2 t r ( K t H P t ′ ) ∂ K t + ∂ t r ( K t ( H P t ′ H T + R t ) K t T ) ∂ K t \frac{tr(P_{t})}{\partial K_{t}}=-\frac{\partial 2tr(K_{t}HP_{t}^{'})}{\partial K_{t}}+\frac{\partial tr(K_{t}(HP_{t}^{'}H^{T}+R_{t})K_{t}^{T})}{\partial K_{t}} Kttr(Pt)=Kt2tr(KtHPt)+Kttr(Kt(HPtHT+Rt)KtT)

利用矩阵微分公式(1)(2)
∂ t r ( A B ) ∂ A = B T ( 1 ) \frac{\partial tr(AB)}{\partial A}=B^{T} (1) Atr(AB)=BT(1)
∂ t r ( A B A T ) ∂ A = 2 A B ( 2 ) \frac{\partial tr(ABA^{T})}{\partial A}=2AB (2) Atr(ABAT)=2AB(2)
可以得到
$$ t r ( P t ) ∂ K t = − 2 ( H P t ′ ) T + 2 K t ( H P t ′ H T + R t ) \frac{tr(P_{t})}{\partial K_{t}}=-2(HP_{t}^{'})^{T}+2K_{t}(HP_{t}^{'}H^{T}+R_{t}) Kttr(Pt)=2(HPt)T+2Kt(HPtHT+Rt)
另上式等于0,可以得到卡尔曼增益:
K t = P t ′ H t T ( H t P t ′ H t T + R t ) − 1 K_{t}=P_{t}^{'}H_{t}^{T}(H_{t}P_{t}^{'}H_{t}^{T}+R_{t})^{-1} Kt=PtHtT(HtPtHtT+Rt)1
下面我们计算 P t ′ P_{t}^{'} Pt
P t ′ = E [ ( x t − x ^ t ′ ) ( x t − x ^ t ′ ) T ] = E [ ( A x t − 1 + B u t + w t − A x ^ t − 1 − B u t ) ( A x t − 1 + B u t + w t − A x ^ t − 1 − B u t ) T ] = E [ ( A ( x t − 1 − x ^ t − 1 ) + w t ) ( A ( x t − 1 − x ^ t − 1 ) + w t ) T ] = E [ ( A ( e t − 1 ) ( A ( e t − 1 ) T ] + E [ w t w t T ] = A P t − 1 A T + Q P_{t}^{'}=E[(x_{t}- \hat x_{t}^{'})(x_{t}- \hat x_{t}^{'})^{T}] \\= E[(Ax_{t-1}+Bu_{t}+w_{t}-A\hat x_{t-1}-Bu_{t})(Ax_{t-1}+Bu_{t}+w_{t}-A\hat x_{t-1}-Bu_{t})^{T}] \\= E[(A(x_{t-1}-\hat x_{t-1})+w_{t})(A(x_{t-1}-\hat x_{t-1})+w_{t})^{T}] \\=E[(A(e_{t-1})(A(e_{t-1})^{T}]+E[w_{t}w_{t}^{T}] \\ =AP_{t-1}A^{T}+Q Pt=E[(xtx^t)(xtx^t)T]=E[(Axt1+But+wtAx^t1But)(Axt1+But+wtAx^t1But)T]=E[(A(xt1x^t1)+wt)(A(xt1x^t1)+wt)T]=E[(A(et1)(A(et1)T]+E[wtwtT]=APt1AT+Q

以上推导参考文章:https://blog.csdn.net/victor_zy/article/details/82862904

2.扩展卡尔曼滤波(EKF)

围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上项,得到一个近似的线性化模型,然后应用卡尔曼滤波完成状态估计。
系统方程
x t = f ( x t − 1 ) + W t x_{t}=f(x_{t-1})+W_{t} xt=f(xt1)+Wt
z t = h ( x t − 1 ) + V t − 1 z_{t}=h(x_{t-1})+V_{t-1} zt=h(xt1)+Vt1

在扩展卡尔曼滤波中,状态的预测以及观测值的预测由非线性函数计算得出,线性卡尔曼滤波中的状态转移矩阵A阵和观测矩阵H阵由f和h函数的雅克比矩阵代替,假设状态有n维,则求法如下:

卡尔曼滤波(KF),扩展卡尔曼滤波(EKF)_第1张图片
EKF的局限性:

  1. 当强非线性时EKF违背局部线性假设,当Taylor展开式中被忽略的高阶项带来的误差较大时,EKF可能会使结果发散;
    2.由于EKF在线性化处理时需要用Jacobian矩阵,计算过程繁琐。所以,在满足线性系统、高斯白噪声、所有随机变量服从高斯(Gaussian)分布这3个假设条件时,EKF是最小方差准则下的次优滤波器,其性能依赖于局部非线性度。
    注:本节文章参考
    作者:回忆不能已 https://blog.csdn.net/qq_18163961/article/details/52505591
    作者: Rap_God https://blog.csdn.net/u012936940/article/details/77249245

参考
[1] http://www.cnblogs.com/ycwang16/p/5999034.html
[2] https://blog.csdn.net/victor_zy/article/details/82862904
[3] https://blog.csdn.net/u010720661/article/details/63253509

你可能感兴趣的