滤波系列（一）卡尔曼滤波算法（KF）：详细数学推导

滤波系列（一）卡尔曼滤波算法（KF）

在本文，将给出卡尔曼滤波算法的详细数学推导过程，如果想直接了解卡尔曼滤波算法的应用，请看博客：卡尔曼滤波算法的应用（python代码）或者直接可以调用Python FilterPy包

KF推导

符号说明

图1：系统的概率图模型

其中 $X_t$ 表示隐状态，$Y_t$表示量测，黑色的箭头表示状态转移过程，红色的箭头表示测量过程，$P(X_t│X_{t-1})$ 为状态转移的概率，$P(Y_t│X_t)$为量测概率。
从概率图模型可以看出，这里有两个假设条件，第一：假设目标的状态转移过程服从一阶马尔可夫模型，即当前时刻的状态 $X_t$ 只与上一时刻的状态 $X_{t-1}$ 有关；第二：假设观测值相互独立，即观测值 $Y_t$ 只与 $t$ 时刻的隐状态 $X_t$ 有关。
状态转移过程：

$$P(X_t│X_{t-1})=N(A{X}_{t-1}+B,Q)（1）$$

等价于：

$$X_t=A{X}_{t-1}+B+\omega ,\omega \sim N(0,Q)（2）$$

$\omega$ 表示过程噪声，它服从均值为0，协方差为$Q$的高斯分布。

量测过程：

$$P(Y_t│X_t)=N(H{X}_t+C,R)（3）$$

等价于：

$$Y_t=H{X}_t+C+\nu ,\nu \sim N(0,R)（4）$$

$\nu$ 表示量测噪声，它服从均值为0，协方差为$R$的高斯分布。

公式推导：

$X_t$ 的后验概率分布为：

$$P(X_t│Y_1,\cdots Y_t)=\frac{P(X_t,Y_1,\cdots Y_t)}{P(Y_1,\cdots Y_t)}（5）$$

$P(Y_1,\cdots Y_t)$ 为量测 $Y_1,\cdots Y_t$发生的概率，在给定量测数据的情况下为一个常数，因此：

$$\begin{gathered} P(X_t│Y_1,\cdots Y_t) \\ \propto P(X_t,Y_1,\cdots Y_t) \\ =P(Y_t│X_t,Y_1,\cdots Y_{t-1})P(X_t│Y_1,\cdots Y_{t-1})P(Y_1,\cdots Y_{t-1}) \\ \propto P(Y_t│X_t)P(X_t│Y_1,\cdots Y_{t-1})（6） \end{gathered}$$

注:同理$P(Y_1,\cdots Y_{t-1})$也是一个常数；同时又由假设可知，观测值相互独立，即观测值$Y_t$ 只与 $t$时刻的隐状态 $X_t$有关，所以$P(Y_t│X_t,Y_1,\cdots Y_{t-1})=P(Y_t│X_t)$。

$P(Y_t│X_t)$已经化为最简，就是量测方程，所以我们将重点解决 $P(X_t│Y_1,\cdots Y_{t-1})$。

注:因为我们已知状态转移方程 $P(X_t│X_{t-1})$，所以我们想把这个表达式引入到上面的表达式里，即$P(X_t,X_{t-1}│Y_1,\cdots Y_{t-1})$，同时我们为了等式端相等，就需要把随机变量 $X_{t-1}$积分掉。

$$\begin{gathered} P(X_t│Y_1,\cdots Y_{t-1})={\int }_{X_{t-1}}P(X_t,X_{t-1}│Y_1,\cdots Y_{t-1})d{X}_{t-1} \\ ={\int }_{X_{t-1}}P(X_t│X_{t-1},Y_1,\cdots Y_{t-1})P(X_{t-1}│Y_1,\cdots Y_{t-1})d{X}_{t-1} \\ ={\int }_{X_{t-1}}P(X_t│X_{t-1})P(X_{t-1}│Y_1,\cdots Y_{t-1})d{X}_{t-1}（7） \end{gathered}$$
注:由假设1知，目标的状态转移过程服从一阶马尔可夫模型，即当前时刻的状态 $X_t$只与上一时刻的状态$X_{t-1}$ 有关，因此$P(X_t│X_{t-1},Y_1,\cdots Y_{t-1})=P(X_t│X_{t-1})$。
注:$$\begin{gathered} P(X_t,X_{t-1}│Y_1,\cdots Y_{t-1})=\frac{P(X_t,X_{t-1},Y_1,\cdots Y_{t-1})}{P(Y_1,\cdots Y_{t-1})} \\ =\frac{P(X_t,X_{t-1},Y_1,\cdots Y_{t-1})P(X_{t-1},Y_1,\cdots Y_{t-1})}{P(X_{t-1},Y_1,\cdots Y_{t-1})P(Y_1,\cdots Y_{t-1})} \\ =P(X_t│X_{t-1},Y_1,\cdots Y_{t-1})P(X_{t-1}│Y_1,\cdots Y_{t-1}) \end{gathered}$$

