卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。

线性滤波

先验与后验估计

由之前的状态空间方程：

$$ \begin{align*} {x_k }&=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_{k}&=Hx_{k}+v_k \end{align*} $$

其中$w_{k}$为过程噪声(Process Noise)，$v_k$为测量噪声(Measurement Noise)，暂且认为它们符合高斯分布，即$p_{(w)}\sim N(0,Q)，p_{(v)}\sim N(0,R)$，其中$0$表示期望，$Q,R$代表协方差矩阵，且$Q=E[ww^T],R=[vv^T]$，推导如下：

$$ \begin{align*} Q=E[ww^T]&=E \begin{bmatrix} \begin{bmatrix}w_1\\w_2\\.\\.\\w_k \end{bmatrix} \begin{bmatrix}w_1&w_2&...&w_k \end{bmatrix} \end{bmatrix} =E \begin{bmatrix}w_1^2&w_1w_2&...&w_1w_k\\ w_2w_1&w_2^2&...&w_2w_k\\ ...&...&...&...\\ w_kw_1&w_kw_1&...&w_k^2 \end{bmatrix}\\ &=\begin{bmatrix}Ew_1^2&Ew_1w_2&...&Ew_1w_k\\ Ew_2w_1&Ew_2^2&...&Ew_2w_k\\ ...&...&...&...\\ Ew_kw_1&Ew_kw_1&...&Ew_k^2 \end{bmatrix} \end{align*} $$

由于$w$的期望为$0$，由方差等于平方的期望减去期望的平方以及协方差公式：

$$ \begin{align*} Var(X)&=E(X^2)-E^2(X)\\ Cov(X,Y)&=E((X-\mu)(Y-v)) \end{align*} $$

所以此处平方的期望就等于方差：

$$ \begin{align*} Var(X)&=E(X^2)\\ Cov(X,Y)&=E(X·Y) \end{align*} $$

即为：

$$ Ew_k^2=\sigma_{w_1}^2,Ew_iw_j=\sigma_i\sigma_j $$

所以：

$$ Q=\begin{bmatrix}Ew_1^2&Ew_1w_2&...&Ew_1w_k\\ Ew_2w_1&Ew_2^2&...&Ew_2w_k\\ ...&...&...&...\\ Ew_kw_1&Ew_kw_1&...&Ew_k^2 \end{bmatrix}= \begin{bmatrix} \sigma_{w_{1} }^2&\sigma_{w_1}\sigma_{w_2}&...&\sigma_{w_1}\sigma_{w_k}\\ \sigma_{w_2}\sigma_{w_1}&\sigma_{w_{2} }^2&...&\sigma_{w_2}\sigma_{w_k}\\ ...&...&...&...\\ \sigma_{w_k}\sigma_{w_1}&\sigma_{w_k}\sigma_{w_1}&...&\sigma_{w_{k} }^2 \end{bmatrix} $$

因此$Q$就是之前所提到的协方差矩阵，同样的，测量噪声也有协方差矩阵$R$，满足类似的条件。

由于存在噪声的影响，所得到的$x_k$并不精确，我们暂且设它为$\hat{x}_k$，表示这是$x_k$的估计值；同样的$x_{k-1}$也是不准确的，设为$\hat{x}_{k-1}$，并且我们设通过下试得到的$x_k$成为先验估计，定义为$\hat{x}^-_k$；

先验估计（公式1）：

$$ \hat{x}^-_k=A\hat{x}_{k-1}+Bu_{k-1} $$

由于$z_k$是测出的结果，是已知的，由$z_k=Hx_k$可知：

$$ \hat{x}_{kmea}=H^-z_k $$

对于上面的两个式子，$\hat{x}^-_k$是算出来的，$\hat{x}_{kmea}$是测出来的，因为收到噪声的影响，它们两个都是不准确的，因此这时候卡尔曼滤波的作用就显现出来了，如何利用两个不准确的值去得出一个相对准确的结果？这里就很容易想到之前提到的数据融合的思想。

