Kalman Filter(KF)卡尔曼滤波的理解与原理推导

背景介绍

卡尔曼滤波使用来做什么的？卡尔曼滤波要解决的问题就是，根据测量数据和上一个时刻的状态来估计当前状态。打个比方就是，你闭着眼睛走路，那么你的状态就是你相对环境的位置。而你的测量数据就是你睁开眼一次看到的场景。而你看一眼可能看得不准确。你凭感觉估计也估计不准确。卡尔曼滤波要做的事就是结合你估计的自己当前相对环境的位置和你看一眼觉得自己相对与环境中的位置（测量数据），来更精确的估计出当前你相对环境的位置。

直观理解卡尔曼滤波

其实卡尔曼滤波思想非常简单。就是现有我们既知道上个时刻的状态，又知道当前时刻的状态设备测量的观测值。也就是说我既可以根据上个时刻的状态预测现在这个时刻的状态。也可以根据设备测量出的观测值确定当前时刻的状态。为了更加准确，所以我需要融合预测值和测量值来估计出当前的状态值。举个例子就是：现在我既知道上一分钟的温度是21，又知道现在温度计的读数是23。因为温度计它可能不准，我得参考下上分钟的温度21，然后我设定了一个假设就是它是随时间线性变化的。我根据上分钟预测现在温度应该是$A\times21+u=22$。然后最终我认为现在的温度是预测的温度与测量的温度的一个折中。即$所估计现在的温度=预测的温度+K\times (温度计读数-H \times 预测的温度)$。你可以先根据我说的直观理解看看下面这个标准的卡尔曼滤波公式。$\hat x_k$是最终估计的状态值。在例子中就是最终估计的温度值。$x_k^-$是我根据上个状态计算出对当前状态的预测值。


用概率论知识来理解卡尔曼滤波就是粉红色是我预测的温度概率分布，绿色是我用温度计测出的温度概率分布，蓝色是我利用预测值和测量值两者融合所计算出的温度估计。

卡尔曼滤波有哪些默认的假设？

在背景介绍中提到了卡尔曼滤波算法的输入有两个：估计的自己当前相对环境的位置和你看一眼觉得自己相对与环境中的位置（测量数据）。算法的输出为：更精确的估计出当前你相对环境的位置。学任何算法一定要搞懂这个算法作出了哪些前提假设，因为畸基本这些算法都是利用这些前提假设进行推导的。如果不懂这些前提假设直接看公式会一脸懵。接下来我们讲讲卡尔曼滤波所设定了一些前提假设。

1. 卡尔曼滤波算法认为当前状态与上一个时刻的状态是线性关系（这句话其实就是假设了马尔可夫性，即认为当前状态只与上个状态相关）。也就是说当前时刻状态=A×上一个时刻状态+u+当前时刻的噪声。假设上一个时刻的状态可以表示为$x_{k-1}$，当前时刻状态表示为$x_k$，当前时刻的噪声表示为$w_k$。已知当前运动输入u。那么把当前时刻状态=A×上一个时刻状态+u+当前时刻的噪声这句话写成公式就是：$x_k=A_kx_{k-1}+u_k+w_k$。由于这个式子里面包含噪声，所以我们无法精确的求出当前状态$x_k$，但是我们可以求出$x_k$的概率分布，然后把这个概率分布的均值作为当前状态的估计。事实上卡尔曼就是这么想的，但是他不是直接算$x_k$的概率分布，而是算在当前的观测数据条件下$x_k$的概率分布。然后把这个概率分布的均值作为更准确的状态估计。假设当前观测数据是$z_k$，那么我们想求出$P(x_k|z_k,x_{k-1},u_k)$的概率分布，然后把这个概率分布的均值作为当前时刻的状态的一个更准确的估计。$P(x_k|z_k,x_{k-1},u_k)$指的是在观测数据是$z_k$，已知上个时刻状态$x_{k-1}$和时刻$k$处的状态是$x_k$的概率分布。
2. 卡尔曼还对观测数据与状态之间的关系也设定了一个假设。他认为时刻$k$处的观测数据$z_k$与状态$x_k$之间的关系也是线性的。即观测数据=C×状态+噪声，也就是说$z_k=C_kx_k+v_k$。
3. 注意：这里面的两个噪声$w_k和v_k$都是假设服从均值为0的高斯分布。假设：$w_k=N(0,R);v_k=N(0,Q)$。注意高斯分布就是正态分布，N是指的是正态分布的意思。R和Q分别是这两个概率分布的方差。
4. 假设上个状态$x_{k-1}$服从标准高斯分布即$N(x_{k-1},S_{k-1})$。当前状态$x_k$也服从高斯分布但是均值和方差待求。观测数据$z_k$服从高斯分布待求。

好了现在卡尔曼他作出的前提假设都讲完了。接下来介绍下他是怎么利用这些假设求出$P(x_k|z_k)$的概率分布，然后把这个概率分布的均值作为当前时刻的状态的一个更准确的估计。

从原理上推导卡尔曼滤波

现在我们梳理一下已知哪些东西，和要求哪些东西？
现在我们已知：

1. $x_{k-1}$的概率分布为N(0,1)，并且噪声$w_k$概率分布为N(0,R)，并且$x_k=A_kx_{k-1}+u_k+w_k$。（所以我们可以根据这个式子算出概率分布$P(x_k|x_{k-1},u_k,w_k)$）
   根据高斯分布线性运算规则可得：$P(x_k|x_{k-1},u_k,w_k)=A_kN(x_{k-1},S_{k-1})+u_k+N(0,R)=N(A_kx_{k-1}+u_k,A_kS_{k-1}A_k^T+R)$。
2. 观测数据$z_k$的分布为N(0,1)，$z_k=C_kx_k+v_k$，噪声$v_k$概率分布为N(0,Q)。所以可以推出$z_k$的概率分布$P(z_k|x_k)$。
   根据高斯分布线性运算规则可得：$P(z_k|x_k)=N(C_kx_k,Q)$

我们要求什么：我们得求$P(x_k|z_k,x_{k-1},u_k)=P(z_k|x_k)P(x_k|x_{k-1},u_k,w_k)$，然后根据这个概率分布算出$x_k$的均值$\bar x_k$和方差$S_k$。当前状态的更准确的估计就是$\bar x$。然后你会发现它的均值和方差分别为（我直接用别人的公式了，反正前面原理基本讲清楚了，别人公式的均值是$\hat x_k$方差是$P_k$）：

参考文献：
[1] https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
[2] https://www.cs.unc.edu/~welch/kalman/media/pdf/maybeck_ch1.pdf
[3] http://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman filter.pdf
[4] https://courses.engr.illinois.edu/ece420/sp2017/UnderstandingKalmanFilter.pdf
[5] https://www.youtube.com/watch?v=VFXf1lIZ3p8