无人驾驶技术之Kalman Filter原理介绍

基本思想

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

具体实例

　　设一个机器人有两个状态量，分别为位置P,速度V。在这里记为：

卡尔曼滤波假设两个变量（位置和速度，在这个例子中）都是随机的，并且服从高斯分布。每个变量都有一个均值μ，表示随机分布的中心（最可能的状态），以及方差 ，表示不确定性。其中，位置和速度之间可以是相关的也可以是不相关的。如果二者相关，那么这意味着可以由其中一个变量推出另一个变量的值。例如，我们基于旧的位置来估计新位置。如果速度过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远；如果二者不相关，则无法相互推出。它们这种相关性，可以用协方差矩阵来表示，简言之，矩阵中的每个元素Σ_ij表示第i个和第j个状态变量之间的相关度。

预测方程

　　下面我们用矩阵来表述此问题。基于高斯分布来建立状态变量，那么在时刻K需要两个信息：最佳估计以及协方差矩阵P_k 。

需要注意的是，我们这里只用到了两个状态位置和速度，实际上可以包含多个变量。接下来，我们根据当前状态（k-1时刻）来预测下一状态（k时刻）。记住，我们并不知道对下一状态的所有预测中哪个是真实的，但我们的预测函数并不在乎。它只管对所有的可能性进行预测，并给出新的高斯分布。

我们用基本的数学公式表示下一状态的预测：

　　　　　　　　　　　　　　　　P_k=P_k-1+Δt*V_k-1

　　　　　　　　　　　　　　　　V_k=              V_k-1

用矩阵表示即为：

现在，预测矩阵已经表示出下一时刻的状态，接下来表示协方差矩阵。在给出协方差矩阵前，我们需要了解一个简单的变换:

　　　　　　　　　　　　             Cov(x)=Σ

　　　　　　　　　　　　　　　   Cov(Ax)=AΣA^T

因此得到 P_k=F_kP_k-1F_k^T.

综上，我们得到预测方程为：

外部控制

　　我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。以汽车运动模型为例，汽车司机可能会控制油门，使得汽车加速行驶。同样地，机器人这个例子中，内部指令也可能使得轮子加速转动或者减速。我们可以将这些额外的控制信息，用一个向量 表示，将它们加到我们的预测方程中做修正。

　　假设由于油门的设置或控制命令，我们知道了期望的加速度a，根据基本的运动学方程可以得到：

　　　　　　　　　　　　　　　　P_k=P_k-1+Δt*V_k-1+1/2*aΔt²

　　　　　　　　　　　　　　　　V_k=              V_k-1+aΔt

用矩阵表示即为：

其中，Bk称为控制矩阵，称为控制向量（对于没有外部控制的简单系统来说，这部分可以忽略）。考虑了预测部分+外部控制后，我们还有一些因素需要考虑，例如噪声。

外部干扰

　　如果这些状态靓完全是基于系统自身参数属性或者一些已知的外部控制来变化，则不会有什么问题。但是，如果存在未知的干扰呢？例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。因此，在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型。

　　由于原始估计中的每个状态变量更新到新的状态后，仍服从高斯分布。我们就可以说的每个状态变量移动到了一个新的服从高斯分布的区域，协方差为Q_k。换句话说，我们就是将这些未跟踪到的干扰当做协方差为Q_k的噪声处理。这就产生了具有不同协方差，但均值相同的新的高斯分布。我们可以通过简单地添加Q_k得到扩展的协方差，下面给出预测步骤的完整表达式：

　　由上式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知的外部控制变量的修正。而新的不确定性由上一不确定性预测，并加上外部环境干扰得到的。

　　以上就是我们给出Kalman Filter的预测部分的讲解，那么接下来我们说下Kalman Filter的另一个中部分---更新，即结合传感器数据对预测部分进行进一步的确定性增强。

用测量数据修正估计

　　我们可能会有多个传感器来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息。但是需要注意的是，有些情况下，传感器读取的数据的单位尺度与我们跟踪状态的单位尺度不同，我们需要用矩阵H_k将我们预测的数据转换为与传感器读取的数据相同的单位尺度，以便进行数据融合。

