卡尔曼滤波的理解

1. 卡尔曼滤波概念

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”

• 卡尔曼滤波思想：

以$K-1$时刻的最优估计$X_{k-1}$为准，预测$K$时刻的状态变量$Xk|k-1$，同时又对该状态进行观测，得到观测变量$Z_k$，再对预测与观测之间进行分析，或者说是以观测量对预测量进行修正，从而得到K时刻的最优状态估计$X_k$。

• 状态转移方程：$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$

这个状态方程是根据上一时刻的状态和控制变量来推测此刻的状态，$w_{k-1}$是服从高斯分布的噪声，也称为过程噪声，是预测过程的噪声，它对应了 $x_k$ 中每个分量的噪声，是期望为 0，协方差为 Q 的高斯白噪声$w_{k-1}$ ~ N(0,Q)，Q即激励噪声。A为状态转移矩阵，B为控制输入矩阵。

• 观测方程：$z_k=H{x}_k+v_k$

$v_k$是观测的噪声，服从高斯分布，也称为测量噪声，$v_k$~N(0,R),R测量噪声。H为状态观测矩阵。

2. 卡尔曼滤波理解

（1）理解例子一

以机器人举例障碍物距离为例。现在已知“传感器测量的机器人离障碍物的距离”，“上个时刻机器人离障碍物距离”和“自己当前时刻的速度”这三个数据。

• 观测值：传感器测量的机器人与障碍物的距离，比如雷达直接测量机器人离障碍物距离7m；
• 估计值：假如上一秒离障碍物10m，速度是4m/s，那么现在这秒估计就离障碍物距离是6m。

如果雷达测量的那个7m准确度是90%，根据速度估计出的那个6m准确度是80%，那么最终的距离估计结果就是：
$$result=(1-\frac{0.9}{0.8+0.9})\ast 6+\frac{0.9}{0.8+0.9}\ast 7=6.52m$$
其中$\frac{0.9}{0.8+0.9}$为卡尔曼增益，它就是表示这个传感器数据相对于根据速度计算出的估计值的靠谱程度。

（2）理解例子二

下面例子是为了加深对卡尔曼滤波过程的理解。如果上一例子看懂了，下面例子也可以跳过。

• 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
• 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
• 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因$K{g}^2=5^2/(5^2+4^2)$，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
• 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：$\sqrt{(1-Kg)\ast 5^2}=2.35$。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
• 就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！

3. 卡尔曼滤波的计算过程

还是以第二节中的"理解一"为例进行说明。

首先我们先回顾总结下直观理解中是怎么做的。

1. 根据上一秒导弹的位置 和 导弹的的速度估计出当前时刻导弹的位置粗略估计值。
2. 将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。

在前面我们也提到了导弹的位置和雷达测量值都是有误差的。所以卡尔曼想用概率来衡量数据的可信度。

卡尔曼认为导弹速度、导弹位置、雷达测距的测量值这些都服从正态分布。我们用$x_{t-1}$表示导弹在t-1时刻的位置的概率分布（它画出来就和上面那个图长的一样）。$v_{t-1}$表示导弹在t-1时刻的速度的概率分布。用$z_t$表示雷达测量到导弹离目标的距离概率分布。并且它们都是正态分布。

现在我们已知

1. 导弹在t-1时刻的位置的概率分布，为 $x_{t-1}=N(10,0.2^2)$。其实0.2就是上个时刻导弹位置的误差
2. 导弹的速度的概率分布，为 $v_{t-1}=N(4,0.7^2)$。其实0.7就是获取速度的那个传感器噪声误差。你会问咋就有噪声误差了？不知道你有没有用过手机里面的指南针那个软件，明明你没动那个指南针还是会在某个范围内波动。还有称体重，明明你没动那个称数据就是不断的动。这就是噪声误差。
   雷达在t时刻测量到导弹离目标的距离的概率分布，为 $z_t=N(7,0.1^2)$，这0.1就是雷达的噪声误差。

在前面我们知道了卡尔曼滤波算法为了融合雷达测量值和上个时刻的导弹状态数据来减少误差，需要实施“两步走”战略。

1. 根据上一秒导弹的位置 和 导弹的速度估计出当前时刻导弹的位置粗略估计值。
2. 将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。

我们来看看具体怎么做的。

1. 计算粗略估计值（这个是做了一个硬假设：导弹是保持匀速运动），大家可以对比着看下直观理解是怎么做的：(注意这里的估计是一秒间隔，下面的时间是乘以1s的)

$x在t时刻的粗略估计值=x_{t-1}-v_{t-1}=N(10,0.2^2)-N(4,0.7^2)=N(6,0.2^2+0.7^2)$

