嵌入式分享~卡尔曼滤波合集

这篇就只说卡尔曼滤波  嵌入式用的多些 就放这里啦~

一、卡尔曼滤波原理

  我不得不说说卡尔曼滤波，因为它能做到的事情简直让人惊叹!意外的是很少有软件工程师和科学家对对它有所了解，这让我感到沮丧，因为卡尔曼滤波是一个如此强大的工具，能够在不确定性中融合信息，与此同时，它提取精确信息的能力看起来不可思议。

1、什么是卡尔曼滤波?

    你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。

    在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点(除了前一个状态量外，不需要保留其它历史数据)，并且速度很快，很适合应用于实时问题和嵌入式系统。

    在Google上找到的大多数关于实现卡尔曼滤波的数学公式看起来有点晦涩难懂，这个状况有点糟糕。实际上，如果以正确的方式看待它，卡尔曼滤波是非常简单和容易理解的，下面我将用漂亮的图片和色彩清晰的阐述它，你只需要懂一些基本的概率和矩阵的知识就可以了。

2、我们能用卡尔曼滤波做什么?

    用玩具举例：你开发了一个可以在树林里到处跑的小机器人，这个机器人需要知道它所在的确切位置才能导航。

 注意这个状态只是关于这个系统基本属性的一堆数字，它可以是任何其它的东西。在这个例子中是位置和速度，它也可以是一个容器中液体的总量，汽车发动机的温度，用户手指在触摸板上的位置坐标，或者任何你需要跟踪的信号。

    这个机器人带有GPS，精度大约为10米，还算不错，但是，它需要将自己的位置精确到10米以内。树林里有很多沟壑和悬崖，如果机器人走错了一步，就有可能掉下悬崖，所以只有GPS是不够的。

 或许我们知道一些机器人如何运动的信息：例如，机器人知道发送给电机的指令，知道自己是否在朝一个方向移动并且没有人干预，在下一个状态，机器人很可能朝着相同的方向移动。当然，机器人对自己的运动是一无所知的：它可能受到风吹的影响，轮子方向偏了一点，或者遇到不平的地面而翻倒。所以，轮子转过的长度并不能精确表示机器人实际行走的距离，预测也不是很完美。

    GPS 传感器告诉了我们一些状态信息，我们的预测告诉了我们机器人会怎样运动，但都只是间接的，并且伴随着一些不确定和不准确性。但是，如果使用所有对我们可用的信息，我们能得到一个比任何依据自身估计更好的结果吗?回答当然是YES，这就是卡尔曼滤波的用处。

3、卡尔曼滤波是如何看到你的问题的

    下面我们继续以只有位置和速度这两个状态的简单例子做解释。

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

    在上图中，位置和速度是不相关的，这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣：位置和速度是相关的，观测特定位置的可能性取决于当前的速度：

  这种情况是有可能发生的，例如，我们基于旧的位置来估计新位置。如果速度过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远。跟踪这种关系是非常重要的，因为它带给我们更多的信息：其中一个测量值告诉了我们其它变量可能的值，这就是卡尔曼滤波的目的，尽可能地在包含不确定性的测量数据中提取更多信息!

4、使用矩阵来描述问题

  当然，在这里我们只用到了位置和速度，实际上这个状态可以包含多个变量，代表任何你想表示的信息。接下来，我们需要根据当前状态(k-1 时刻)来预测下一状态(k 时刻)。记住，我们并不知道对下一状态的所有预测中哪个是“真实”的，但我们的预测函数并不在乎。它对所有的可能性进行预测，并给出新的高斯分布。

  它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢?下面用一个基本的运动学公式来表示：

5、外部控制量

    我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。

6、外部干扰

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

    但是，如果存在未知的干扰呢?例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。

    在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”(即我们没有跟踪的干扰)之间的不确定性模型：

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

    由上式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。

    而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。

7、用测量值来修正估计值

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

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

    卡尔曼滤波的一大优点就是能处理传感器噪声，换句话说，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。

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

    现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。

    我们必须在预测值(粉红色)和传感器测量值(绿色)之间找到最优解。

    剩下的就是重叠部分了，这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的最优估计。        whaosoft  aiot  http://143ai.com 

    瞧!这个重叠的区域看起来像另一个高斯分布。

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

