卡尔曼滤波matlab与c语言的代码以及对其的理解

卡尔曼滤波的matlab与c语言的代码

Gitee代码仓库： https://gitee.com/yang-xiaolongzz/kf.git
具体的matlab与c语言的相关代码我已放置gitee仓库， https://gitee.com/yang-xiaolongzz/kf.git
matlab版本的代码是学习自b站的“老实忠厚的老王”的视频后理解与改进的算法
：下面我将讲一讲我个人对卡尔曼滤波的理解，有对我算法疑惑或者想了解其参数如何整定的可以继续进行阅读
（在这里先放一张matlab跑出的卡尔曼滤波算法的效果）

---

提示：如果只是单纯想滤陀螺仪数据的，那么可以直接将上述代码当盲盒使用。而如果想写出符合自己实际需求的卡尔曼滤波器算法，个人建议采用如下步骤：1.模型分析 2.matlab的算法实现3.基于matlab验证后的代码进行c或c++代码的移植

---

前言

为什么一定要经过matlab实现之后在写c语言版本的？
是因为C语言版本的卡尔曼滤波大多数都是将矩阵以及矩阵运算经过二次计算化简省略的，其目的是为了更快的运行速度，换句话说，C语言版本的算法主要是给机器看的，而不是给人看的，这就导致很多人一上来就看C语言版本的卡尔曼滤波算法，搞得一头雾水。
而matlab的代码大多保留了其矩阵的原本形式与计算过程，所以无论是阅读还是调参，matlab版本的代码都有助于你对卡尔曼滤波有着更深刻的认识

---

下面，想要深入了解卡尔曼滤波算法之前，我们先简要谈谈卡尔曼滤波的真实原型-------贝叶斯滤波

一、贝叶斯滤波模型

如果想要深入了解贝叶斯滤波的推导及原理的可以看这篇博客，讲的很详细
你真的搞懂贝叶斯滤波了吗
在这里，想搞懂卡尔曼滤波，我们只需要对贝叶斯滤波做一个大概的认识：

• 贝叶斯滤波认为一切事件都没有明确的信息点，只有该信息的期望值与方差
  也就是说，我们的模型所估计出的信息都是以该信息的平均值和他对应的方差。

• 贝叶斯滤波，说到底就是利用条件概率公式来进行推测，更新来递归使得每次递归后
  其预测模型的协方差减小，而逐渐收敛。所以卡尔曼滤波里的先验概率，后验概率都是来源于此。

• 为什么没听过贝叶斯滤波算法？只听过卡尔曼滤波或者粒子滤波这类算法？
  这是因为，贝叶斯滤波中的三大概率：“先验概率”，“后验概率”，”似然概率“ 的求解都涉及无穷积分的运算，这使得该算法在实际运用中根本不可能实现

• 因此很多人在这个理想化的模型之上，做了近似化简或者等效替代等处理，从而衍生出了众多滤波算法。如本文所谈的卡尔曼滤波，粒子滤波，直方图滤波等等，他们本质都是贝叶斯滤波，只不过是在不同条件下对贝叶斯滤波进行化简或者延伸。

二、卡尔曼滤波模型的提出及其条件

上文说过。卡尔曼滤波模型最开始设计的目的，就是为了解决贝叶斯滤波在实际应用中的无穷积分问题而产生的。其中系统线性化便是经典卡尔曼滤波最显著的特征

• 卡尔曼滤波认为，对于一个系统，他的状态方程方程矩阵（也就是代码中的F矩阵）与观测方程矩阵（代码中的H矩阵）都为线性矩阵，具体原因下面会讲。
• 其认为，在该系统中，状态预测噪声与观测器观测噪声，均是服从均值为零的高斯噪声（也就是服从正态分布）

以上两条假设，使得在滤波过程中的计算均为高斯闭环计算，也就是说不管是预测还是更新，得到的新的事件仍然是服从正态分布的。也正是这点从而大大简化了运算量。

详细的公式推导可以参考这篇博客：从概率学看卡尔曼滤波

在介绍了卡尔曼滤波算法的由来以及其条件之后，我们来进行代码的讲解。

