卡尔曼滤波原理详解及系统模型建立（simulink）

卡尔曼滤波原理详解及系统模型建立（simulink）

卡尔曼滤波器

卡尔曼滤波器是在上个世界五六十年代的时候提出的，到今天已经有六十年左右的时间，但卡尔曼滤波算法不管在控制、制导、导航或者通讯方面对数据的预测能力依然处在一个不可撼动的位置上，但是很多人对于其算法内部的工作原理究竟是怎么运作的依然不理解，所以在工程上很多人都只是把卡尔曼滤波当成是一种“黑箱”预测算法，并不清楚内部原理。但实际上没有任何算法是“黑箱”，只是算法内部的运行规律并不直观，所以让人很难理解，现在也有很多对卡尔曼滤波的解释，但是我这篇文章里希望从原理入手，尽可能定性地对卡尔曼滤波的每一步都做出更加通俗的解释，最后对卡尔曼滤波的系统过程建立相对应的模型，对其进行各种响应的测试，这样也能够更深入地理解卡尔曼滤波。

原理

首先我们要明白的就是，卡尔曼滤波不是传统意义上我们理解的滤波，这是一种算法，在变化的数据中去除噪声对系统未来输出做出预测的算法，是基于概率统计原理的预测算法，是一整个系统，是系统就有输入和输出，所以第一步，我们要明确卡尔曼滤波的输入输出到底是在干啥玩意儿。

很简单看下面这个图：

KF就是卡尔曼滤波，算法的输入值是一个可测的量，这个量可以是任何量反正得你能测量出来，而且还知道这个测量值的精度大概在多少，有了这个测量值我们就根据测量值来估计这个系统的真实输出，并同时给出我新估计的这个值的精度大概在什么范围内，这就是卡尔曼滤波做的工作，但这个工作是不断进行的，对系统不断测量，然后不断估计，这样持续一段时间之后就能估计出系统一个非常准确的输出值。这里要明确的一点是，测量值可能非常不准确，估计值也非常不准确，这符合工程中的很多工作状况，但仅仅根据这两个不准确的值最后就可以估计出一个相对准确的系统输出值，这也就是卡尔曼滤波的作用。

下面我们就来具体讲卡尔曼滤波是咋运行的

网络上有很多例子，小车啊室内温度的估计还是估计小兔兔体重啥的，但这里为了形象我还是要借用一下这个小车的例子，方便我们讲原理：

我有一辆小车车，在一个水平轴上向右行驶，它的初始位置我是用脚测量的，非常不准，我也只能确定这个位置在一个高斯分布的范围内，如图：

当然了我还有眼睛，大概能看出来这车的速度，现在我就可以根据我刚刚脚测得的位置和我目测的速度，大概估计下一秒他的位置，它大概运动到了这个位置：
这图非常合理，为啥呢，你会看到高斯分布的方差变大了，因为这是我估计出来的位置，脚测量和眼睛测量的误差叠加在一起所以导致我估计的这个位置的精度更加不准确，所以它是一个更加“肥胖”的高斯分布，但没关系，我再用脚测量一下不就好了很简单，好了我又用脚测量了一下，如图：

这里黄色的是我估计的位置分布，蓝色的是我此时重新测量的，那车车真实的位置在哪里呢？不知道，我永远不会知道，但是我可以同时相信这两个位置分布，然后估计出一个最优的位置分布，这里蓝色的高斯分布更加瘦，所以就更加值得我信赖，所以我会信它更多一点，那我们最终确定车车的位置就在这里：

图中绿色的部分就是我现在能信任的最准确的小车的位置，它是由我估计出来和测量出来的值共同估计出的，也就是卡尔曼滤波的输出值，而下一步我会根据这个最优估计值进行估计，（也就是我前面讲的“脚测”，但是这个时候脚的估计值精度已经明显提高了）然后再用脚测量一次，然后根据两个值的“可信赖度”去决定我信谁更加多一点，然后估计出一个最优估计值，那你就会问了，我怎么知道我该信谁多少呢？具体要信多大的量呢？这也就是卡尔曼最tm牛逼的地方，他给出了卡尔曼增益，告诉了大家每次估计完成之后应该更偏向于哪个值具体多少。

前方劝退警告

如果只想对卡尔曼滤波有一个感性的认识可以直接跳过这一部分去看我最后的系统建模，但是我觉得如果想对卡尔曼滤波有个深入的理解就跟我一起耐心地把整个过程推导一遍，虽然看起来非常多，但是慢慢啃下来肯定会有收获的，这里需要有一些矩阵求导和控制原理相关的基础，如果不太懂可以去查一下，我也会尽量用通俗的语言来讲每一步：

正式开始推导

首先我们问问自己我们到底想要啥，我们是不是就想要每一个时刻（k时刻）的系统真实状态，这里我们用X(k)来表示k时刻系统的真实状态矩阵：

这里面A是状态转移矩阵，w（k）是噪声矩阵，认为是高斯分布的，X就是我们想要的每个时刻的系统状态的真实值，这个值永远不能得到，U是系统的输入向量，这个式子不是卡尔曼滤波的内容，这其实是马尔科夫提出的理论：一个系统的某些因素在转移中，第n次结果只受到第n-1次的结果影响，这里很好理解，我们如果知道前一时刻的系统状态，那我们可以根据这个系统的变化方式计算出这一时刻的系统状态。这里怎么去理解状态转移矩阵A，简单来说就是这个系统的变化方式，比如如果系统里变化量我们认为是室温，那X（k）就表示k时刻的室温，而状态转移矩阵这个时候就是1，因为我们认为温度是不变化的，如果X（k）是小车的运动速度、加速度、位移，那A就是速度和加速度的变化关系，位移和速度还有加速度的变化关系，就是那些高中的运动公式。

下面我们在k时刻对系统的真实值进行测量，也就是下面公式里的Z（k），这时候的测量值就是我们可以得到的了，H是测量矩阵，比如我们要测的是小车的位移，那给状态变量矩阵左乘一个测量矩阵就得到了我们要的位移。v（k）是测量误差，也是高斯分布。

到这里为止，上面的两个式子都不是卡尔曼滤波的内容，上面是两个一定成立的等式，下面就要开始正式卡尔曼了：

我们先看他的第一个等式：

这里面X(k-1|k-1)表示前一时刻系统的卡尔曼滤波最优估计值，咦，你会问，这还没开始预测呢哪来的最优估计值，因为卡尔曼滤波本身就是一个循环迭代的过程，每次估计都要用到上一次的最优估计值，（第一次用的是测量值，随便估计一个就可以了），所以我们先假装已经有一个了，而X(k|k-1)就是我们根据上一刻的最优估计值左乘一个状态转移矩阵。

（还没写完，慢慢更新）