整理一下式子6和式子7，

$$\begin{gathered} P(X_t│Y_1,\cdots Y_t)\propto P(Y_t│X_t)P(X_t│Y_1,\cdots Y_{t-1}) \\ \propto P(Y_t│X_t){\int }_{X_{t-1}}P(X_t│X_{t-1})P(X_{t-1}│Y_1,\cdots Y_{t-1})d{X}_{t-1}(8) \end{gathered}$$

这里出现了递归！！！$P(X_t│Y_1,\cdots Y_t)$ 与 $P(X_{t-1}│Y_1,\cdots Y_{t-1})$递归，令$P(X_t│Y_1,\cdots Y_t)=N({\hat{\mu }}_t,{\hat{\Sigma }}_t)$,那么$P(X_{t-1}│Y_1,\cdots Y_{t-1})=N({\hat{\mu }}_{t-1},{\hat{\Sigma }}_{t-1})$，令$P(X_t│Y_1,\cdots Y_{t-1})=N({\bar{\mu }}_t,{\bar{\Sigma }}_t)$。
由公式7得预测步（利用前t-1个时刻的量测来预测第t个时刻的状态）：

$$P(X_t│Y_1,\cdots Y_{t-1})={\int }_{X_{t-1}}P(X_t│X_{t-1})P(X_{t-1}│Y_1,\cdots Y_{t-1})d{X}_{t-1}=N({\bar{\mu }}_t,{\bar{\Sigma }}_t)(9)$$

由公式6得更新步（利用第$t$个时刻的量测来更新第$t$个时刻的状态）：
$$P(X_t│Y_1,\cdots Y_t)=N({\hat{\mu }}_t,{\hat{\Sigma }}_t)\propto P(Y_t│X_t)P(X_t│Y_1,\cdots Y_{t-1})(10)$$
$t=1$ : $P(X_1│Y_1)=N({\hat{\mu }}_1,{\hat{\Sigma }}_1)$更新（第一个时刻只有更新，没有预测）
$t=2$ : $P(X_2│Y_1)=N({\bar{\mu }}_2,{\bar{\Sigma }}_2)$ 预测
$P(X_2│Y_1,Y_2)=N({\hat{\mu }}_2,{\hat{\Sigma }}_2)$更新
$t=3$ : $P(X_3│Y_1,Y_2)=N({\bar{\mu }}_3,{\bar{\Sigma }}_3)$ 预测
$P(X_3│Y_1,Y_2,Y_3)=N({\hat{\mu }}_3,{\hat{\Sigma }}_3)$ 更新
$\dots$
$t=t$: $P(X_t│Y_1,\cdots Y_{t-1})=N({\bar{\mu }}_t,{\bar{\Sigma }}_t)$ 预测
$P(X_t│Y_1,\cdots Y_t)=N({\hat{\mu }}_t,{\hat{\Sigma }}_t)$ 更新
给出一个动态模型的描述(不考虑常数项)：