---

三、如何从写出符合实际情况的卡尔曼滤波器？

1.利用matlab进行模型建模

在matlab建模中，最重要的一点便是矩阵的阶数大小问题，这也是为什么要先用matlab写出算法验证后，再将其转换为c语言代码。因为自己第一遍写的代码很有可能因为矩阵病态等问题，导致无法滤波，甚至有时会迭代发散等，这些都是建模时常见的问题

卡尔曼滤波一共分为两个步骤，第一步为预测步，第二步为更新步。以上两部循环往复以实现迭代滤波的效果

• 构建预测方程

• 其中对F矩阵的选取，有很多种形式
  其实在卡尔曼滤波的矩阵形式中，对于预测方程而言，是由两个矩阵构成的，即 状态转移矩阵F 与 控制矩阵B。
  状态转移矩阵F是指目标上一时刻状态与下一时刻状态信息的关系
  控制矩阵B是指在建模中可以利用到的有效的对目标状态进行改变的量，如加速度等
  即如果对一个小车的车速进行滤波，你可以把加速度计读到的加速度信息作为观测器的输入使用，也可以将其作为控制矩阵的控制量进行输入。

• 在这里，我们不使用控制矩阵，也就可以默认F为我们的预测方程矩阵。并且为了深入研究，我们的F矩阵采用了三阶离散形式泰勒公式进行预测模型下一时刻的位置信息。所以这里的状态向量有三个，也就是位置，位置的导数，位置的二阶导。

• F矩阵具体数据填写：首先该矩阵为三阶方阵，书写顺序方式按上面图里的顺序写就好，如果看不懂的需要去补习一下矩阵的乘法。

读到后面或许会发现，由于观测到的信息只有位置信息和速度信息，那么就缺少了位置的二阶导的观测信息。因此，仅从滤波效果来看，将三阶的预测矩阵F降阶为二阶矩阵后（即只预测位置与速度）滤波效果基本没有变化。
那么在这种情况下的三阶预测方程就真的一点意义都没有么，在用matlab实际验证后你就会发现。在三阶的情况下，即使没有加速度计的观测下，却能预测出相对准确加速度信息。
并且之所以在这种情况下，二阶滤波与三阶滤波效果相差无几，那是因为本身加速度对于位置信息的“贡献”远小于速度与位置信息，故在实际应用时可以将为二阶。

• 观测方程H矩阵的建立
  这里需要注意，观测方程H的阶数并不是单纯指你的传感器能观测到的数据的种类。其阶数应与预测方程里的状态变量的种类保持相等，即上述中，在预测方程中设置三个变量；哪怕观测器只观测到了两个变量，也必须写为第三行均为零的三阶方阵，而不是二阶方阵，否则便会导致矩阵病态，出现运算错误。

接下来重点讲一讲Q矩阵与R矩阵的意义与调试方法，这也是很多人关心的，卡尔曼滤波参数的整定问题

• 预测噪声协方差矩阵 Q
  首先，在上面的介绍中我们知道，在卡尔曼滤波里假设认为一切事件都是以正态分布的概率形式出现的，那么也就必然引出两个概念，均值与方差。所谓均值便是我们预测与更新的目标。而方差（多变量是叫协方差）其最直观的物理含义，便是不确定度。
  对于预测噪声协方差Q来讲，其数值越大，表示对预测模型所预测的数据的不确定性越高，也可以说是对预测的置信度越低。所以，Q的值给的越小，则表示越相信预测模型所给出的期望值，滤波出的结果也就越贴合预测模型
  这里需要说明一点，在网上有人质疑，在查阅了协方差的定义后发现协方差矩阵里，为什么数值可以大于一？明明协方差不能大于一啊。这其实还是对协方差矩阵的定义没有搞清楚，所谓协方差矩阵，其主对角线上的元素，表示的是该变量自身的方差，只有主对角线之外的元素才表示协方差，其物理含义是变量直线的关联程度；我们先前所说的代表不确定度的Q参数，仅仅是针对主对角线上的元素而言的。所以通常我们的协方差矩阵里除了主对角线上以外的元素都为零。
  那么，主对角线之外的元素永远都为零嘛？事实上在及特殊的情况下可以赋值，那便是在你清楚两个状态变量之间不独立时；比方说你认为一个小车的速度与他的位置相关联(即速度为1m/s时位置一定为正之类)。但在通常绝大多数情况下，我们讨论的状态变量之间都是独立的。

