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

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

# 理解卡尔曼滤波 Kalman Filter？

Kalman 系列

认识kalman是大二做车时知道这个东西可以用来做传感器滤波，后来慢慢知道KALMAN大神几乎是创立现代控制方法，而一开始的kalman滤波器是为了阿波罗计划而提出的。

相比于复杂的数学公式和艰深的理论，我更喜欢直观的理解，尤其是用图的方式。

卡尔曼滤波器提出的背景

火箭在发生过程中需要时刻预测自己的状态（位置，速度），我们可以用一些传感器获得这两个量，但是由于存在电离层和大气干扰，传感器的测量值有时误差会非常大，我们需要一种方法，预测一个新的更加准确的、且不完全依赖传感器返回值的状态信息。

事实上，如果传感器的返回值非常离谱，那我们完全有理由不相信此时的数据，因为火箭运动需要 符合某个运动方程，即使存在一些干扰，火箭的状态也应该是在某个合理的范围内的。

一个顺理成章的思路是：把传感器观测值和理论预测值做加权平均。既不完全相信传感器，也不完全相信理论模型

上式的意思就是把由前n-1个时刻得到的n时刻的理论值和n时刻的观测值进行加权平均。

接下来的问题是：如何确定 α?

——为了解决这个问题，我们不妨从最简单的情况出发，即设计一个1维kalman滤波器下手。

一维KALMAN滤波器

假设我们只需要观测一个值，即设计一个1维Kalman Filter。

由于理论模型和观测都是存在相应干扰的， 假设二值均服从正态分布（我们生活中几乎到处都是正态分布）。

红色是理论值分布，绿色是观测值分布。(这里注意，理论值也是一个概率分布，这是因为我们的理论模型肯定不能完全吻合实际情况)

新的值应该结合这两者，即像蓝色分布那样。

现在只考虑均值 μ,一个很直观的思路是：变化更平稳（不确定度更低）的值可信度应该更大，即方差越小权重应该越高。同时保证总权重为1，则有：

可以发现这样算出来的答案和之前的想法不谋而合了！

理论模型值的概率分布 和观测值的概率分布只能通过大量数据采样得到，这要求我们在应用上述方法进行滤波前，应该通过大量观察对理论模型和观测数据的分布有一个准确的了解。实际工程我们一般假设传感器是具有某个确定分布的，这个分布可以对传感器进行一段时间的观测得到（如slam中经常会让系统静止两小时以计算传感器的分布）。而系统方差是会随着我们每一次进行kalman融合而改变的（因为状态矩阵a的影响），我们需要持续的计算系统方差——迭代。

迭代，迭代，迭代

我们能做的就是在系统运行开始后计算每一次的理论值方差，根据这个方差和传感器方差计算出融合权重，每次都要重新计算————迭代。

状态更新方程

当我们得到t时刻的kalman估计值，我们需要引入状态更新方程，得到新的理论值,同时计算理论值的方差。

上式将α改为k，以符合kalman理论的通用符号表示，下同。

公式（3） 就是1维的KALMAN增益计算公式，所谓KALMAN增益K就是有多相信当前的测量值。

观测矩阵H

同时，实际模型中的测量值一般不是状态变量，二者存在关系：

拓展到多维——就是用协方差矩阵 P 替换σ

简而言之，协方差矩阵就是多维情况下的方差。直接用P替换σ即可（H为放方阵时，不为方阵的特殊情况见后文）。

网上传播的很广的5个公式

结束了吗？

没有,推导从不代表问题被解决，我一直贯彻的想法就是把公式化为直觉，才算真正学到了。

关于kalman滤波器的学习，我一直是断断续续，好似明白了，等过一阵子，在拾起来却又发现某个地方之前是完全没有理解到位的，依稀记得大二第一次接触kalman滤波器时，我抱着一本从学校图书馆借来的《kalman滤波器原理与matlab设计》，在回家的绿皮火车上看了几十页，似懂非懂，浑浑噩噩，再到后来，因为做小车要用到，便将网上流传甚广的两元kalman滤波器代码down了下来，放在了小车直立平衡环的角速度滤波上，当时只觉得kalman好像确实比一阶互补滤波要更稳，但是小车需要晃一段时间才能逐渐收敛到稳定的状态，那是我第一次对kalman的迭代性质有一个切实的认识。

现在想起来，当时我对kalman的似懂非懂就是卡在了协方差外推公式中，为什么要左乘一A矩阵右乘A矩阵的转置上，现在想起来真是由于当初自己的线代知识不够牢固而导致的贻笑大方的问题。

后来，就是研究生阶段再读kalman滤波器，便有了此文，也是修修改改反反复复多次，终于自认是推导明白了，但过程中也是出现了较多曲折，走进过一些误区，分享出来，警示一下大家。