$$\{\begin{array}{l}{X}_t=A{X}_{t-1}+\omega ,\omega \sim N(0,Q)\\ Y_t=H{X}_t+\nu ,\nu \sim N(0,R)\end{array}(11)$$

由一阶马尔科夫假设和观测独立性假设知：$cov(X_{t-1},\omega )=0,cov(X_{t-1},\nu )=0,cov(\omega ,\nu )=0$
由递归表达式可知：

$$P(X_t│Y_1,\cdots Y_{t-1})=N({\bar{\mu }}_t,{\bar{\Sigma }}_t)(12)$$

$$\begin{gathered} P(X_{t-1}│Y_1,\cdots Y_{t-1})=N({\hat{\mu }}_{t-1},{\hat{\Sigma }}_{t-1}) \\ =E[X_{t-1}]+\Delta X_{t-1},\Delta X_{t-1}\sim N(0,{\hat{\Sigma }}_{t-1})(13) \end{gathered}$$

由状态转移过程可知：

$$\begin{gathered} P(X_t│Y_1,\cdots Y_{t-1})=A{X}_{t-1}+\omega \\ =A(E[X_{t-1}]+\Delta X_{t-1})+\omega =AE[X_{t-1}]+A\Delta X_{t-1}+\omega \\ =E[X_t]+\Delta X_t(14) \end{gathered}$$

$$\{\begin{array}{l}E[X_t]=AE[X_{t-1}]=A{\hat{\mu }}_{t-1}\\ \Delta X_t=A\Delta X_{t-1}+\omega \end{array}(15)$$

注：$V[X_t]=E[(X_t-E[X_t])(X_t-E[X_t]{)}^T]=E[\Delta X_t\Delta X_t^T]$。

$$\begin{gathered} P(X_t│Y_1,\cdots Y_{t-1})=N({\bar{\mu }}_t,{\bar{\Sigma }}_t) \\ =N(AE[X_{t-1}],E[\Delta X_t\Delta X_t^T])=N(A{\hat{\mu }}_{t-1},E[\Delta X_t\Delta X_t^T])(16) \end{gathered}$$

$$\begin{gathered} E[\Delta X_t\Delta X_t^T]=E[(A\Delta X_{t-1}+\omega )(A\Delta X_{t-1}+\omega {)}^T] \\ =E[(A\Delta X_{t-1}+\omega )(\Delta X_{t-1}^T{A}^T+{\omega }^T)]=E[A\Delta X_{t-1}\Delta X_{t-1}^T{A}^T+\omega {\omega }^T] \\ =AE[\Delta X_{t-1}\Delta X_{t-1}^T]A^T+E[\omega {\omega }^T]=A{\hat{\Sigma }}_{t-1}{A}^T+Q(17) \end{gathered}$$

注：$cov(X_{t-1},\omega )=0,cov(X_{t-1},\nu )=0,cov(\omega ,\nu )=0$。
预测公式：

$${\bar{\mu }}_t=A{\hat{\mu }}_{t-1}(18)$$

$${\bar{\Sigma }}_t=A{\hat{\Sigma }}_{t-1}{A}^T+Q(19)$$

由量测方程知：
$$\begin{gathered} P(Y_t│Y_1,\cdots Y_{t-1})=H{X}_t+\nu =H(AE[X_{t-1}]+A\Delta X_{t-1}+\omega )+\nu \\ =HAE[X_{t-1}]+HA\Delta X_{t-1}+H\omega +\nu =E[Y_t]+\Delta Y_t(20) \end{gathered}$$

$$\{\begin{array}{l}E[Y_t]=HAE[X_{t-1}]=HA{\hat{\mu }}_{t-1}=H{\bar{\mu }}_t\\ \Delta Y_t=HA\Delta X_{t-1}+H\omega +\nu \end{array}(21)$$

$$\begin{gathered} P(Y_t│Y_1,\cdots Y_{t-1})=N(HAE[X_{t-1}],E[\Delta Y_t\Delta Y_t^T]) \\ =N(H{\bar{\mu }}_t,E[\Delta Y_t\Delta Y_t^T])(22) \end{gathered}$$

$$\begin{gathered} E[\Delta Y_t\Delta Y_t^T]=E[(HA\Delta X_{t-1}+H\omega +\nu )(HA\Delta X_{t-1}+H\omega +\nu {)}^T] \\ =E[(HA\Delta X_{t-1}+H\omega +\nu )(\Delta X_{t-1}^T{A}^T{H}^T+{\omega }^T{H}^T+{\nu }^T)] \\ =E[HA\Delta X_{t-1}\Delta X_{(}t-1{)}^T{A}^T{H}^T+H\omega {\omega }^T{H}^T+\nu {\nu }^T] \\ =HAE[\Delta X_{t-1}\Delta X_{(}t-1{)}^T]A^T{H}^T+HE[\omega {\omega }^T]H^T+R \\ =H(A{\hat{\Sigma }}_{t-1}{A}^T+Q)H^T+R=H{\bar{\Sigma }}_t{H}^T+R(23) \end{gathered}$$

$$\begin{gathered} E[\Delta X_t\Delta Y_t^T]=E[(A\Delta X_{t-1}+\omega )(HA\Delta X_{t-1}+H\omega +\nu {)}^T] \\ =E[(A\Delta X_{t-1}+\omega )(\Delta X_{t-1}^T{A}^T{H}^T+{\omega }^T{H}^T+{\nu }^T)] \\ =E[A\Delta X_{t-1}\Delta X_{t-1}^T{A}^T{H}^T+\omega {\omega }^T{H}^T] \\ =AE[\Delta X_{t-1}\Delta X_{t-1}^T]A^T{H}^T+E[\omega {\omega }^T]H^T] \\ =(AE[\Delta X_{t-1}\Delta X_{t-1}^T]A^T+E[\omega {\omega }^T]])H^T \\ =(A{\hat{\Sigma }}_{t-1}{A}^T+Q)H^T={\bar{\Sigma }}_t{H}^T(24) \end{gathered}$$

$$\begin{gathered} E[\Delta Y_t\Delta X_t^T]=E[(HA\Delta X_{t-1}+H\omega +\nu )(A\Delta X_{t-1}+\omega {)}^T] \\ =E[(HA\Delta X_{t-1}+H\omega +\nu )(\Delta X_{t-1}^T{A}^T+{\omega }^T)] \\ =E[HA\Delta X_{t-1}\Delta X_{t-1}^T{A}^T+H\omega {\omega }^T] \\ =HE[A\Delta X_{t-1}\Delta X_{t-1}^T{A}^T+\omega {\omega }^T] \\ =H(A{\hat{\Sigma }}_{t-1}{A}^T+Q)=H{\bar{\Sigma }}_t(25) \end{gathered}$$

$$\left(\begin{array}{c}P(X_t│Y_1,\cdots Y_{t-1})\\ P(Y_t│Y_1,\cdots Y_{t-1}))\end{array}\right)\sim N\left(\left[\begin{array}{c}A{\hat{\mu }}_{t-1}\\ HA{\hat{\mu }}_{t-1}\end{array}\right],\left[\begin{array}{cc}A{\hat{\Sigma }}_{t-1}{A}^T+Q& {\bar{\Sigma }}_t{H}^T\\ H{\bar{\Sigma }}_t& H{\bar{\Sigma }}_t{H}^T+R)\end{array}\right]\right)(26)$$
注：给出一个公式：

