算法-卡尔曼滤波kalman全流程详细推导

卡尔曼滤波kalman全流程详细推导
1. 应用场景
对于任何状态连续性变化的系统，比如无人车运动中的速度和位置、车道线的位置等，均可以考虑使用kalman进行状态估计和优化。
2. 算法
本文举无人车的例子对kalman算法进行详细推导。
2.1. 状态变量构建
用$\overrightarrow{x_k}$表示无人车当前时刻的一个状态，只包括位置和速度：
$$\overrightarrow{x_k}=(\vec{p},\vec{v})$$
虽然我们可以通过传感器获得当前时刻无人车位置和速度的一个值。但是由于传感器误差的存在，我们并不知道无人车实际的位置和速度，它们之间有很多种可能正确的组合，但其中一些组合的可能性要大于另一些。基于这一点可以假设状态变量符合高斯分布，则可以基于高斯分布对状态进行表示，由两部分组成：包括最佳估计$\widehat{x_k}$和状态协方差矩阵$P_k$。
$$\widehat{x_k}=\left[\begin{array}{c}position\\ velocity\end{array}\right],P_k=\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right](1)$$
初始的$\widehat{x_k}$可用传感器的观测值直接表示，但是状态协方差矩阵$P_k$则要自己指定，指定的依据为，如果变量之间的变化趋势一致，即如果一个变量大于自身的期望时，另一个变量也大于自身的期望，则协方差为正，如果两个变量相互独立，则协方差为0。
2.2. 预测
之后就可以根据当前状态$(k-1)$时刻来预测下一状态$(k)$。初始预测时的输入$\widehat{x_k}$即为初始的$\widehat{x_k}$，虽然我们并不知道对下一状态的所有预测中哪个是真值，但是预测函数并不在乎，因为它对所有的可能性进行预测，并给出了新的高斯分布。用状态转移矩阵$F_k$来表示这一预测过程，$F_k$根据运动方程人为指定，用公式表示为：
$$\{\begin{array}{l}{p}_k=p_{k-1}+\Delta t{v}_{k-1}\\ v_k=v_{k-1}\end{array}\Rightarrow \widehat{x_k}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\widehat{x_{k-1}}=F_k\widehat{x_{k-1}}(2)$$
此时有了最佳状态估计，但是还没有对应的协方差矩阵，下面为更新协方差矩阵的公式：
$$P_k=F_k{P}_{k-1}{F}_k^T(3)$$
证明如下：
假设$X$是以$n$个标量随机变量组成的列向量，
$$X=\left[\begin{array}{c}{X}_1\\ ⋮\\ X_n\end{array}\right]$$
并且${\mu }_i$是其第i个元素的期望，即${\mu }_i=E(X_i)$。协方差矩阵被定义的第$i$，$j$项为：
$${\Sigma }_{ij}=cov(X_i,X_j)=E[(X_i-{\mu }_i)(X_j-{\mu }_j)]$$
又因$X$的期望由其中每一个元素的期望决定，即：
$$E(X)=E\left[\begin{array}{c}{X}_1\\ ⋮\\ X_n\end{array}\right]=\left[\begin{array}{c}E(X_1)\\ ⋮\\ E(X_n)\end{array}\right]$$
所以向量$X$的协方差矩阵可以表示为：
$$\begin{gathered} \Sigma =E[(X-E(X))(X-E(X){)}^T] \\ =\left[\begin{array}{ccc}E[(X_1-{\mu }_1)(X_1-{\mu }_1)]& \cdots & E[(X_1-{\mu }_1)(X_n-{\mu }_n)]\\ ⋮& ⋮& ⋮\\ E[(X_n-{\mu }_n)(X_1-{\mu }_1)]& \cdots & E[(X_n-{\mu }_n)(X_n-{\mu }_n)]\end{array}\right] \end{gathered}$$
假设向量$X$的协方差矩阵为$cov(X)$
则有$cov(X)=E[(X-E(X))(X-E(X){)}^T]$
令$Y=AX+a$
$EY=E(AX+a)=AEX+a$
$Y-EY=A(X-EX)$
所以$$\begin{gathered} cov(Y)=E[(Y-E(Y))(Y-E(Y){)}^T] \\ =E[A(X-E(X))(X-E(X){)}^T{A}^T] \\ =A(E(X-E(X))(X-E(X){)}^T)A^T \\ =Acov(X)A^T \end{gathered}$$
所以可证$P_k=F_k{P}_{k-1}{F}_k^T$
考虑外部控制量
接下来就是看我们是否知道一些外部控制量，比如假设由于油门的设置或控制指令，可以知道$a$，根据基本的运动学方程可得
$$\begin{gathered} \{\begin{array}{l}{p}_k=p_{k-1}+\Delta t{v}_{k-1}+\frac{1}{2}a\Delta t^2\\ v_k=v_{k-1}+a\Delta t\end{array}\Rightarrow F_k\widehat{x_{k-1}}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a \\ =F_k\widehat{x_{k-1}}+B_k\overrightarrow{u_k}(4) \end{gathered}$$
其中，$B_k$称为控制矩阵，${\vec{u}}_k$称为控制向量。
考虑外部干扰
这些状态量除了受系统自身性质的属性和外部控制作用的影响外，有可能还会受到一些未知因素的影响，比如轮胎打滑或者斜坡，需要再添加一些新的不确定性来跟踪这些不确定性的干扰，可以将这些没有被跟踪的干扰当做协方差为$Q_k$的噪声来处理。通过简单地添加$Q_k$可以得到扩展的协方差，公式如下：
$$P_k=F_k{P}_{k-1}{F}_k^T+Q_k(5)$$
由公式（4）和（5）可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。
到此为止，我们对系统可能的动向有了一个模糊的估计，用$\widehat{x_k}$和$P_k$来表示。接下来再结合当前帧的测量结果可以进一步提升当前帧的系统状态估计结果。
2.3. 融合测量值更新状态估计结果
传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，所以需要用矩阵$H_k$将前面的状态估计结果转换为与传感器的单位和尺度一致，公式如下：
$$\{\begin{array}{l}\overrightarrow{{\mu }_{excepted}}=H_k\widehat{x_k}\\ {\Sigma }_{excepted}=H_k{P}_k{H}_k^T\end{array}(6)$$
从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。
我们将这种不确定性（例如传感器噪声）用协防差$R_k$表示，该分布的均值就是我们读取到的传感器数据，称之为$\overrightarrow{z_k}$。
现在我们有了两个高斯分布，一个在预测值附近，一个在传感器读数附近。两种情况同时发生的概率即是我们想要的状态，只需要将两个高斯分布相乘即可。
融合高斯分布的理论推导如下（以一维高斯分布为例）：
一维高斯分布的概率密度函数：
$$f(x)=\frac{1}{\sqrt{2\pi }\delta }{e}^{-\frac{(x-\mu {)}^2}{2{\delta }^2}}$$
其本质问题可抽象为：已知两个独立高斯分布$N_1～({\mu }_1,{\delta }_1^2),N_2～({\mu }_2,{\delta }_2^2)$，求新的概率分布$N=N_1\times N_2～(?,?)$
$N_1$的概率分布函数为$f_1(x)$，$N_2$的概率分布函数为$f_2(x)$，则：
$$\begin{gathered} f_1(x)f_2(x)=\frac{1}{\sqrt{2\pi }{\delta }_1}{e}^{-\frac{(x-{\mu }_1{)}^2}{2{\delta }_1^2}}\cdot \frac{1}{\sqrt{2\pi }{\delta }_2}{e}^{-\frac{(x-{\mu }_2{)}^2}{2{\delta }_2^2}} \\ =\frac{1}{2\pi {\delta }_1{\delta }_2}{e}^{-(\frac{(x-{\mu }_1{)}^2}{2{\delta }_1^2}+\frac{(x-{\mu }_2{)}^2}{2{\delta }_2^2})} \end{gathered}$$
可以直接先单独分析指数部分，设：
$$\begin{gathered} \beta =\frac{(x-{\mu }_1{)}^2}{2{\delta }_1^2}+\frac{(x-{\mu }_2{)}^2}{2{\delta }_2^2} \\ =\frac{({\delta }_1^2+{\delta }_2^2)x^2-2({\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2)x+({\mu }_1^2{\delta }_2^2+{\mu }_2^2{\delta }_1^2)}{2{\delta }_1^2{\delta }_2^2} \\ =\frac{x^2-2\frac{{\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}x+\frac{{\mu }_1^2{\delta }_2^2+{\mu }_2^2{\delta }_1^2}{{\delta }_1^2+{\delta }_2^2}}{\frac{2{\delta }_1^2{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}} \end{gathered}$$
构造新的正态分布=$$\begin{gathered} \frac{(x-\frac{{\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}{)}^2+\frac{{\mu }_1^2{\delta }_2^2+{\mu }_2^2{\delta }_1^2}{{\delta }_1^2+{\delta }_2^2}-(\frac{{\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}{)}^2}{\frac{2{\delta }_1^2{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}} \\ =\underset{\gamma }{\underbrace{\frac{(x-\frac{{\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}{)}^2}{\frac{2{\delta }_1^2{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}}}}+\underset{\lambda }{\underbrace{\frac{\frac{{\mu }_1^2{\delta }_2^2+{\mu }_2^2{\delta }_1^2}{{\delta }_1^2+{\delta }_2^2}-(\frac{{\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}{)}^2}{\frac{2{\delta }_1^2{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}}}} \end{gathered}$$
设$\lambda$如上所示，则$\beta =\gamma +\lambda$，其中$\gamma$为一个$N～(\mu ,{\delta }^2)$的正态分布，$\lambda$是一个常数值。继续简化$\lambda$，如下：
$$\begin{gathered} \lambda =\frac{\frac{{\mu }_1^2{\delta }_2^2+{\mu }_2^2{\delta }_1^2}{{\delta }_1^2+{\delta }_2^2}-(\frac{{\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}{)}^2}{\frac{2{\delta }_1^2{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}} \\ =\frac{({\mu }_1^2{\delta }_2^2+{\mu }_2^2{\delta }_1^2)({\delta }_1^2+{\delta }_2^2)-({\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2{)}^2}{2{\delta }_1^2{\delta }_2^2({\delta }_1^2+{\delta }_2^2)} \\ =\frac{({\mu }_1^2{\delta }_2^2{\delta }_1^2+{\mu }_2^2{\delta }_1^4+{\mu }_2^2{\delta }_2^2{\delta }_1^2+{\mu }_1^2{\delta }_2^4)-({\mu }_2^2{\delta }_1^4+2{\mu }_1{\mu }_2{\delta }_2^2{\delta }_1^2+{\mu }_1^2{\delta }_2^4)}{2{\delta }_1^2{\delta }_2^2({\delta }_1^2+{\delta }_2^2)} \\ =\frac{{\delta }_1^2{\delta }_2^2({\mu }_1^2+{\mu }_1^2-2{\mu }_1{\mu }_2)}{2{\delta }_1^2{\delta }_2^2({\delta }_1^2+{\delta }_2^2)} \\ =\frac{({\mu }_1-{\mu }_2{)}^2}{2({\delta }_1^2+{\delta }_2^2)} \end{gathered}$$
则可得两个高斯分布相乘为：
$$\begin{gathered} f_1(x)f_2(x)=\frac{1}{2\pi {\delta }_1{\delta }_2}{e}^{-\beta }=\frac{1}{2\pi {\delta }_1{\delta }_2}{e}^{-(\gamma +\lambda )} \\ =\frac{1}{2\pi {\delta }_1{\delta }_2}{e}^{-\gamma }\cdot e^{-\lambda } \\ =\frac{1}{2\pi {\delta }_1{\delta }_2}{e}^{-\frac{(x-\mu {)}^2}{2{\delta }^2}}\cdot e^{-\frac{({\mu }_1-{\mu }_2{)}^2}{2({\delta }_1^2+{\delta }_2^2)}} \end{gathered}$$
其中：
$$\mu =\frac{{\mu }_2{\delta }_1^2+{\mu }_1{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2},{\delta }^2=\frac{{\delta }_1^2{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}$$
把常数项综合为$S_g$可得其直观表达方式：
$$f_1(x)f_2(x)=S_g\cdot \frac{1}{\sqrt{2\pi }\delta }{e}^{-\frac{(x-\mu {)}^2}{2{\delta }^2}}$$
$$S_g=\frac{1}{\sqrt{2\pi ({\delta }_1^2+{\delta }_2^2)}}{e}^{-\frac{({\mu }_1-{\mu }_2{)}^2}{2({\delta }_1^2+{\delta }_2^2)}}$$
到此，两个高斯分布相乘的分布函数即推导出来，即相乘后的分布函数为一个被压缩或被放大的高斯分布，$S_g$为缩放因子，相乘后的概率密度的积分不等于1，但其方差和均值性质不变，所以$N=N_1\times N_2～(\mu ,{\delta }^2)$，也就是我们常说的两个高斯分布相乘同样服从高斯分布。
则$\mu$和${\delta }^2$变量替换和变形处理后有卡尔曼滤波算法中的：
$$\{\begin{array}{l}{\mu }^{\prime }={\mu }_0+\frac{{\delta }_0^2({\mu }_1-{\mu }_0)}{{\delta }_0^2+{\delta }_1^2}\\ {\delta }^{\prime 2}={\delta }_0^2-\frac{{\delta }_0^4}{{\delta }_0^2+{\delta }_1^2}\end{array}(7)$$
将式（7）中的两个式子相同的部分用k表示：
$$k=\frac{{\delta }_0^2}{{\delta }_0^2+{\delta }_1^2}(8)$$
$$\{\begin{array}{l}{\mu }^{\prime }={\mu }_0+k({\mu }_1-{\mu }_0)\\ {\delta }^{\prime 2}={\delta }_0^2-k{\delta }_0^2\end{array}(9)$$
进一步推广到多维高斯分布，则可以将公式（8）和（9）写成矩阵的形式，如果$\Sigma$表示高斯分布的协方差，$\vec{\mu }$表示每个维度的均值，则：
$$K={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}(10)$$
$$\{\begin{array}{l}{\vec{\mu }}^{\prime }={\vec{\mu }}_0+K({\vec{\mu }}_1-{\vec{\mu }}_0)\\ {\Sigma }^{\prime }={\Sigma }_0-K{\Sigma }_0\end{array}(11)$$
矩阵$K$称为卡尔曼增益，下面将会用到。
将所有公式整合起来
我们有两个高斯分布，预测部分$({\mu }_0,{\Sigma }_0)=(H_k\widehat{x_k},H_k{P}_k{H}_k^T)$，和测量部分$({\mu }_1,{\Sigma }_1)=({\vec{z}}_k,R_k)$，将它们放到式（11）中算出它们之间的重叠部分：
$$\{\begin{array}{l}{H}_k{\hat{x}}_k^{\prime }=H_k{\hat{x}}_k+K({\vec{z}}_k-H_k{\hat{x}}_k)\\ H_k{P}_k^{\prime }{H}_k^T=H_k{P}_k{H}_k^T-K{H}_k{P}_k{H}_k^T\end{array}(12)$$
由式（10）可得卡尔曼增益为：
$$K=H_k{P}_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}(13)$$
将式（12）和式（13）的两边同时左乘矩阵的逆（注意$K$里面包含了$H_k$）将其约掉，再将式（12）的第二个等式两边同时右乘矩阵$H_k^T$的逆得到以下等式：
$$\{\begin{array}{l}{\hat{x}}_k^{\prime }={\hat{x}}_k+K^{\prime }({\vec{z}}_k-H_k{\hat{x}}_k)\\ P_k^{\prime }=P_k-K^{\prime }{H}_k{P}_k\end{array}(14)$$
$$K^{\prime }=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}(15)$$
上式给出了完整的更新步骤方程。${\hat{x}}_k^{\prime }$就是新的最优估计，我们可以将它和$P_k^{\prime }$放到下一个预测和更新方程中不断迭代。