• kalman滤波器是具有markov性质的，但是其是综合了所有历史信息的，因此不是简单的从上一时刻预测下一时刻
• kalman滤波器更像是一个做融合问题的方法，而不是预测，其对传感器有一定要求，传感器的误差均值应为0，而预测类问题往往是无法对系统有一个准确观测
• 除了状态外推方程外，kalman滤波器的变量就只剩Q,R了，其中Q代表的是系统预测的不确定性，R代表的是传感器的不确定性

直观理解kalman

kalman滤波器的核心就是加权融合，如果将状态更新方程写成连续形式,设 Δt=1

示意图

状态变量的变化，绿色为真值，蓝色为融合值，+号为误差符合正态分布的测量

方差σx的变化，逐渐变小

Q,R 的影响

下面的实验也是1维的，假设A=1,H=1,真实x=-0.37，初值为0.

增大R

随着R越大，收敛的越慢

R越小，会相对快速的相信传感器的值，所以会比较快的收敛的真实值附近。上图可以看到，随着R增大，收敛的越慢，系统惯性变大了，因为一开始系统初值为0 ，随意会很缓慢的到真实值附近。

但是随着R越大，并联后的阻值开始的时候就相对越大，即系统方差一开始也会较大。如下图

而根据前面的结论，如果系统方差较大，那么也会更相信传感器。总结一下，R变大了（要减小对传感器的信赖），导致滤波初期Q也会相对变大，初期Q变大了，应该会导致更相信传感器，这不是矛盾了吗？但是其实没有。千万不要陷入这样的误区，要记住P增大是R增大带来的影响，R是自变量，是主要原因，并且是先变化的，即使R导致初始Q变大了，也只是副作用而已，其影响不会大过R带来的。

增大Q

随着Q变大，越来越相信传感器的值

总结一下，有以下结论：

• 增大R，即传感器测量方差，则系统惯性大
• 增大Q，即系统理论方差，则系统惯性小

kalman增益的变化

kalman增益会慢慢收敛到某个固定值，这也是互补滤波和kalman效果一样的原因。而k值的变化也会受P，Q，R的初值影响，不过一般而言都会随着迭代次数增加而慢慢收敛。

图中黄色的为kalman增益

P的初值，状态的初值的影响

实际工程中,不光要合理设置Q,R，也要合理设置P，和X的初值，其中Q,R的相对值更加重要，而且数量级要正确，只要数量级正确，都会慢慢收敛的，这一部分本文暂且不表。

kalman总结：

从上面的实验可以大致看出，kalman需要保证观测值分布必须保证0均值分布，不能有固定误差，否则一定会带偏估计值，同时kalman初期是需要时间收敛的，收敛的快慢取决于参数的整定，这也解释了我大二做车时的疑惑，当初进行角速度滤波时，kalman方法会导致小车一开始摇摆的相对剧烈，而一阶互补滤波就不会有这种问题。但是后期kalman增益收敛了，就会稳定。

贝叶斯滤波

挖坑待填。。。。

二维系统

我们假设Q,R是恒定的，如果还是以匀速运动模型为例（假设状态变量有两维，位置和速,且我们只能观测到位置）则有:

因为协方差矩阵是对称的，所以写成上式不影响最终结果。

补充：

笔者特地翻看了opencv kalman filter的源码，多维变量公式（3）中计算kalman增益时，分母求逆的计算是用最小二乘法得到的，这样可以保证分母出现不可逆的情况时也能用违逆代替正常计算。

其次，使用opencv 或者pykalman 进行滤波时，Q的维数是和状态变量保持一致的，R时和观测变量保持一致的。

二阶系统Q,R的初值怎么给？Q,R的值可以很大吗？初始值对系统的影响如何？运动模型建立错误对结果的影响如何？

笔者做了一个简单的实验，有兴趣的可以转战github看一看 github.com/Karmaadonis/kalman_filter/blob/main/kalm

# 卡尔曼滤波及其变种有哪些

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

卡尔曼滤波自从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. 将积分出来的系统状态弥补上卡尔曼滤波计算出误差

系统状态计算

上式只是一个公式化表达，其实就是将上一时刻的状态，在加上这一时间段状态的变化量。姿态估计中，状态的变化量通常是角速度计的输出乘以时间间隔。

状态误差方程及预测方程

状态误差方程

由公式（4-1）可以得到状态误差的方程为

测量方程及更新方程

补充

在上面的推导中，将我们的目标变量，即系统的姿态角作为外部一个单独的积分计算，但是实际上更多的做法是将姿态角和角速度的偏差直接放在状态向量中进行计算。然后对每一时刻的角速度偏差应用到角速度计的输出，再将其作为系统输入应用到状态方程。也就是像概述中的图示那样。但是其实各种卡尔曼滤波的建模方式都不一样，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/11639460