8、融合高斯分布

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

9、总结

    以上所有公式中，你只需要用到式(7)、(18)、(19)。(如果忘了的话，你可以根据式(4)和(15)重新推导一下)

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

二、卡尔曼滤波及其变种

从基础卡尔曼滤波到互补卡尔曼滤波

卡尔曼滤波自从1960被Kalman发明并应用到阿波罗登月计划之后一直经久不衰，直到现在也被机器人、自动驾驶、飞行控制等领域应用。基础卡尔曼滤波只能对线性系统建模；扩展卡尔曼滤波对非线性方程做线性近似以便将卡尔曼滤波应用到非线性系统。后来研究者发现将系统状态分成主要成分和误差，并将卡尔曼滤波用来预测误差，会使得系统的近似程度更高，效果更好。在姿态解算任务中，研究者们用辅助传感器如加速度计和磁力计来修正角速度计的积分结果，得到互补卡尔曼滤波的形式。

卡尔曼滤波是一种工具，对实际问题的不同建模方式会得到不同形式的卡尔曼滤波器。这导致了对于初学者不知道从何看起是好。另外也似乎很少有文章对基础卡尔曼滤波到各种形式的滤波形式做总结说明，于是便有了这篇文章。本文会从以下几个方面分析和讲解多种卡尔曼滤波器形式：

1. 基础卡尔曼滤波——对线性系统的预测
2. 扩展卡尔曼滤波——基础卡尔曼滤波在非线性系统的扩展
3. 基于四元素的卡尔曼滤波器——基于实际问题的讲解
4. 状态误差卡尔曼滤波——将状态误差引入状态向量
5. 互补卡尔曼滤波——一种只使用角度误差和角速度误差作为状态向量和测量向量的滤波器形式

符号定义

小写字母为变量；加粗小写字母为向量；大写和加粗大写为矩阵

基础卡尔曼滤波器

宏观认识

卡尔曼滤波包含两个步骤

1. 预测（prediction）—— Dynamic model
2. 更新（correction/measurment update）—— Observation model

所谓预测，就是用一个数学模型，根据当前的传感器输入，直接计算此时系统的状态。可以理解为一个方程的计算就行。

所谓的更新，就是在某些时刻或者每一时刻，获取一些系统的其他状态输入（我们将这个值叫做测量值），比较此刻预测的系统状态和测量的系统状态，对预测出的系统状态进行修正，因此也叫测量更新（measurment update）。

整体框架如下图所示

状态方程及测量方程

详细公式推导

本文作为一篇概述性文章，为了不使篇幅过于冗长，不进行基础卡尔曼滤波器公式的推导。想完全理解基础卡尔曼滤波器可以参考下面这几篇资料：

1. 卡尔曼滤波基础知识及公式推导——较为形象化地讲解预测和更新这两个过程之间地概率分布关系
2. wiki Kalman Filter——准确的公式化推导

如何理解卡尔曼滤波器？

从概率分布的角度

卡尔曼滤波器将系统状态的变化中的过程噪声假设为均值为0的高斯噪声，使得状态向量也变为一个符合高斯分布的随机向量。同时对观测过程的噪声也假设为均值为0的高斯噪声。通过测量方程，即公式（1-2）得到将状态向量映射到测量向量的函数。于是，当得到测量值的时候，可以利用测量值与状态向量之间的关系得出另外一个对状态向量的估计。利用测量值得出的状态估计和状态方程计算的状态均符合高斯分布，两个高斯分布的联合概率分布依旧保持高斯特性。进一步推导可以得到公式（1-5）到公式（1-7）。关于这个角度的理解可以阅读上面推荐的第一篇文章。

从最小化误差的角度

扩展卡尔曼滤波

非线性方程的线性近似

