卡尔曼滤波(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$$ $W_t\sim N(0,Q_t)$
观测方程
$$z_t=H_t{x}_t+Vt$$ $V_t\sim N(0,R_t)$

1.2 卡尔曼滤波算法

预测过程
${\hat{x}}_t^{{}^{\prime }}=A_t{\hat{x}}_{t-1}+B_t{u}_t$ 预测t时刻的状态
$P_t^{{}^{\prime }}=A_t{P}_{t-1}{A}_t^T+Q_t$ 预测方差矩阵
更新过程
$K_t=P_t^{{}^{\prime }}{H}_t^T(H_t{P}_t^{{}^{\prime }}{H}_t^T+R_t{)}^{-1}$ 计算卡尔曼增益
${\hat{x}}_t={\hat{x}}_t^{{}^{\prime }}+K_t(z_t-H_t{\hat{x}}_t^{{}^{\prime }})$ 基于观测进行状态更新
$P_t=(I-K_t{H}_t)P_t^{{}^{\prime }}$ 计算更新状态的方差矩阵

其中，
${\hat{x}}_t$, n维向量，表示t时刻卡尔曼估计值
${\hat{x}}_t^{{}^{\prime }}$, n维向量，表示t时刻预测值
$P_t$,$n\ast n$方差矩阵，表示t时刻被观测的n个状态的方差
$P_t^{{}^{\prime }}$ ,$n\ast n$方差矩阵，表示t时刻预测的n个状态的方差
$u_t$, l 维向量，表示t时刻的输入
$z_t$, m维向量，表示t时刻的观测
$A_t$, $m\ast m$矩阵，表示状态转移矩阵
$B_t$, $m\ast n$矩阵，表示输入增益矩阵
$H_t$, $m\ast n$矩阵，表示观测矩阵
$Q_t$, $n\ast n$矩阵，表示过程噪音$W_t$的方差矩阵
$R_t$, $m\ast m$矩阵，表示观测噪音$V_t$的方差矩阵

注：本节引用了 http://www.cnblogs.com/ycwang16/p/5999034.html
1.3 推导过程
真实值与预测值之间的误差为
$e_t^{{}^{\prime }}=x_t-{\hat{x}}_t^{{}^{\prime }}$
预测误差协方差矩阵为
$P_t^{{}^{\prime }}=E[e_t^{{}^{\prime }}{e}_t^{{}^{\prime }T}]=E[(x_t-{\hat{x}}_t^{{}^{\prime }})(x_t-{\hat{x}}_t^{{}^{\prime }}{)}^T]$
真实值与卡尔曼估计值之间的误差为：
$$\begin{gathered} e_t=x_t-{\hat{x}}_t=x_t-({\hat{x}}_t^{{}^{\prime }}+K_t(H{x}_t+V_t-H{\hat{x}}_t^{{}^{\prime }})) \\ =(I-K_{t}H)(x_t-{\hat{x}}_t^{{}^{\prime }})-K_t{V}_t \end{gathered}$$
将$e_t$代入卡尔曼估计误差协方差矩阵$P_t=E[e_t{e}_t^T]$
$$\begin{gathered} P_t=E[[(I-K_{t}H)(x_t-{\hat{x}}_t^{{}^{\prime }})-K_t{V}_t][(I-K_{t}H)(x_t-{\hat{x}}_t^{{}^{\prime }})-K_t{V}_t{]}^T] \\ =(I-K_{t}H)E[(x_t-{\hat{x}}_t^{{}^{\prime }})(x_t-{\hat{x}}_t^{{}^{\prime }}{)}^T](I-K_{t}H{)}^T+K_{t}E[V_t{V}_t^T]K_t^T \end{gathered}$$
其中$E[V_t{V}_t^T]=R_t$,将预测误差协方差矩阵代入，得到
$P_t=(I-K_{t}H)P_t^{{}^{\prime }}(I-K_{t}H{)}^T+K_t{R}_t{K}_t^T$

卡尔曼滤波本质是最小均方差估计，而均方差是$P_t$的迹，于是我们有
$$tr(P_t)=tr(P_t^{{}^{\prime }})-2tr(K_{t}H{P}_t^{{}^{\prime }})+tr(K_t(H{P}_t^{{}^{\prime }}{H}^T+R_t)K_t^T)$$
最优估计$K_t$使$tr(P_t)$最小，所以上式两边对$K_t$求导
$$\frac{tr(P_t)}{\partial K_t}=-\frac{\partial 2tr(K_{t}H{P}_t^{{}^{\prime }})}{\partial K_t}+\frac{\partial tr(K_t(H{P}_t^{{}^{\prime }}{H}^T+R_t)K_t^T)}{\partial K_t}$$

利用矩阵微分公式(1)(2)
$$\frac{\partial tr(AB)}{\partial A}=B^T(1)$$
$$\frac{\partial tr(AB{A}^T)}{\partial A}=2AB(2)$$
可以得到
$$$$\frac{tr(P_t)}{\partial K_t}=-2(H{P}_t^{{}^{\prime }}{)}^T+2K_t(H{P}_t^{{}^{\prime }}{H}^T+R_t)$$
另上式等于0，可以得到卡尔曼增益：
$$K_t=P_t^{{}^{\prime }}{H}_t^T(H_t{P}_t^{{}^{\prime }}{H}_t^T+R_t{)}^{-1}$$
下面我们计算$P_t^{{}^{\prime }}$
$$\begin{gathered} P_t^{{}^{\prime }}=E[(x_t-{\hat{x}}_t^{{}^{\prime }})(x_t-{\hat{x}}_t^{{}^{\prime }}{)}^T] \\ =E[(A{x}_{t-1}+B{u}_t+w_t-A{\hat{x}}_{t-1}-B{u}_t)(A{x}_{t-1}+B{u}_t+w_t-A{\hat{x}}_{t-1}-B{u}_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] \\ =A{P}_{t-1}{A}^T+Q \end{gathered}$$

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

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

围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上项，得到一个近似的线性化模型，然后应用卡尔曼滤波完成状态估计。
系统方程
$x_t=f(x_{t-1})+W_t$
$z_t=h(x_{t-1})+V_{t-1}$

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

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