$$ \hat{x}_k=\hat{x}^-_k+G(H^-z_k-\hat{x}^-_k)，G\in[0,1] $$

分析式子很容易理解，当我们的测量数据很少时，$G\approx0$，则$\hat{x}_k=\hat{x}^-_k$，我们选择相信计算值；当我们的测量数据很多时，$G\approx1$，则$\hat{x}_k=H^-z_k$，我们选择相信测量值；

一般教科书上设$G=K_kH$；

后验估计（公式2）：

$$ \hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k),K_k\in[0,H^-] $$

同样的，当我们的测量数据很少（不准确）时，$K_k\approx0$，则$\hat{x}_k=\hat{x}^-_k$，我们选择相信计算值；当我们的测量数据很多（相对准确）时，$K_k\approx H^-$，则$\hat{x}_k=H^-z_k$，我们选择相信测量值；

卡尔曼增益

因此，我们的目标变得十分明确了，如何选择一个合适的$K_k$值，使得$\hat{x}_k\rightarrow x_k$，其中$x_k$表示真实值。

此时，我们定义一个误差值$e_k$，以及一个先验误差值$e_k^-$：

$$ e_k=x_k-\hat{x}_k\\ e_k^-=x_k-\hat{x}^-_k $$

误差也是符合高斯分布，$p(e_k)\sim(0,P)$，其中协方差矩阵$P=E[ee^T]$，与之前推导的$Q$是类似的。

因此，最终的目的就是选择一个合适的$K_k$值，使得$e_k$最小，也就是使得它的方差最小，因为方差越小，说明越接近期望值$0$，即$e_k\approx0$。而方差最小就是使得它的协方差矩阵的迹$tr(P)=\sigma^2_{e_{1} }+\sigma^2_{e_{2} }+...+\sigma^2_{e_{k} }$最小。

所以，最终的目的为：寻找一个合适的$K_k$使得$tr(P)$最小！

$$ \begin{align*} tr(P)&=E[ee^T]\\ &=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] \end{align*} $$

已知：

$$ \hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k)\\ z_k=Hx_k+v_k $$

带入上式：

$$ \begin{align*} x_k-\hat{x}_k&=x_k-(\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k))\\ &=x_k-\hat{x}^-_k-K_kz_k+K_kH\hat{x}^-_k\\ &=x_k-\hat{x}^-_k-K_k(Hx_k+v_k)+K_kH\hat{x}^-_k\\ &=x_k-\hat{x}^-_k-K_kHx_k-K_kv_k+K_kH\hat{x}^-_k\\ &=(x_k-\hat{x}^-_k)-K_kH(x_k-\hat{x}^-_k)-K_kv_k\\ &=(I-K_kH)(x_k-\hat{x}^-_k)-K_kv_k\\ &=(I-K_kH)e_k^--K_kv_k \end{align*} $$

所以：

可能会用到的公式：

$(AB)^T=B^TA^T$，$(A+B)^T=A^T+B^T$

$$ \begin{align*} tr(P)&=E[ee^T]\\ &=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T]\\ &=E[[(I-K_kH)e_k^--K_kv_k][(I-K_kH)e_k^--K_kv_k]^T]\\ &=E[[(I-K_kH)e_k^--K_kv_k][e_k^{-T}(I-K_kH)^T-v_k^TK_k^T]]\\ &=E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T-(I-K_kH)e_k^-v_k^TK_k^T\\ &-K_kv_ke_k^{-T}(I-K_kH)^T+K_kv_kv_k^TK_k^T]\\ \end{align*} $$

分别求每一项的期望：

可能会用到的已知条件：

$E[e_k^-]=0,E[e_k^{-T}]=0,E[v_k^T]=0,E[v_k]=0$

$$ \begin{align*} &E[-(I-K_kH)e_k^-v_k^TK_k^T]\\ &=-(I-K_kH)E[e_k^-v_k^T]K_k^T\\ &=-(I-K_kH)E[e_k^-]E[v_k^T]K_k^T\\ &=0 \end{align*} $$