卡尔曼滤波器建立在线性的状态方程和测量方程也就是公式（1-1）和公式（1-2）。但是在实际应用中，更多的关系是非线形关系，比如如何从连续的角速度计算出车辆当前的姿态角等。为了能够利用基本卡尔曼滤波器的预测和更新过程，对于非线性的状态方程和观测方程，我们利用一阶的泰勒展开，将非线性公式近似为线性公式。

在公式（2-4）和公式（2-5）中：

基础卡尔曼滤波器 && 扩展卡尔曼滤波器总结

基于四元数的拓展卡尔曼滤波器

从实际问题讲起

在运动物体的姿态估计，比如车辆的姿态计算中，常常利用IMU(Inertial Meseasurment Unit)惯性测量单元计算物体的姿态。为了方便叙述，这里的姿态估计意味着我们希望解算车辆在每一时刻与起始坐标系之间三个轴的偏转角，通常用roll、pitch、yaw表示。

IMU(惯性测量单元)

求导的定义是函数值的微增量关于自变量的微增量的极限。表示旋转的单位四元数作差后，其不再是单位四元数，也就不是旋转四元数了

状态向量及控制输入

我们直接将车辆的姿态角以四元数形式表达作为系统状态向量

预测方程

由于在上面的推导中已经求出雅各比矩阵，所以预测方程可以直接根据拓展卡尔曼滤波公式写出来

测量更新

前文我们使用了角速度计的输出作为控制输入，并构建了状态方程和预测方程。IMU通常都会有加速度计输出，这部分输出可以用来作为测量量，并对预测的状态进行测量更新。我们先回顾测量更新中状态向量的更新过程。

测量模型

更新方程

状态误差卡尔曼滤波器（ErKF : Error-state Kalman Filter)

概述

在使用卡尔曼滤波器做姿态估计（Attitude Estimation）中，很大一部分都采用不是直接将系统姿态角作为卡尔曼滤波的状态，而是将姿态角的积分误差和角速度计的误差作为系统状态。将角速度计的输出弥补上估计出的角速度计误差，然后对其积分，得到姿态角的估计，再弥补上姿态角的误差估计。整个的流程图大概如下面的图，引用自Intertial Head-Tracker Sensor Fusion by a Complementary Separate-Bias Kalman Filter

PS: 要强调的是，各种卡尔曼滤波的形式多种多样，同时各种符号的定义也都并不完全一致，这也是入门卡尔曼滤波比较难的地方，有时候找资料都不知道怎么找。这也是写这篇文章的目的，提供一个基础的脉络给卡尔曼滤波的初学者。因此这里给出的ErKF只是形式之一，主要是引用自论文Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation

状态误差的递推公式

于是，有了状态误差的递推公式，我们就可以像卡尔曼滤波一样推导预测和更新过程

预测过程

与直接对系统状态做卡尔曼滤波稍有不同，使用误差状态的卡尔曼滤波在计算姿态角的时候可以看成三步：

1. 在卡尔曼滤波系统外使用积分算出此时的系统状态
2. 使用卡尔曼滤波算出此时系统状态的误差
3. 将积分出来的系统状态弥补上卡尔曼滤波计算出误差

系统状态计算

测量方程及更新方程

补充

在上面的推导中，将我们的目标变量，即系统的姿态角作为外部一个单独的积分计算，但是实际上更多的做法是将姿态角和角速度的偏差直接放在状态向量中进行计算。然后对每一时刻的角速度偏差应用到角速度计的输出，再将其作为系统输入应用到状态方程。也就是像概述中的图示那样。但是其实各种卡尔曼滤波的建模方式都不一样，Error-state Kalman Filter和Complimentarty Kalman Filter也没有严格的定义。所以索性这一章节当作对状态误差的理解和推导，在下面的互补卡尔曼滤波给出一种似乎是应用更广泛的卡尔曼滤波器。

互补卡尔曼滤波

前言

