卡尔曼滤波原理+matlab实现(有各个变量的详细解释噢!)

写这篇文章是因为自己经过一段时间初步学完了卡尔曼滤波，把自己的一些理解记录下来，针对新手学习，把自己的曾经的困扰以及后来如何理解记录下来，希望能对你有一些启示。文中都用大白话阐述，能够便于理解，就是纯心得记录。本文主要以介绍卡尔曼滤波的五大公式为主，其他算法的解释举例说明网上都非常多，就不复制粘贴赘述了，需要的看这里。因为本人也是小白一枚，如果文章中有出现错误的地方，希望大佬斧正。

卡尔曼滤波器是一种线性的滤波器，所以在算法中都强制假设变量间都是线性关系，那有人要问了？那要解决非线性问题怎么办？那就要用到扩展卡尔曼滤波，这是卡尔曼滤波器的扩展版，解决了非线性的问题。 卡尔曼滤波适用于存在不确定因素的动态系统里找到系统的最优状态解。并且卡尔曼滤波的好处还有很多，只需要在时域里实现，不需要转换到频域等等。

左边为先验，右边为后验以及更新。

看上面的五个公式，逐一解释其中的变量代表的意义以及公式的由来。

第一个公式: Xt^_：X代表的是系统的状态矩阵，t代表某个时刻, ^代表（hot）估计，表示这是X的估计值，表示这是X的先验值。(后面有同样的表示也代表同样的意思，就不再赘述了) F：状态转移矩阵，有什么用呢，用来控制系统的状态的个数，可以这么理解，就是让FXt-1^等于系统的状态。 B：控制矩阵，作用和状态转移矩阵类似。 ut-1:上一时刻的系统的控制量，也就是系统内部产生的控制量。 公式的推导：由系统的状态变量结合系统环境写出计算公式，将计算公式转换成矩阵的形式，就是这个公式的推导过程。 例子：以一个小车的运动为例：小车在某地假设存在两个状态变量：位置s和速度v，那么由高中物理公式有S = V0t+1/2at²，还有Vt = V0+at。其中在每一个时刻的V0即相当于上一时刻的速度即Vt-1。则此时的方程就应该为[St; Vt] = [1 t; 0 1][St-1; Vt-1] + [1/2*t² ; t]*ut-1 第二个公式: Pt:误差的协方差矩阵，用来传递估计系统状态时(第一个公式)产生的的误差的传递的 Q:误差的协方差矩阵，就是Wt-1的期望值，这个W就是最开始说的不确定因素里的过程噪声。 公式推导：这个公式记住就可以。 第三个公式： Kt:卡尔曼系数，也叫卡尔曼增益。 H：观察矩阵的控制矩阵，用来控制观察值的。观察值是什么？就是我们用一些测量工具对系统状态变量的测量值。也就是，对于一个状态变量，我们有两种获得方式，1是系统估计出来的，也就是1做的，2就是这个测量仪器测量出来的，这两者都有误差，卡尔曼增益的作用就是让我们在这两个结果中，更加相信哪个结果，然后得到一个相对更接近真实值的结果，也就是有点类似权重的意思。 R：观察协方差矩阵，也就是有最开始讲的测量误差求期望得到的。说是一个矩阵，但通常是一个常数。因为仪器测量存在误差，那每次测量的误差值基本上都是不变的，所以矩阵里的值也都是一个常数值。 公式推导:卡尔曼增益公式的推导比较复杂，由上一个公式逐步推导出来，选择合适的K，使得Pt_的迹最小(即方差最小)，然后一步步推导出来，期间用了大量的数学知识，线性代数基础。这里不做介绍，有兴趣的可以查看网上资料。 第四个公式： Zt：观察矩阵。Zt = H*Xt +vt 用于更新数据，给出系统状态变量的最优估计值。 第五个公式： I：单位矩阵 更新Pt值，然后再从第一个公式开始迭代循环，直到求出最优解。 这五个公式是卡尔曼滤波的最重要的五个公式，同时也是卡尔曼滤波的五个步骤，将五个步骤头尾相连，迭代循环，就完成了一个最简单的卡尔曼滤波器。

1.协方差矩阵 协方差矩阵是什么？协方差矩阵-百度百科 看完还是不知道协方差矩阵是什么？简单的说，协方差矩阵就是一个由方差和协方差组成的矩阵。方差在主对角线上，协方差在对应的其他位置。协方差矩阵能充分的体现变量间的关系。举个例子：由A、B、C三个变量组成的3*3的协方差矩阵，ABC三个变量对应的方差为σ²A、σ²B、σ²C，协方差为σAB、σAC、σBC，σAB = σBA，另外两个也相同，所以协方差矩阵是个对称的矩阵，由这三个方差和三个协方差组成的矩阵就是协方差矩阵： 协方差σAB>0则表示A和B是呈正相关的