卡尔曼滤波器原理

一、递归算法

维基百科对递归的解释是：

递归（Recursion），又译为递回，在数学与计算机科学中，是指在函数的定义中使用函数自身的方法。

举个栗子
多次测量一个硬币的直径，观测结果分别为 $z_1,z_2,\cdots ,z_k$（ $k$ 为测量的次数），我们希望得到这个硬币直径的估计值 $\tilde{x}$。

我们可以对观测结果取平均值：

测量次数$k$	估计值 $\widetilde{x_k}$
1	$z_1$
2	$\frac{1}{2}(z_1+z_2)$
3	$\frac{1}{3}(z_1+z_2+z_3)$
$⋮$	$⋮$
k	$\frac{1}{k}(z_1+z_2+\cdots +z_k)$

整理一下：
$$\begin{array}{rl}{\tilde{x}}_k& =\frac{1}{k}(z_1+z_2+\cdots +z_k)\\ & =\frac{1}{k}(z_1+z_2+\cdots +z_{k-1})+\frac{1}{k}{z}_k\\ & =\frac{1}{k}\frac{k-1}{k-1}(z_1+z_2+\cdots +z_{k-1})+\frac{1}{k}{z}_k\\ & =\frac{k-1}{k}{\tilde{x}}_{k-1}+\frac{1}{k}{z}_k\\ & ={\tilde{x}}_{k-1}-\frac{1}{k}{\tilde{x}}_{k-1}+\frac{1}{k}{z}_k\end{array}$$

即
$${\tilde{x}}_k={\tilde{x}}_{k-1}+\frac{1}{k}(z_k-{\tilde{x}}_{k-1})$$

从上式可以看出，随着测量次数 $k$ 的增加，测量结果 $z_k$ 对我们的估计就不再重要了。其实也就是，随着数据量的增加，我们对估计值越发有信心，所以之后的数据结果变得不那么重要了；相反，当 $k$ 很小的时候，测量结果 $z_k$ 的作用较大。

令 $K_k=1/k$，
$${\tilde{x}}_k={\tilde{x}}_{k-1}+K_k(z_k-{\tilde{x}}_{k-1})\text{\hspace{1em}(*)}$$
这就是卡尔曼滤波器的核心公式之一，$K_k$ 称为卡尔曼增益。即

$$当前的估计值=上一次的估计值+卡尔曼增益\times (当前的测量值-上一次的估计值)$$

从这个式子可以看出，当前的估计值与上一次的估计值有关，上一次的估计值与上上次的估计值有关，这就是递归的思想。这也是卡尔曼滤波器的优势，它不需要追溯很久之前的数据，只需要上一次的数据就可以了。

二、数据融合

百度百科对数据融合的解释是：

数据融合是将多信息源的数据和信息加以联合、相关及组合，获得更为精确的估计的过程。

举个栗子
两个天平测量同一个物体的重量，测量结果如下（我们认为数据分布服从正态分布）：

	测量结果（数学期望）（单位g）	误差（标准差）（单位g）
scale_1	$z_1=30$	${\sigma }_1=2$
scale_2	$z_2=32$	${\sigma }_1=4$

我们希望从这两组数据中得到物体重量的最优估计。

绿色曲线与蓝色曲线分别对应两个称的测量数据的概率密度曲线，因为scale_1的标准差相对scale_2的小，所以它的可信度更高一点。
因此对物体重量的估计值应该如红色虚线所示，它的值更加靠近scale_1。

三、协方差矩阵

百度百科对数据融合的解释是：

设 $X=(X_1,X_2,\cdots ,X_N{)}^T$ 为 $n$ 维随机变量，称矩阵
$$C=(c_{ij}{)}_{n\times n}=\left[\begin{array}{cccc}{c}_{11}& c_{12}& \cdots & c_{1n}\\ c_{21}& c_{22}& \cdots & c_{2n}\\ ⋮& ⋮& \ddots & ⋮\\ c_{n1}& c_{n2}& \cdots & c_{nn}\end{array}\right]$$

为 $n$ 维随机变量 $X_i$ 和 $X_j$ 的协方差矩阵，也记作 $D(X)$，其中 $$c_{ij}=Cov(X_i,X_j),i,j=1,2,\cdots ,n$$