同理可得：

$$ \begin{align*} &E[-K_kv_ke_k^{-T}(I-K_kH)^T]\\ &=-K_kE[v_k]E[e_k^{-T}](I-K_kH)^T\\ &=0 \end{align*} $$

所以最终：

可能会用到的定义：

已知协方差矩阵$P=E[ee^T]$，则定义$P_k^-=E[e_k^-e_k^{-T}]$

$p_{(v)}\sim(0,R)$，所以$R_k=E[v_kv^T_k]$

$$ \begin{align*} tr(P)&=E[ee^T]\\ &=E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T]+E[K_kv_kv_k^TK_k^T]\\ &=(I-K_kH)E[e_k^-e_k^{-T}](I-K_kH)^T+K_kE[v_kv_k^T]K_k^T\\ &=(P_k^--K_kHP_k^-)(I^T-H^TK_k^T)+K_kR_kK_k^T\\ &=P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kR_kK_k^T\\ \end{align*} $$

由于，协方差矩阵的转置就是它本身，所以$P_k^{-T}=P_k^-$

$(P_k^-H^TK_k^T)^T=K_kHP_k^{-T}=K_kHP_k^-$，所以$tr(K_kHP_k^-)=tr(P_k^-H^TK_k^T)$

因此：

$$ \begin{align*} tr(P)&=tr(P_k^-)-tr(K_kHP_k^-)-tr(P_k^-H^TK_k^T)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kR_kK_k^T)\\ &=tr(P_k^-)-2tr(K_kHP_k^-)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kR_kK_k^T) \end{align*} $$

$tr(P)$对$K_k$求导，使得$\frac{dtr(P_k)}{dK_k}=0$：

可能会用到的公式：

$\frac{dtr(AB)}{dA}=B^T$，$\frac{dtr(ABA^T)}{dA}=2AB$

$$ \begin{align*} \frac{dtr(P_k)}{dK_k}&=\frac{tr(P_k^-)}{dK_k} -\frac{2tr(K_kHP_k^-)}{dK_k} +\frac{tr(K_kHP_k^-H^TK_k^T))}{dK_k} +\frac{tr(K_kR_kK_k^T))}{dK_k}\\ &=0-2(HP_k^-)^T+2K_kHP_k^-H^T+2K_kR_k\\ &=0 \end{align*} $$

所以：

$$ \begin{align*} (HP_k^-)^T&=K_k(HP_k^-H^T+R_k) \end{align*} $$

卡尔曼增益（公式3）：

$$ \begin{align*} K_k&=\frac{P_k^-H^T}{HP_k^-H^T+R_k} \end{align*} $$

因此，结合我们之前的结论：

当我们的测量数据很少（不准确）时，$K_k\approx0$，则$\hat{x}_k=\hat{x}^-_k$，我们选择相信计算值；当我们的测量数据很多（相对准确）时，$K_k\approx H^-$，则$\hat{x}_k=H^-z_k$，我们选择相信测量值；

此处也是相同的：

$R_k$表示测量噪声，当测量噪声很大时，$K_k\approx0$，则$\hat{x}_k=\hat{x}^-_k$，我们选择相信计算值；

当测量噪声很小时，$K_k\approx H^-$，则$\hat{x}_k=H^-z_k$，我们选择相信测量值；

误差协方差矩阵

总结前面推导的三个公式：

先验估计（公式1）：

$$ \hat{x}^-_k=A\hat{x}_{k-1}+Bu_{k-1} $$

后验估计（公式2）：

$$ \hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k),K_k\in[0,H^-] $$

卡尔曼增益（公式3）：

$$ \begin{align*} K_k&=\frac{P_k^-H^T}{HP_k^-H^T+R_k} \end{align*} $$

上面的公式里只有$P_k^-$是未知的，需要推导。由于已知协方差矩阵$P=E[ee^T]$，所以定义$P_k^-=E[e_k^-e_k^{-T}]$，如何求这个误差协方差矩阵是以下篇幅讨论的重点。

