卡尔曼滤波的使用举例

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

 

顺带提一下协方差矩阵，下面会用到。

协方差矩阵：

 

以下来源：https://blog.csdn.net/su_bao/article/details/80997077

 

卡尔曼滤波的作用：可以在任何具有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向进行有根据的预测，即使伴随各种干扰，卡尔曼滤波也总能进行比较精准的预测。在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小（除了前一个状态量外，不需要保留其它历史数据），速度快等优势，很适合应用于解决实时问题和用于嵌入式系统。

卡尔曼滤波的应用举例：机器人自我导航

可以说机器人有一个状态 ，表示位置和速度：

注意这个状态只是系统基本属性的一堆数字，它可以是任何其它的东西。在这个例子中是位置和速度

这个机器人带有GPS，精度大约为10米，它需要将自己的位置精确到10米以内。只有GPS是不够的。

我们并不知道实际的位置和速度，它们之间有很多种可能正确的组合，但其中一些组合的可能性较大。

卡尔曼滤波假设两个变量（位置和速度，在这个例子中）都是随机的，并且服从高斯分布。每个变量都有一个均值μ，表示随机分布的中心（最可能的状态），以及方差 ，表示不确定性。

现实中位置和速度是相关的，观测特定位置的可能性取决于当前的速度。例如，我们基于旧的位置来估计新位置。如果速度过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远。

跟踪这种关系是非常重要的，因为它带给我们更多的信息：其中一个测量值告诉了我们其它变量可能的值。

卡尔曼滤波的目的就是，尽可能地在包含不确定性的测量数据中提取更多信息！ 

这种相关性用协方差矩阵来表示，简而言之，矩阵中的每个元素  表示第 i 个和第 j 个状态变量之间的相关度。（协方差矩阵是一个对称矩阵，这意味着可以任意交换 i 和 j）。协方差矩阵通常用“”来表示，其中的元素则表示为“ ”。

使用矩阵来描述问题：

我们基于高斯分布来建立状态变量时刻 k 需要两个信息：最佳估计 （即均值，其它地方常用 μ 表示），以及协方差矩阵  。 

接下来，我们需要根据当前状态（k-1 时刻）来预测下一状态（k 时刻）。

我们并不知道对下一状态的所有预测中哪个是“真实”的，但我们的预测函数并不在乎。它对所有的可能性进行预测，并给出新的高斯分布。 

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。

 

现在，我们有了一个预测矩阵来表示下一时刻的状态，但是，我们仍然不知道怎么更新协方差矩阵。此时，我们需要引入另一个公式，如果我们将分布中的每个点都乘以矩阵 A，那么它的协方差矩阵  会怎样变化呢？很简单，下面给出公式：

外部控制量：

可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。

如在我们机器人这个例子中，导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息，我们可以用一个向量来表示，将它加到我们的预测方程中做修正。

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

　　以矩阵的形式表示就是： 

称为控制矩阵，称为控制向量（对于没有外部控制的简单系统来说，这部分可以忽略）。

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。 但是，如果存在未知的干扰呢？

 

外部未知干扰：

如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。 
　　在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型： 

原始估计中的每个状态变量更新到新的状态后，仍然服从高斯分布。我们可以说的每个状态变量移动到了一个新的服从高斯分布的区域，协方差为。换句话说就是，我们将这些没有被跟踪的干扰当作的噪声来处理，我们用协方差为表示这种噪声。

这产生了具有不同协方差（但是具有相同的均值）的新的高斯分布。 

我们通过简单地添加得到扩展的协方差，下面给出预测步骤的完整表达式： 

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

好了，我们对系统可能的动向有了一个模糊的估计，用和来表示。如果再结合传感器的数据会怎样呢？

用测量值来修正估计值

我们可能会有多个传感器来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息。

注意，传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，我们用矩阵  来表示传感器的数据。 

我们可以计算出传感器读数的分布，用之前的表示方法如下式所示： 

我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。

从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。 

我们将这种不确定性（例如：传感器噪声）用协方差表示，该分布的均值就是我们读取到的传感器数据，称之为。 
现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。

我们必须在预测值（粉红色）和传感器测量值（绿色）之间找到最优解。 
　　那么，我们最有可能的状态是什么呢？对于任何可能的读数，有两种情况：（1）传感器的测量值；（2）由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。

剩下的就是重叠部分了，这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的最优估计。 
　　瞧！这个重叠的区域看起来像另一个高斯分布。

如你所见，把两个具有不同均值和方差的高斯分布相乘，你会得到一个新的具有独立均值和方差的高斯分布！下面用公式讲解。

融合高斯分布：

先以一维高斯分布来分析比较简单点，具有方差  和 μ 的高斯曲线可以用下式表示： 

　　如果把两个服从高斯分布的函数相乘会得到什么呢？ 

　　将式（9）代入到式（10）中（注意重新归一化，使总概率为1）可以得到： 

 

下面进一步将式（12）和（13）写成矩阵的形式，如果 Σ 表示高斯分布的协方差， 表示每个维度的均值，则： 

　　矩阵称为卡尔曼增益，下面将会用到。放松！我们快要完成了！

将所有公式整合起来：

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

　　由式（14）可得卡尔曼增益为： 

将式（16）和式（17）的两边同时左乘矩阵的逆（注意里面包含了  ）将其约掉，再将式（16）的第二个等式两边同时右乘矩阵  的逆得到以下等式： 

上式给出了完整的更新步骤方程。就是新的最优估计，我们可以将它和放到下一个预测和更新方程中不断迭代。

 

总结

　　以上所有公式中，你只需要用到式（7）、（18）、（19）。（如果忘了的话，你可以根据式（4）和（15）重新推导一下） 
　　我们可以用这些公式对任何线性系统建立精确的模型，对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。