• 观测噪声协方差矩阵 R
  观测噪声的协方差矩阵与上述预测噪声协方差Q的含义基本相同，R矩阵主对角线上的数给的越小，滤波出的值就越接近观测值。这里需要明确，所谓的大小均是Q与R中相对应的参数所对比的大小；比如Q与R中的主对角线上的第一个元素，他们俩之间的比值，如果Q比R越大，则滤波出的数据就越贴近观测器所观测的数据，反之则更贴近预测模型所预测的数据。

• 预测与更新协方差矩阵
  由于这两个矩阵在实际调参中并无多大作用，其作用只是在更新迭代中记录着每一次预测与更新出目标事件的方差大小，用于计算卡尔曼增益。
  这里需要注意。在实际代码构建这两个协方差矩阵时；预测协方差矩阵并不需要设为静态变量（c语言），换句话说就是，预测协方差只在每次循环内部作用。而更新协方差矩阵需要有记忆性，记忆上一次循环所给出的协方差数值。

• 卡尔曼增益K
  在这里，并不会像网上其他人意义对卡尔曼增益做过多的解释，因为如果你手算并了解了卡尔曼增益后你就会发现，它的作用效果完全取决与Q与R 参数大小的选取。

至此，该算法基本参数变量就设置完毕了，具体的调试心得与对其算法的深入理解将在下面讲述

2.对卡尔曼滤波中预测模型的建立与参数整定的理解

• 模型建立
  在之前有大致讲述预测模型的含义，但并未深究，下面我们来探讨一下，究竟该如何选取预测模型，预测模型的不同又会带来怎样的影响？

首先，大家觉得，如果我就仅仅将预测模型设为Xk = Xk-1（也就是认为预测出的状态与上一时刻保持相同）会如何？我们的第一反应是，很不靠谱，毕竟没有什么信息是保持一尘不变的，若真如此，我还滤他作甚？但我们需要以实践检验真理，下面是我将上述预测模型作为预测方差，写出的一阶卡尔曼滤波的效果图：
是不是感觉，还行？但仔细观测会发现，在滤波的后段，滤出的波形整体较真实值偏下，这又是为何？

---

首先来解释第一个问题，为什么在预测模型如此扯淡的情况下？仍有相对还行的滤波效果？ 其实这也是卡尔曼滤波的精髓所在，即它能在预测模型与观测数据都不准的情况下，给出一个相对较准的期望值；也就是说，预测模型与观测数据两者中只要包含着有效的信息，那么在卡尔曼滤波的融合下，给出的预测值就会相对于观测者更为准确。 这时有人会问，这个 **Xk = Xk-1**模型，和我们真实信号有什么关系？它所包含的信息又是什么？我个人认为，这个模型所包含的对于真实信号而言的有效信息，是 **平滑** 这一特性。也就是说虽然这个预测模型的趋势走向与我们真实信号可以说没有什么任何关联，但它与真实信号都有一个共同特点-----就是平滑的，而非突变信号。所以说我个人认为，取这样一个预测模型的卡尔曼滤波事实上与**平滑滤波**的效果相差无几。 而这样粗糙的模型，弊端也是显而易见的，那就是总会比真实信号滞后一些；如果想要减小这种滞后的“趋势”就需要调小R参数，调大Q参数，这样做的意义是降低预测模型的置信度，也可以理解为权重。但这样做之后滤出的波形便没有之前那也平滑了。读者可以自行调节试一试这种最简单基础的模型的卡尔曼滤波，这对于理解卡尔曼滤波参数的整定有着很大的帮助。 在几番调试后就会发现，Q与R的比值大小决定了滤波后的平滑程度与滞后程度，而这两者又像位于跷跷板两端一样，一方的效果好，另一方的效果便变得差了。

---