为 $X$ 的分量 $X_i$ 和 $X_j$ 的协方差（设它们都存在）。

举个栗子

球员	身高 $x$	体重 $y$	年龄 $z$
瓦尔迪	179	74	33
奥巴梅扬	187	80	31
萨拉赫	175	71	28
数学期望	180.3	75	30.7

方差：
$$\begin{array}{rl}{\sigma }_x^2& =\frac{1}{3}[(179-180.3{)}^2+(187-180.3{)}^2+(175-180.3{)}^2]=24.89\\ & \\ {\sigma }_y^2& =\frac{1}{3}[(74-75{)}^2+(80-75{)}^2+(71-75{)}^2]=14\\ & \\ {\sigma }_z^2& =\frac{1}{3}[(33-30.7{)}^2+(31-30.7{)}^2+(28-30.7{)}^2]=4.22\end{array}$$

协方差：
$$\begin{array}{rl}Cov(x,y)& =Cov(y,x)=\frac{1}{3}[(179-180.3)(74-75)+(187-180.3)(80-75)+(175-180.3)(71-75)]=18.7\\ & \\ Cov(x,z)& =Cov(z,x)=\frac{1}{3}[(179-180.3)(33-30.7)+(187-180.3)(31-30.7)+(175-180.3)(28-30.7)]=4.4\\ & \\ Cov(y,z)& =Cov(z,y)=\frac{1}{3}[(74-75)(33-30.7)+(80-75)(31-30.7)+(71-75)(28-30.7)]=3.3\end{array}$$

因此，协方差矩阵为：
$$P=\left[\begin{array}{ccc}{\sigma }_x^2& Cov(x,y)& Cov(x,z)\\ Cov(y,x)& {\sigma }_y^2& Cov(y,z)\\ Cov(z,x)& Cov(z,y)& {\sigma }_z^2\end{array}\right]=\left[\begin{array}{ccc}24.89& 18.7& 4.4\\ 18.7& 14& 3.3\\ 4.4& 3.3& 4.22\end{array}\right]$$

通过定义来计算协方差矩阵比较繁琐，下面给出简洁的计算公式：

假设 $X=(x,y,z{)}^T$ 为三维随机变量，我们求它的协方差矩阵，
令
$$A=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]-\frac{1}{3}\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1\\ 1& 1& 1\end{array}\right]\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$$

则协方差矩阵为：
$$P=\frac{1}{3}{A}^{T}A$$

四、状态空间表达式

举个栗子

弹簧阻尼系统

受力分析：
$$m\ddot{q}+c\dot{q}+kq=u$$

设状态变量 $X=(x_1,x_2{)}^T$

有
$$\begin{gathered} x_1=q \\ {x}_2=\dot{q} \end{gathered}$$

则
$$\begin{array}{rl}\dot{x_1}& =x_2\\ \dot{x_2}& =\ddot{q}=\frac{1}{m}u-\frac{c}{m}\dot{q}-\frac{k}{m}q\\ & =\frac{1}{m}u-\frac{c}{m}{x}_2-\frac{k}{m}{x}_1\end{array}$$

设测量量 $Z=(z_1,z_2{)}^T=(x_1,x_2{)}^T$，分别为位移和速度

写成矩阵的形式：
$$\begin{gathered} \left[\begin{array}{c}\dot{x_1}\\ \dot{x_2}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -\frac{k}{m}& -\frac{c}{m}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]u \\ \left[\begin{array}{c}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right] \end{gathered}$$

即
$$\begin{array}{rl}\dot{X}(t)& =AX(t)+Bu(t)\\ Z(t)& =HX(t)\end{array}$$

上式即连续时间系统的状态空间表达式，它是从微分方程或传递函数建立起来的，叫做系统的实现。在离散系统中，从差分方程或脉冲传递函数求取离散状态空间表达式，也是一种实现。

百度百科对差分方程的解释是：

在数学上，递推关系（recurrence relation），也就是差分方程（difference equation），是一种递推地定义一个序列的方程式：序列的每一项目是定义为前一项的函数。

设系统差分方程为：
$$\begin{array}{rl}& y(k+n)+a_{n-1}y(k+n-1)+\cdots +a_{1}y(k+1)+a_{0}y(k)\\ & =b_{n}u(k+n)+b_{n-1}u(k+n-1)+\cdots +b_{1}u(k+1)+b_{0}u(k)\end{array}$$