$$\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)\sim N\left(\left[\begin{array}{c}{\mu }_1\\ {\mu }_2\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{11}& {\Sigma }_{12}\\ {\Sigma }_{21}& {\Sigma }_{22}\end{array}\right]\right)(27)$$

$$P(x_1│x_2=a)=N({\mu }_1+{\Sigma }_{12}{\Sigma }_{22}^{-1}(a-{\mu }_2),{\Sigma }_{11}-{\Sigma }_{12}{\Sigma }_{22}^{-1}{\Sigma }_{21})(28)$$

$$\begin{gathered} P(X_t│Y_1,\cdots Y_t)= \\ =N(A{\hat{\mu }}_{t-1}+{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}(Y_t-HA{\hat{\mu }}_{t-1}),A{\hat{\Sigma }}_{t-1}{A}^T+Q-{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}H{\bar{\Sigma }}_t) \\ =N({\bar{\mu }}_t+{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}(Y_t-H{\bar{\mu }}_t),{\bar{\Sigma }}_t-{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}H{\bar{\Sigma }}_t)(29) \end{gathered}$$

$$\{\begin{array}{l}{\hat{\mu }}_t={\bar{\mu }}_t+{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}(Y_t-H{\bar{\mu }}_t)\\ {\hat{\Sigma }}_t={\bar{\Sigma }}_t-{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}H{\bar{\Sigma }}_t)\end{array}(30)$$

化解后得，更新公式：

$$K_t={\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}(31)$$

$$\begin{gathered} {\hat{\mu }}_t={\bar{\mu }}_t+{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}(Y_t-H{\bar{\mu }}_t) \\ ={\bar{\mu }}_t+K_t(Y_t-H{\bar{\mu }}_t)(32) \end{gathered}$$

$$\begin{gathered} {\hat{\Sigma }}_t={\bar{\Sigma }}_t-{\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}H{\bar{\Sigma }}_t \\ ={\bar{\Sigma }}_t-K_{t}H{\bar{\Sigma }}_t(I-K_{t}H){\bar{\Sigma }}_t(33) \end{gathered}$$

总结一下，至此就推导出了kalman滤波的5个公式：
预测公式：

$${\bar{\mu }}_t=A{\hat{\mu }}_{t-1}(18)$$

$${\bar{\Sigma }}_t=A{\hat{\Sigma }}_{t-1}{A}^T+Q(19)$$
更新公式：
$$K_t={\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}(31)$$

$${\hat{\mu }}_t={\bar{\mu }}_t+K_t(Y_t-H{\bar{\mu }}_t)(32)$$

$${\hat{\Sigma }}_t=(I-K_{t}H){\bar{\Sigma }}_t(33)$$

卡尔曼滤波算法的流程图（摘自维基百科）：

图2：卡尔曼滤波算法流程图

图解卡尔曼滤波算法：

图3通过一个卡尔曼滤波的例子，直观展示了在预测和更新过程中状态变量的概率密度分布的变化情况。

图3：图解卡尔曼滤波算法

其中，${\hat{x}}_{t-1}$ 表示 $t-1$ 时刻更新的状态，${\bar{x}}_t$ 表示 $t$ 时刻预测的状态，$y_t$ 表示 $t$ 时刻的量测，${\hat{x}}_t$ 表示 $t$ 时刻更新的状态，图中红色的直线对应的x坐标表示真实的状态。从图3可以看出，经过卡尔曼滤波算法（预测：${\hat{x}}_{t-1}$$-->$${\bar{x}}_t$；更新：${\hat{x}}_{t-1}$，$y_t$$-->$${\hat{x}}_t$ ）更新过后的状态将更加接近于真实的状态。

参考资料

徐亦达机器学习：Kalman Filter 卡尔曼滤波