如若想解决这一问题，则必须修改我们的预测模型，一般情况下，扩展至二阶预测模型就足够了。
而在我们的matlab代码里使用了三阶预测模型，仅是为了探讨研究，实际滤波效果与二阶相差无几，故在c语言版本的代码里采用了降阶的二阶版本滤波算法。
下面我们简单讲讲二阶预测模型时，参数Q与R 中各参数的调节。
第一，我们做一个约定，Q1表示三阶方阵Q的对角线上第一个元素，Q2,R1等同理。
其次，这里Q与R参数的意义与之前一阶时意义相同，即其大小代表着对于噪声（观测）值的方差，也就是不确定度。
最后，也是最重要的是，但凡我们讨论Q与R参数的大小是，都是指相对大小，也就是对应参数的比值。例如将Q1的值调大事实上真正作用的效果是，Q1与R1的比值变大所作用的。
在我个人经验来看，对于我们这个基于泰勒展开的线性预测模型而言；第一阶的参数也就是Q1与R1这一对参数的比值大小直接决定了滤波后的波形的平滑程度。而第二阶参数对Q2与R2的比值大小，影响着滤波的动态响应（也就是延迟性质）。也正是这一特性完美解决了之前那个一阶的粗糙模型的弊端（上文提到的“跷跷板效应”）。即他将滤波的平滑特性与响应跟随特性对应的参数分离成了两个，不再互相制约。
最后，虽然调节大小是，我们看的时参数对Q1与Q2（R1与R2）的比值，但这两对参数之间的数量级也会影响滤波效果。例如：如果Q参数对的数量级都是0.01级别的，而R参数都是个位数级别的，那么如果想要滤波的响应跟随效果更好，则应该将R参数对的两个参数整体改小，着相当于增加了滤波目标的一阶导（速度）在融合时相对于目标占比的权重，使其响应更加迅速。

事实上，再实际应用调节时我们会发现，具有明显调节效果的，就是改变参数对整体数量级来调节的方法，而改变参数对内部比值的作用更多的时微调曲线的平滑程度等（当然前提是参数对的比值要大致合适，不能过于离谱）。
而在调节时往往也应该先调节整体的数量级，只不过这里为了讲解，理清概念而先讲了参数对之间的比例之间的关系。

---

• 参数大致初值的选取
  之前我们讨论了参数如何调节，但仍有很多人纠结参数最开始该如何设置呢？
  首先，我们先说最简单的，也就是预测协方差矩阵 与第一次滤波时期望值初值的设立，
  事实上它的大小并不会影响整体滤波效果，因为随着滤波次数的增加，无论是协方差还是目标期望值都会收敛与最佳值，故可以随意给定。
  而最关键的也是很多人关心的，也就是预测噪声Q与观测噪声R的大致初值的选取。
  在初值设立时，还是遵守之前调节参数时所谈到的两个原则
• 参数对内部的比值
• 参数对之间的数量级
  首先，如果，不用过于在意R参数是否真的对应你的测量器噪声的实际协方差，更没有必要专门取收集测量数据的噪声，再计算它正态分布的协方差。因为你要清楚，无论是Q还是R；最终作用于滤波的效果，都是通过他们来计算出卡尔曼增益K作为权重将期望值与测量值比例加权得到的。那么即使你的R参数比实际测量器的噪声大几个数量级，那么Q参数相对于也增大几个数量级后，效果也是一样的。
  所以，我们可以大胆的给Q小数点一位或者两位（例如0.01），给R给到个位数（例如1）
  并且，大部分情况下，Q一般会给的小一些，R也一般都在个位数的数量级
  在大致的粗糙给值后，再根据之前说的方法进行调节，微调等。

总结

卡尔曼滤波的形式多种多样，光是预测模型，一种情况就可以写成好几种形式，具体要怎样设立，怎样使用，还需要实践检验才行，这也是为什么我会给出matlab的代码，因为通常情况下，不进行建模检验直接写c的代码，基本上难度很大。
只有深入了解并自己推导验证之后就会发现，卡尔曼滤波的兼容性与性能非常强大，也可以应用于多种场景。