实现的任务就是确定一种状态空间表达式：
$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{u}_k\\ Z_k& =H{X}_k\end{array}$$

后面我们推导卡尔曼滤波器主要用的就是上面描述的离散系统的状态空间表达式。

在此基础上，映入不确定性，即
$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{u}_{k-1}+w_{k-1}\\ Z_k& =H{X}_k+v_k\end{array}$$

由于数据的不确定性，卡尔曼滤波器要解决的问题就是将系统估计值与观测值进行数据融合，得到误差比它们都要小的最优估计。

五、卡尔曼滤波数学推导

(1)卡尔曼增益数学推导

系统状态空间方程为：
$$\begin{array}{rl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\\ z_k& =H{x}_k+v_k\end{array}$$

其中 $w_k$ 为系统过程噪声，$v_k$ 为观测噪声，假设两者为相互独立，正态分布的白色噪声，协方差矩阵分别为 $Q、R$，即
$$\begin{gathered} w_k\sim N(0,Q) \\ {v}_k\sim N(0,R) \end{gathered}$$

后面会用到一个公式：
$$\begin{gathered} Q=E[w{w}^T] \\ R=E[v{v}^T] \end{gathered}$$

验证过程如下：

令$x=[x_1,x_2{]}^T,w=[w_1,w_2{]}^T$
$$\begin{gathered} w{w}^T=\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right]\left[\begin{array}{c}{w}_1,w_2\end{array}\right]=\left[\begin{array}{cc}{w}_1^2& w_1{w}_2\\ w_2{w}_1& w_2^2\end{array}\right] \\ E[w{w}^T]=\left[\begin{array}{cc}E(w_1^2)& E(w_1{w}_2)\\ E(w_2{w}_1)& E(w_2^2)\end{array}\right] \end{gathered}$$
由于 $Cov(w_1,w_2)=E(w_1{w}_2)-E(w_1)E(w_2)$，又 $w$ 的数学期望为0，故
$Cov(w_1,w_2)=E(w_1{w}_2)$，同理 $Cov(w_2,w_1)=E(w_2{w}_1)$
又因为 $Var(x)=E(x^2)-E^2(x)$，故 $Var(x_1)=E(x_1^2),Var(x_2)=E(x_2^2)$
综上，$$E[w{w}^T]=\left[\begin{array}{cc}{\sigma }_{w_1}^2& Cov(w_1{w}_2)\\ Cov(w_2{w}_1)& {\sigma }_{w_2}^2\end{array}\right]=Q$$

根据第四章得到的状况空间方程
$$\begin{array}{rl}{\tilde{x}}_k^{-}& =A{\tilde{x}}_{k-1}+B{u}_{k-1}\\ z_k& =H{x}_k\end{array}$$

$x_k$：状态的真实值
${\tilde{x}}_k^{-}$：先验状态估计值，即状态的预测值
${\tilde{x}}_k$：后验状态估计值，即状态的最优估计值

将上式变换一下，有
$$\begin{array}{rlr}{\tilde{x}}_k^{-}& =A{\tilde{x}}_{k-1}^{-}+B{u}_{k-1}& \to 根据模型计算得到的预测值\\ {\tilde{x}}_{k_{mea}}& =H^{-1}{z}_k& \to 根据测量得到的预测值\end{array}$$

而最优估计值应位于两个预测值之间，即
$${\tilde{x}}_k={\tilde{x}}_k^{-}+G(H^{-1}{z}_k-{\tilde{x}}_k^{-})$$

其中，$G=K_{k}H$，$K_k$ 就是卡尔曼增益。

我们的目标就是寻找最合适的 $K_k$ 使得最优估计值 ${\tilde{x}}_k$ 最接近真实值 $x_k$。

引入误差：$e_k=x_k-{\tilde{x}}_k$，也就是状态真实值与最优估计值之差，且 $e_k\sim N(0,P)$

因此我们的目标变为寻找最合适的 $K_k$ 使得 $e_k$ 最小，因为它是一个向量，即是得其各分量的方差之和最小，即寻找最合适的 $K_k$ 使得协方差矩阵 $P$ 的迹最小。