2. 根据粗略估计值的概率分布与雷达的测量值概率分布得到精确估计值的概率分布。根据贝叶斯滤波所推导，直接把两个概率分布相乘即可：

$x_t=x在t时刻的估计值\ast z_t=N(6,0.2^2+0.7^2)\ast N(7,0.1^2)=N(6,0.53^2)\ast N(7,0.1^2)$

高斯公式：
$$N(u_1,{\sigma }_1^2)\ast N(u_2,{\sigma }_2^2)=N(\frac{{\sigma }_1^2{u}_2+{\sigma }_2^2{u}_1}{{\sigma }_1^2+{\sigma }_2^2},\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2})$$

重构一下上式：
$$=>N((1-\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2})\ast u_1+\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}\ast u_2,\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2})$$

其中$u_1=6,u_2=7,{\sigma }_1=0.53,{\sigma }_2=0.1$，而卡尔曼增益为：
$$\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}=\frac{0.53^2}{0.53^2+0.1^2}$$
事实上可以看到我们就是把方差作为可信度，方差越大越不可信。然后也就是说用方差做权重进行加权和得到均值。

4. 卡尔曼滤波公式理解

先把上面的系统状态方程和观测方程重复写一遍如下：

• 状态转移方程：$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$

这个状态方程是根据上一时刻的状态和控制变量来推测此刻的状态，$w_{k-1}$是服从高斯分布的噪声，也称为过程噪声，是预测过程的噪声，它对应了 $x_k$ 中每个分量的噪声，是期望为 0，协方差为 Q 的高斯白噪声$w_{k-1}$ ~ N(0,Q)，Q即激励噪声。A为状态转移矩阵，B为控制输入矩阵。

• 观测方程：$z_k=H{x}_k+v_k$

$v_k$是观测的噪声，服从高斯分布，也称为测量噪声，$v_k$~N(0,R),R测量噪声。H为状态观测矩阵。

卡尔曼滤波算法分为两步:预测和更新。

• 预测(时间更新)：根据上一时刻( $k－1$ 时刻) 的后验估计值来估计当前时刻( $k$ 时刻) 的状态，得到 $k$ 时刻的先验估计值;
• 更新(测量更新)：使用当前时刻的测量值来更正预测阶段估计值，得到当前时刻的后验估计值。

时间更新方程和测量更新方程也被称为预测方程和校正方程，所以卡尔曼算法是一个递归的预测—校正方法。

卡尔曼滤波的5个公式和意义

• 卡尔曼滤波器时间更新方程-预测更新：

方程	说明
${\hat{x}}_k^{-}=A{\hat{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}$	卡尔曼增益，或卡尔曼系数
${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$	最优估计-后验状态估计值
$P_k=(I-K_{k}H)P_k^{-}$	后验估计协方差

参数	意义
${\hat{x}}_{k-1}$和${\hat{x}}_k$	分别表示 $k－1$ 时刻和 $k$ 时刻的后验状态估计值，即更新后的结果，也叫最优估计。
${\hat{x}}_k^{-}$	k 时刻的先验状态估计值，是滤波的中间计算结果，即根据上一时刻（$k-1$时刻）的最优估计预测的k时刻的结果，是预测方程的结果。
$P_{k-1}$和$P_k$	分别表示$k－1$ 时刻和 $k$ 时刻的后验估计协方差，即${\hat{x}}_{k-1}$和${\hat{x}}_k$的协方差，表示状态的不确定性，是滤波的结果之一。
$P_k^{-}$	$k$时刻的先验估计协方差(即${\hat{x}}_k^{-}$的协方差)，是滤波的中间计算结果
H	是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。
$Z_k$	测量值（观测值），是滤波的输入。
$K_k$	滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。
A	状态转移矩阵，实际上是对目标状态转换的一种猜想模型。例如在机动目标跟踪中， 状态转移矩阵常常用来对目标的运动建模，其模型可能为匀速直线运动或者匀加速运动。当状态转移矩阵不符合目标的状态转换模型时，滤波会很快发散。
Q	过程激励噪声协方差（系统过程的协方差）。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号， 所以 Q 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量，也叫预测模型本身带来的噪声。状态转移协方差矩阵
R	测量噪声协方差。滤波器实际实现时，测量噪声协方差 $R$一般可以观测得到，是滤波器的已知条件。
B	是将输入转换为状态的矩阵
$(z_k-H{\hat{x}}_k^{-})$	实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测），得到后验。

---

参考：

理解：https://www.zhihu.com/question/23971601
计算方式：https://zhuanlan.zhihu.com/p/77327349