正如前文所说，卡尔曼滤波器的建模形式多种多样，而且很多研究也是在上世纪，对于误差状态卡尔曼滤波（Error-state Kalman Filter)和互补卡尔曼滤波（Comlimentary Kalman Filter)其实没有严格的定义。这里的互补卡尔曼滤波其实也可以看成上文ErKF的另一种形式。主要采用自论文Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter。卡尔曼滤波的工作太多，博客和论文也五花八门，看起来十分不易。这篇论文从引用、论文叙述、符号标识看起来都很不错，很适合想要将卡尔曼滤波应用到姿态估计的工程师阅读。甚至有一些工作，直接使用普通卡尔曼滤波输出，然后利用互补滤波器的概念，在多种姿态输出之间做加权平均，也叫互补卡尔曼滤波器，比如这篇专利：一种基于互补卡尔曼滤波算法计算融合姿态角度的方法

互补的概念

从加速度计计算姿态角roll、pitch

加速度计（Accelerometer）可以输出三个轴的加速度，在静止的情况下，三个轴的合向量就是重力加速度。因此，我们可以利用三个轴加速度之间的关系计算静止状态下的俯仰角（pitch）和翻滚角（roll）

关于如何推导从三个轴的加速度计算roll和pitch，可以看这篇文章

最后得出的形式也非常简单：

从磁力计计算姿态角yaw

具体的计算公式可以看这篇博客

从加速度计算的姿态弥补

从加速度计可以计算出roll角和pitch角，因此，可以将这个结果和角速度的积分结果结合起来，得到一个更好的估计姿态。不过要注意的是，从加速度计算的姿态弥补有两个局限：

1. 加速度计只能计算出Roll角和Pitch角，因此yaw角无法得到互补信息
2. 当车辆处于较大的加速度运动状态时，三轴加速度的合向量跟重力加速度会有偏差，因此互补结果应该根据这个偏差的大小做改变。

互补滤波器

互补滤波器使用角速度的积分结果和加速度与磁力计的计算结果进行插值，得到更好的结果

互补卡尔曼滤波器

互补卡尔曼滤波器将姿态角的误差、角速度的误差当作状态向量；并利用其余传感器如加速度计和磁力计计算出的姿态角与估计的姿态角之间的差作为测量量。通过以下步骤得到系统的姿态角：

1. 卡尔曼滤波器输出姿态角的误差和角速度的误差
2. 将当前时刻角速度的输出加上角速度的误差，并利用积分公式算出姿态角
3. 将步骤2算出的姿态角加上步骤1输出的姿态角误差

状态向量和测量向量

状态向量

预测&&更新过程

有了上面状态方程和测量方程的推导，剩下的就是按照公式（4-3）到公式（4-10）的过程代入。这里唯一不同的就是卡尔曼滤波输出的向量是角速度的误差和姿态的误差。在计算姿态的时候先将角速度的误差应用到角速度计的数据，对角度积分，将角度的误差应用到角度积分结果，得到最终的角度输出。整个框架如下图

后话

1. 卡尔曼滤波是一个很古老的算法，但同时又是被广泛应用的算法。即使在今天姿态解算中很多用了因子图，但是对IMU的预积分依旧要使用卡尔曼滤波。但是卡尔曼滤波算法只是一个工具，不同的系统建模方式会产生不同形式的卡尔曼滤波器，这也导致了初学者不知道从哪里入手。
2. 在查资料的过程中发现，卡尔曼滤波的一些变种如Error-State Kalman Filter和Complimentary Kalman Filter其实并不是严格定义的。
3. 笔者对卡尔曼滤波并没有很丰富的应用经验，本身也不是专门做运动控制和姿态解算的。本文的叙述在追求通俗易懂之外尽力保证公式和符号定义的准确。但无法保证没有错误。
4. 对于卡尔曼滤波器中各个变量的初始值设置是门学问，论文中都会有独立的章节讲述初始值如何设置。这方面可能得结合实际应用和效果得出最优的方案。

Reference

[1] Roll and Pitch Angles From Accelerometer Sensors

[2] 四元数、欧拉角、旋转矩阵转换

[3] 四元素乘积求导

[4] 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法

[5] Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter

[6] Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation

[7] Kalman Filter的原始论文

[8] 卡尔曼滤波基础知识及公式推导

[9] AHRS: Attitude and Heading Reference Systems

原创作者: whaosoft 转载于: https://blog.51cto.com/whaosoft/11642611