可能会用到的定义与公式：

$e_k=x_k-\hat{x}_k$，$P_k=E[ee^T]$

$e_k^-=x_k-\hat{x}^-_k$ ，$P_k^-=E[e_k^-e_k^{-T}]$

$\hat{x}^-_k=A\hat{x}_{k-1}+Bu_{k-1}$

$x_k =Ax_{k-1}+Bu_{k-1}+w_{k-1}$

首先推导$e_k^-=x_k-\hat{x}^-_k$:

$$ \begin{align*} e_k^-&=x_k-\hat{x}^-_k\\ &=Ax_{k-1}+Bu_{k}+w_{k-1}-A\hat{x}_{k-1}-Bu_{k}\\ &=A(x_{k-1}-\hat{x}_{k-1})+w_{k-1}\\ &=Ae_{k-1}+w_{k-1} \end{align*} $$

所以：

由于$e_{k-1}=x_{k-1}-\hat{x}_{k-1}$，所以$e_{k-1}$作用的是上一时刻，而$w_k$是作用于当前时刻的，它们之间是相互独立的。$E[e_{k-1}w_{k}^T]=E[e_{k-1}]E[w_{k}^T]$，且$E[e_{k-1}]=E[w_{k}^T]=0$

$$ \begin{align*} P_k^-&=E[e_k^-e_k^{-T}]\\ &=E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^T]\\ &=E[(Ae_{k-1}+w_{k-1})(e_{k-1}^TA^T+w_{k-1}^T)]\\ &=E[Ae_{k-1}e_{k-1}^TA^T+Ae_{k-1}w_{k-1}^T+w_{k-1}e_{k-1}^TA^T+w_{k-1}w_{k-1}^T]\\ &=E[Ae_{k-1}e_{k-1}^TA^T]+E[Ae_{k-1}w_{k-1}^T]+E[w_{k-1}e_{k-1}^TA^T]+E[w_{k-1}w_{k-1}^T]\\ &=AE[e_{k-1}e_{k-1}^T]A^T+E[w_{k-1}w_{k-1}^T]\\ &=AP_{k-1}A^T+Q \end{align*} $$

误差协方差矩阵（公式4）：

$$ \begin{align*} P_k^-=AP_{k-1}A^T+Q \end{align*} $$

由于每一次的误差协方差矩阵$P_k^-$，都需要上一次的$P_{k-1}$，所以每一时刻我们都需要更新一下$P_k$的值，为下一时刻使用做准备。

更新误差协方差：（公式5）：

$$ \begin{align*} P_k&=P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kR_kK_k^T\\ &=P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_k(HP_k^-H^T+R_k)K_k^T\\ &=P_k^--K_kHP_k^--P_k^-H^TK_k^T+\frac{P_k^-H^T}{HP_k^-H^T+R_k}(HP_k^-H^T+R_k)K_k^T\\ &=P_k^--K_kHP_k^-\\ &=(I-K_kH)P_k^- \end{align*} $$

至此，我们可以总结一下整个卡尔曼滤波的过程：

介绍卡尔曼滤波论文：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

非线性滤波器

前面的卡尔曼滤波我们可以在一个线性系统得到一个最优的估计值，那么卡尔曼滤波器在非线性系统中的应用如何呢？一般来说有许多方法，但是最基本的方法就是将非线性系统进行线性化，这种滤波器我们称为扩展卡尔曼滤波器（Extended Kalman Filter，简称EKF）。

对于线性系统的状态空间表达式：

$$ \begin{align*} {x_k }&=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_{k}&=Hx_{k}+v_k \end{align*} $$

其中$p_{(w)}\sim N(0,Q)，p_{(v)}\sim N(0,R)$，其中$0$表示期望，$Q,R$代表协方差矩阵

对于非线性系统的状态空间表达式：

$$ \begin{align*} x_k&=f(x_{k-1},u_{k-1},w_{k-1})\\ z_k&=h(x_k,v_k) \end{align*} $$

