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

背景介绍

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

直观理解卡尔曼滤波

其实卡尔曼滤波思想非常简单。就是现有我们既知道上个时刻的状态，又知道当前时刻的状态设备测量的观测值。也就是说我既可以根据上个时刻的状态预测现在这个时刻的状态。也可以根据设备测量出的观测值确定当前时刻的状态。为了更加准确，所以我需要融合预测值和测量值来估计出当前的状态值。举个例子就是：现在我既知道上一分钟的温度是21，又知道现在温度计的读数是23。因为温度计它可能不准，我得参考下上分钟的温度21，然后我设定了一个假设就是它是随时间线性变化的。我根据上分钟预测现在温度应该是$A\times 21+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_k{x}_{k-1}+u_k+w_k$。由于这个式子里面包含噪声，所以我们无法精确的求出当前状态$x_k$，但是我们可以求出$x_k$的概率分布，然后把这个概率分布的均值作为当前状态的估计。事实上卡尔曼就是这么想的，但是他不是直接算$x_k$的概率分布，而是算在当前的观测数据条件下$x_k$的概率分布。然后把这个概率分布的均值作为更准确的状态估计。假设当前观测数据是 z k z_k