我们通过预测数据计算出传感器读数分布如下式所示：

　　　　　　　　　　　　　　　　　　　　　　

　　卡尔曼滤波的一大优点就是能处理传感器噪声，换句话说，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。 从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。 我们将这种不确定性（例如：传感器噪声）用协方差R_k表示，该分布的均值就是我们读取到的传感器数据，称之为。

　　现在我们得到两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。 我们必须通过这两个高斯分布找到最优解。那么，最有可能的状态是什么呢？对于任何可能的值，有两种情况：（1）传感器的测量值；（2）由前一状态得到的预测值。那么如果得到这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。 就可到满足二者共同的部分，那么这个重叠部分也就是两个估计最优可能的值，即所有信息中的最优估计。

　　把两个具有不同均值和方差的高斯分布相乘，你会得到一个新的具有独立均值和方差的高斯分布！推导如下：

假设预测部分满足N（x;μ_0,σ₀²）,测量部分满足N（x;μ_1,σ₁²），那么N（x;μ_0,σ₀²）*N（x;μ_1,σ₁²）=?N(x;μ'_,σ'²)

通过将高斯分布概率密度公式带入，并进行归一化使得总概率为一，可以得到：

　　　　　　　　　　　                 　

将上式相同部分用K表示：

　　　　　　　　　　　　　　　　　　　　　 　

将上式进一步写成矩阵形式，用Σ表示高斯分布的协方差，表示每个维度的均值，则：

　　　　　　　　　　　　　　　　　　　　　　

其中矩阵K称为卡尔曼增益。

下面整理所有公式，推导更新步骤。

我们有两个高斯分布，预测部分和测量部分，将它们放到上式中算出它们之间的重叠部分：

　　　　　　　　　　　　　　　　　　

将式（1）（2）带入K的等式中得到卡尔曼增益为：

　　　　　　　　　　　　　　　　　　　　　　　　

将（1）（2）的两式同时左乘矩阵的逆H_k^-1(注意K中有H_k)约掉H_k，再将式（2）右边同时右乘矩阵H_K^T的逆得到下式：

　　上式给出了完整的更新步骤。就是最优估计，我们可以将它和P_k'放到下一次预测和更新方程中不断迭代。以上所有公式你只需用到式（0）（4）（5）（6）。

总结：

　　想象一下，你在一辆装有外部传感器的汽车里。 汽车传感器可以检测移动的物体：例如，传感器可能检测到行人，甚至是自行车。 对于多样性，让我们以自行车示例逐步执行卡尔曼滤波算法。
卡尔曼滤波器算法将执行以下步骤：

• 第一次测量 - 滤波器将接收自行车相对于汽车位置的初始测量值。 这些测量将来自雷达或激光雷达传感器；
• 初始化状态和协方差矩阵 - 滤波器将根据第一次测量初始化自行车的位置；
• 然后汽车将在一段时间Δt后接收另一个传感器测量值；
• 预测 - 算法将预测自行车在一段时间Δt后的位置。 在Δt之后预测自行车位置的一种基本方法是假设自行车的速度是恒定的; 因此，自行车移动距离为速度* Δt。 在扩展卡尔曼滤波器中，我们假设速度是恒定的; 在无迹的卡尔曼滤波器中，我们将介绍一个更复杂的运动模型；
• 更新 - 滤波器将“预测”位置与传感器测量值所示的位置进行比较。 将预测的位置和测量的位置组合以给出更新的位置。 卡尔曼滤波器将根据每个值的不确定性对预测位置或测量位置施加更多权重。
• 然后汽车将在一段时间Δt后接收另一个传感器测量值。 然后该算法执行另一个预测和更新步骤。

　　我们可以用这些公式对任何线性系统建立精确的模型，对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。

　　本期的无人驾驶技术之Kalman Filter就介绍到这里，下一期介绍扩展卡尔曼滤波技术，敬请期待。