其中$p_{(w)}\sim N(0,Q)，p_{(v)}\sim N(0,R)$，其中$0$表示期望，$Q,R$代表协方差矩阵

虽然噪声仍然满足正态分布，但是正态分布的随机变量通过非线性系统后就不再是正态的了。

所以需要对非线性系统进行线性化处理，这里会用到泰勒展开。

比如我们对$f(x)$在$x_0$处进行泰勒展开：

$$ f(x)=f(x_0)+\frac{\partial f}{\partial x}(x-x_0) $$

对于一个非线性系统来说，最好的一个线性化的点就是它的真实点，但是由于系统存在误差，所以我们永远无法知道系统的真实点是多少，所以一般$f(x)$在$\hat{x}_{k-1}$(即上一时刻的后验估计)处进行线性化。

• 过程方程线性化

假设误差$w_{k-1}=0$

$$ \begin{align*} x_k&=f(\hat{x}_{k-1},u_{k-1},w_{k-1})+A(x_k-\hat{x}_{k-1})+Ww_{k-1}\\ &=f(\hat{x}_{k-1},u_{k-1},0)+A(x_k-\hat{x}_{k-1})+Ww_{k-1} \end{align*} $$

记：$f(\hat{x}_{k-1},u_{k-1},0)=\widetilde{x}$，而$A，W$是雅可比矩阵，且$A=\frac{\partial f}{\partial x}|\hat{x}_{k-1},u_{k-1}$，$W=\frac{\partial f}{\partial w}|\hat{x}_{k-1},u_{k-1}$

• 测量方程线性化

将$z_k$在$\widetilde{x}$处线性化：

假设误差 $v_{k}=0$

$$ z_{k}=h(\widetilde{x}_{k},v_k)+H(x_k-\widetilde{x}_{k})+Vv_k $$

而$H，V$是雅可比矩阵，且$H=\frac{\partial f}{\partial x}|\widetilde{x}_{k}$，$V=\frac{\partial f}{\partial v}|\widetilde{x}_{k}$

所以非线性系统线性化后：

$$ \begin{align*} x_k&=f(\hat{x}_{k-1},u_{k-1},0)+A(x_k-\hat{x}_{k-1})+Ww_{k-1}\\ z_{k}&=h(\widetilde{x}_{k},v_k)+H(x_k-\widetilde{x}_{k})+Vv_k \end{align*} $$

由于$p_{(w)}\sim N(0,Q)，p_{(v)}\sim N(0,R)$，其中$0$表示期望，$Q,R$代表协方差矩阵，那么很容易得到$p_{(Ww)}\sim N(0,wQw^T)，p_{(Vv)}\sim N(0,vRv^T)$

至此，我们可以总结一下整个卡尔曼滤波的过程：

EKF在SLAM中的局限性

假设了马尔可夫性，也就是$ k$ 时刻的状态只与$ k − 1$时刻相关，而与$ k − 1 $之前的状态和观测都无关（或者和前几个有限时间的状态相关）。这有点像是在视觉里程计中，只考虑相邻两帧关系一样。如果当前帧确实与很久之前的数据有关（例如回环），那么滤波器就会难以处理这种情况。

$EKF$ 滤波器仅在 $\hat{x}_{k−1}$ 处做了一次线性化，然后就直接根据这次线性化结果，把后验概率给算了出来。这相当于在说，我们认为该点处线性化近似，在后验概率处仍然是有效的。而实际上，当我们离开工作点较远的时候，一阶泰勒展开并不一定能够近似整个函数，这取决于运动模型和观测模型的非线性情况。

从程序实现上来说，EKF 需要存储状态量的均值和方差，并对它们进行维护和更新。如果把路标也放进状态的话，由于视觉 SLAM 中路标数量很大，这个存储量是相当可观的，且与状态量呈平方增长（因为要存储协方差矩阵）。因此，EKF SLAM 普遍被认为不可适用于大型场景。