$$P_k=E[e_k{e}_k^T]=E[(x_k-{\tilde{x}}_k)(x_k-{\tilde{x}}_k{)}^T]$$

其中
$$\begin{array}{rl}{x}_k-{\tilde{x}}_k& =x_k-[{\tilde{x}}_k^{-}+K_k(z_k-H{\tilde{x}}_k^{-})]\\ & =x_k-{\tilde{x}}_k^{-}-K_k{z}_k+K_{k}H{\tilde{x}}_k^{-}\\ & =x_k-{\tilde{x}}_k^{-}-K_k(H{x}_k+v_k)+K_{k}H{\tilde{x}}_k^{-}\\ & =x_k-{\tilde{x}}_k^{-}-K_{k}H{x}_k-K_k{v}_k+K_{k}H{\tilde{x}}_k^{-}\\ & =(x_k-{\tilde{x}}_k^{-})-K_{k}H(x_k-{\tilde{x}}_k^{-})-K_k{v}_k\\ & =(I-K_{k}H)(x_k-{\tilde{x}}_k^{-})-K_k{v}_k\\ & =(I-K_{k}H)e_k^{-}-K_k{v}_k\end{array}$$

故
$$\begin{array}{rl}{P}_k& =E\left(\right[(I-K_{k}H)e_k^{-}-K_k{v}_k\left]\right[(I-K_{k}H)e_k^{-}-K_k{v}_k{]}^T)\\ & =E\left(\right[(I-K_{k}H)e_k^{-}-K_k{v}_k\left]\right[e_k^{-T}(I-K_{k}H{)}^T-v_k^T{K}_k^T])\\ & =E((I-K_{k}H)e_k^{-}{e}_k^{-T}(I-K_{k}H{)}^T-(I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T-K_k{v}_k{e}_k^{-T}(I-K_{k}H{)}^T+K_k{v}_k{v}_k^T{K}_k^T)\\ & =E((I-K_{k}H)e_k^{-}{e}_k^{-T}(I-K_{k}H{)}^T)-E((I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T)-E(K_k{v}_k{e}_k^{-T}(I-K_{k}H{)}^T)+E\left(K_k{v}_k{v}_k^T{K}_k^T\right)\end{array}$$

其中
$$\begin{array}{rl}E((I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T)& =(I-K_{k}H)E\left(e_k^{-}{v}_k^T\right)K_k^T\\ & =0(因为先验误差e_k^{-}与测量噪声v_k相互独立，且v_k的期望为0)\end{array}$$

同理，
$$E(K_k{v}_k{e}_k^{-T}(I-K_{k}H{)}^T)=0$$

故
$$\begin{array}{rl}{P}_k& =(I-K_{k}H)E\left(e_k^{-}{e}_k^{-T}\right)(I-K_{k}H{)}^T+K_{k}E(v_k{v}_k^T)K_k^T\\ & =(I-K_{k}H)P_k^{-}(I-K_{k}H{)}^T+K_{k}R{K}_k^T\\ & =(P_k^{-}-K_{k}H{P}_k^{-})(I-H^T{K}_k^T)+K_{k}R{K}_k^T\\ & =P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+K_{k}H{P}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T\\ & =P_k^{-}-(K_{k}H{P}_k^{-}{)}^T-K_{k}H{P}_k^{-}+K_{k}H{P}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T\end{array}$$

因此
$$tr(P_k)=tr(P_k^{-})-2tr(K_{k}H{P}_k^{-})+tr(K_{k}H{P}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)$$

我们的目标是其值最小，即
$$\begin{array}{rl}\frac{d(tr(P_k))}{d(K_k)}& =0-2(H{P}_k^{-}{)}^T+2K_{k}H{P}_k^{-}{H}^T+2K_{k}R\\ & =0\end{array}$$

上面推导用到了关于矩阵乘积求导公式，如下

$$\frac{d(tr(AB))}{dA}=B^T$$

$$\frac{d(tr(AB{A}^T))}{dA}=2AB$$

因此我们得到
$$\begin{array}{r}-P_k^{-T}{H}^T+K_k(H{P}_k^{-}{H}^T+R)=0\\ -P_k^{-}{H}^T+K_k(H{P}_k^{-}{H}^T+R)=0\\ K_k(H{P}_k^{-}{H}^T+R)=P_k^{-}{H}^T\\ K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\end{array}$$

这样我们就得到了卡尔曼滤波器中最核心的公式：卡尔曼增益

我们分析一下，当观测噪声协方差矩阵 $R$ 特别大（即观测噪声方差特别大）时，$K_k\to 0$，此时最优估计就是先验估计；当观测噪声协方差矩阵 $R$ 特别小（即观测噪声方差特别小）时$K_k\to H^{-1}$，此时最优估计就是观测结果。

(2)先验估计误差协方差矩阵及后验估计误差协方差矩阵数学推导

现在我们有了以下三个公式：

先验估计
$${\tilde{x}}_k^{-}=A{\tilde{x}}_{k-1}+B{u}_{k-1}$$

后验估计
$${\tilde{x}}_k={\tilde{x}}_k^{-}+K_k(z_k-H{\tilde{x}}_k^{-})$$

卡尔曼增益
$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$

卡尔曼增益中，$P_k^{-}$ 还是未知的，接下来我们来计算它
$$\begin{array}{rl}{P}_k^{-}& =E[e_k^{-}{e}_k^{-T}]\end{array}$$

其中，
$$\begin{array}{rl}{e}_k^{-}& =x_k-{\tilde{x}}_k^{-}\\ & =A{x}_{k-1}+B{u}_{k-1}+w_{k-1}-A{\tilde{x}}_{k-1}-B{u}_{k-1}\\ & =A(x_{k-1}-{\tilde{x}}_{k-1})+w_{k-1}\\ & =A{e}_{k-1}+w_{k-1}\end{array}$$

将其带入，有
$$\begin{array}{rl}{P}_k^{-}& =E[e_k^{-}{e}_k^{-T}]\\ & =E[(A{e}_{k-1}+w_{k-1})(A{e}_{k-1}+w_{k-1}{)}^T]\\ & =E[(A{e}_{k-1}+w_{k-1})(e_{k-1}^T{A}^T+w_{k-1}^T)]\\ & =E[A{e}_{k-1}{e}_{k-1}^T{A}^T+A{e}_{k-1}{w}_{k-1}^T+w_{k-1}{e}_{k-1}^T{A}^T+w_{k-1}{w}_{k-1}^T]\\ & =E\left[A{e}_{k-1}{e}_{k-1}^T{A}^T\right]+E\left[A{e}_{k-1}{w}_{k-1}^T\right]+E\left[w_{k-1}{e}_{k-1}^T{A}^T\right]+E\left[w_{k-1}{w}_{k-1}^T\right]\\ & =AE\left[e_{k-1}{e}_{k-1}^T\right]A^T+E\left[w_{k-1}{w}_{k-1}^T\right]\\ & =A{P}_{k-1}{A}^T+Q\end{array}$$

这样就可以利用卡尔曼滤波器来估计状态变量的值了，它分为两个部分，一个是预测，另一个是校正。

	预测
先验估计	${\tilde{x}}_k^{-}=A{\tilde{x}}_{k-1}+B{u}_{k-1}$
先验估计协方差	$P_k^{-}=A{P}_{k-1}{A}^T+Q$

	校正
卡尔曼增益	$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$
后验估计	${\tilde{x}}_k={\tilde{x}}_k^{-}+K_k(z_k-H{\tilde{x}}_k^{-})$

然后我们还要更新 $P_k$

上一节，我们证明了
$$P_k=P_k^{-}-(K_{k}H{P}_k^{-}{)}^T-K_{k}H{P}_k^{-}+K_{k}H{P}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T$$

再带入 $$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$

可得
$$P_k=(I-K_{k}H)P_k^{-}$$

这就得到了卡尔曼滤波器完整的五个公式：
$$\begin{gathered} {\tilde{x}}_k^{-}=A{\tilde{x}}_{k-1}+B{u}_{k-1} \\ {P}_k^{-}=A{P}_{k-1}{A}^T+Q \\ {K}_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R} \\ {\tilde{x}}_k={\tilde{x}}_k^{-}+K_k(z_k-H{\tilde{x}}_k^{-}) \\ {P}_k=(I-K_{k}H)P_k^{-} \end{gathered}$$