卡尔曼滤波原理详解及系统模型建立（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)就是我们根据上一刻的最优估计值左乘一个状态转移矩阵再加上控制量得到的，这里因为我们已经估计值了所以没有噪声项。
卡尔曼滤波的第二个等式：

这个实际上是一个假象的式子，先宏观来看，卡尔曼这个式子实际上就是用我们根据上一时刻估计的系统状态来修正一下，最终得到此刻系统输出的最准确的估计值，怎么估计呢，就是最开始那个小车的例子了，先假设有一个卡尔曼增益K(k)，这个增益会在此刻衡量人为估计值和测量值哪个更可信，然后做一个决策，得到最终最优卡尔曼估计值，在这里我们先不要纠结这个值到底该怎么取，这一步要先理解整个式子是怎么假设出来的就可以了，简单来说就是假设了一个参数去衡量我该信哪个更多得到最终的估计值。

在引出卡尔曼滤波的后三个关键等式之前：

先来看看卡尔曼究竟是如何取到这个卡尔曼增益，最终让估计值更加精确的，要明白我们在干啥，看回最开始的小车的例子，我们每次希望得到的是最后这张图：

也就是说我们想要的是绿色的这个分布，我们不仅要得到位置，还要求位置的精度足够高，就相当于我们希望绿色高斯分布的方差尽量小，这里记P(k|k)是k时刻我们估计出的最优值的协方差，好了，我们的目标就是：最小化P(k|k)。

那P(k|k)哪里来？

所谓卡尔曼最优估计协方差就是最优估计与真实值的误差，那们记e(k|k)为k时刻最优估计的误差（后面e(k|k-1)是根据前时刻的估计值误差），显然该误差等于最优估计和系统真实输出的差即：e(k|k) = X(k)-X(k|k)

但X(k|k)可以用卡尔曼滤波的第二个等式表示，所以对e(k|k)进行化简：

化到这一步以后，你会发现K(k)也就是卡尔曼增益依然存在，还是没有解决问题，没有告诉大家这个值该怎么取，但是这里出现了一个有意思的东西了:(X(k)-X(k|k-1))，这是啥玩意？刚刚我们说X(k)-X(k|k)是k时刻最优估计的误差，那这里显然就是人为估计的误差，也就是我们说的修正之前的误差，也就是说，这一步可以看出来，可以用修正前的估计误差和卡尔曼增益得到最优估计的误差。

那下面就来计算最优估计协方差：

可以看到从第四个等号开始用了P(k|k-1)来代替E[(X(k)-X(k|k-1))(X(k)-X(k|k-1))^T]，直接用修正前的估计协方差来替代这一坨乱七八糟的。那现在目标就很明确了，现在就要来最小化这个最优估计协方差，问题就转化成：将卡尔曼增益视为自变量，使得这个矩阵的迹最小时的卡尔曼增益就是我们要的K(k)。（最后一个等号把白噪声的协方差用R表示了，化简过程中要注意白噪声的期望是0，所以中间两项消去了）

那我们就来取最优估计协方差矩阵的迹：

求最小值就顺便对K(k)求个导：
再顺便让导数等于零：

然后顺便把K(k)拿出来：

好了，到这里终于得到了我们朝思暮想的卡尔曼增益，可以发现是由未修正估计的协方差和测量误差的协方差决定的，以上也是卡尔曼滤波后三个等式中的一个，到这里，我们只要想个办法求出未修正估计值的协方差就大功告成了。同样的我们记e(k|k-1)为未修正估计值的误差，而且e(k|k-1) = X(k)-X(k|k-1)，这步很简单就不讲了。

同样的，求该协方差值：

第二行开始把k时刻系统真实输出用k-1时刻的真实输出来表示，同样把噪声的期望认为是0，这样就化简到了倒数第二个等号，可以看到又出现了一个神奇的东西！X(k-1)-X(k-1|k-1)，这不就是e(k-1|k-1)吗，这个东西我们刚刚算过是不，那就替换过来，就有了红色的等式，噪声协方差用Q表示，这时候就很神奇了，估计协方差和前时刻的最优估计协方差是一个转移矩阵平方的关系，顺便加上噪声协方差，所以到这里为止我们可以用前时刻最优估计协方差来表示现在的估计协方差，也可以用现在的估计协方差表示现在的最优估计协方差，所以很明显，协方差的估计可以用来迭代了，红色的等式也就是卡尔曼滤波的第四个等式，就快要胜利了。

整个完整的卡尔曼滤波的迭代过程这里已经可以写出来了，但你问为啥这里只有四个式子还少一个呢？其实我们最开始已经有第五个式子了，只是还不够好看，接下来就把刚刚没做完的做完：

这里就是把括号打开了哈，仔细化一下很简单的，然后把右边的卡尔曼增益项用刚刚的式子替代：

就化简出了一个非常简单的结果了，这就是卡尔曼最优估计值的协方差，只用估计协方差和卡尔曼增益就可以表示出来了，这里R被消去不是说跟估计值的协方差噪声没有关系了，而是把R放进卡尔曼增益里表示更加简洁，这也就是卡尔曼滤波的第五个式子了！到这里完整的卡尔曼滤波已经被推导出来了，喜大普奔！！！完结撒花！！！

总结一下
卡尔曼滤波的五个等式：

前两步是根据上一个最优估计值得出此刻的估计值和估计值的协方差，紧接着就可以得到此刻的最优估计值和最优估计值的协方差，然后利用此刻的最优估计值和最优估计值的协方差进行下一个迭代，完美。

恭喜跟我看到这里的童鞋们，我真是谢谢您嘞，说明我也没白忙这么半天。

我们可以再回头看一下卡尔曼滤波的五个等式，看起来有点多，但是实际上非常紧凑，推理非常严谨，但是有一些我也没想清楚卡尔曼究竟是怎么想到要这么化简的，觉得是巧合但又不是，只能感慨数学的伟大，卡尔曼的牛逼。

开始系统建模
这一部分开始最好随时把刚刚推导出来的五个卡尔曼滤波的等式放在一个方便看的地方，配合我的模型来边看边理解会比较好。

下面就根据刚刚推导出的五个卡尔曼滤波的公式来建立一个simulink模型，模型如下：

这个模型要怎么看，就分两部分看，上面半部分实际上就是卡尔曼滤波的前两个公式，也就是预测值更新的过程，下面一大坨都是卡尔曼增益更新的迭代过程，每个节点我都把具体的变量标清楚了，仔细看是可以看出卡尔曼滤波五个公式的影子的，这里我默认是不加入控制量的，也方便后面讲整个系统的响应，所以我这里的状态转移矩阵和测量矩阵都是1，也就是用卡尔曼滤波来预测像温度变化这种简单的物理量变化，温度变化一般就认为是不变化的，所以状态转移矩阵也就是1，而温度也是直接可测的，所以测量矩阵也就是1。

所以有了模型以后就很清楚了，卡尔曼滤波的五个等式看起来参数非常多，但大多数参数都在不断的循环更新中，所以整个系统真正能调整的参数只有三个，就是系统开始循环的初始值，这个要人为定一个。还有就是最最重要的两个参数，过程误差和测量误差的协方差，这就比较奇怪了，能调整的参数居然是两个噪声的协方差？！但是要知道过程误差和测量误差很多时候是不可得的，是系统本身就有的，所以我感觉这就是卡尔曼滤波很神奇的地方，即便是对误差的协方差有一个人为的不准确的判断，也能对系统的真实状态有个比较好的预测。

下面我们就来对卡尔曼滤波的完整系统进行一个深入的研究，首先还是要把系统建立得更完整一点，我们给定一个真实系统，加上一个噪声，再对真实系统的输出进行测量，再加上测量噪声，将总的测量信号接入卡尔曼滤波，然后同时检测系统真实输出和测量值还有卡尔曼最优估计值，完整系统如下：

然后加入一个比较经典的阶跃输入信号，先去除所有噪声，系统过程噪声和测量噪声，包括卡尔曼滤波内部的所有协方差值都设置为0，看看卡尔曼滤波的输出：

这时候只有测量值可以检测到，没有卡尔曼输出，原因灰常简单，当R等于0的时候，卡尔曼增益等于测量矩阵的逆，这样P(k|k)=[I-K(k|k)H]P(k|k-1)就永远等于0，卡尔曼输出没有任何意义，所以要给R赋一个很小的初值才能认为传感器的精度非常高。同样的，先验误差如果等于0，也就是Q等于0，此时卡尔曼增益等于0，也就是说不考虑测量值，卡尔曼滤波只相信自己的估计值，但是你想想哈，卡尔曼滤波的输入就是测量值，你又不信它，太过分了吧，您凭空想象吗，所以这个时候的所有估计值都是没有任何意义的，即便我们希望某个误差很小，也必须是一个不等于0的值。

那首先我们将先验误差和后验误差的协方差设同时设置为0.001，同样输入一个阶跃信号，看卡尔曼滤波的输出：

你问这跟刚那个图不是一毛一样么，不是的哈不信你翻回去瞅瞅，刚那个是蓝色的，这时候卡尔曼滤波已经有输出了，只不过卡尔曼滤波的输出和测量值重合了，为啥呢，也好理解，看卡尔曼增益：

其实这个时候先验误差影响是不大的，在传感器误差协方差接近于0的时候，就是我们认为测量误差非常非常小的时候，也就是认为传感器是一个神仙传感器，它测量出来的值是绝对真实可信的，这是一个最准的传感器，于是卡尔曼增益这里分母的R是一个高阶小量，分子约掉，最后卡尔曼增益等于测量矩阵的逆，最终看回卡尔曼滤波的第二个式子，会发现卡尔曼滤波的最优估计值永远等于测量值，这很好理解，算法都认为测量值准确无误了，那必然每次估计出的值只要相信传感器就好了呀，所以在这种情况下，它会复制传感器的值。

下面我们固定先验误差的值不变，保持0.001，然后改变测量误差协方差的值，改变幅度很小的时候其实变化也并不大，我这里直接展示改变到10和1000的变化较大的时候。

当R为10时：

会发现最优估计值和测量值开始有一点偏移，这里实际上也是比较好理解的，增大了R值，系统在预测过程中对测量值的信赖度降低，所以接近测量值的速度会降低，同样的把R值增大结果类似，下面R变为1000：

可不就是么，收敛速度会降低，如果固定R不动，增大先验误差的协方差道理也是一样的，会使收敛速度加快，因为增大Q的同时也就是减小了R，卡尔曼增益就更大了，卡尔曼增益变大，测量余量影响的比例就越大，所以测量值起到的作用也就越大，到这里Q和R的互相关系也就差不多描述清楚了。总结一下就是Q和R都不能为0，卡尔曼滤波的输出和先验误差、测量误差的协方差的具体值是没有关系的，和它们的比值高度相关，当然这都是从原理出发，实际在使用的时候还是要对传感器的漂移有个实际的估计。

但是到这里为止，系统和传感器都是不真实带有误差的，我只不过在卡尔曼滤波器的内部主观加上了误差的协方差，下面就来测试一个真实带有噪声的系统。现在给系统过程误差加上0.01的方差，传感器本身也带有0.01的方差。系统输出和测量值如图所示：
红色线是卡尔曼滤波的值，还没加进去，蓝色是测量值，橙色是系统的真实输出，可以看到测量值围绕真实值上下飘动，下面开始用卡尔曼滤波预测，首先假设卡尔曼滤波内部对先验误差和测量误差的估计都是准确的，这里设置Q和R都是0.01，卡尔曼估计结果如下：

红色线围绕真实输出上下飘动，放大点可以看出来卡尔曼滤波可以得到一个比测量值更加接近真实值的输出：

这里我直接讲一个我的实验结论，就是刚刚我提到卡尔曼增益和单独的Q和R取值关系不大，只和它们的比值有关，所以这里如果同时给R和Q增大相同的倍数，卡尔曼滤波的估计结果都差不多，和真实输出的残差范围也基本没有变化，这个结果就不在这里展示了，直接看上面的图就可以，结果都类似于上面这个实验，这里我就展示一下输出结果有明显不同的实验。

（下面的仿真对系统真实的过程误差和传感器的真实测量误差都没有改变，只改变卡尔曼滤波里的方差）

下面固定Q不变，增大或者减小R值观察卡尔曼滤波的结果。

首先固定Q为0.01不变，将R增大到1：

放大来看：

卡尔曼滤波的估计值收敛速度明显变慢了，但收敛后的值也比较平缓，原因是R增大，卡尔曼增益减小，对传感器的输入值不能足够相信，但更信任自己的估计值，所以每次卡尔曼估计出的结果都不会有太大的振动，因为每次都是根据上次自己的估计值来估计的，而且自己很信任自己的估计，所以很平稳，只是收敛速度慢。

下面固定Q为0.01不变，将R减小为0.0001

放大看：

会发现看不到测量值了，其实这是收敛速度过快的表现，此时卡尔曼增益非常大，所以卡尔曼滤波系统内部对测量值的置信度是非常高的，几乎与测量值完全重合。

做到这里就基本上可以概括卡尔曼滤波里两个重要参数对滤波器的影响了，最后来看一个正弦波输入卡尔曼滤波时R值逐渐增大的效果，不详细解释了，如果看到这里应该可以对照前面的平衡式自己看看哈，直接上图加深理解好了：R值分别为0.0001，0.01，1，10




最后再来总结一下卡尔曼滤波里先验误差和传感器误差的参数调节

①Q和R都不能取0，Q取0时意味着完全不相信测量值的输入，所以系统输出值衡为初值；R取0时意味着无条件相信测量值，所以卡尔曼滤波的输出与测量值完全重合，这两种情况下卡尔曼滤波毫无意义。

②讨论Q和R的单独取值同样意义不大，因为卡尔曼增益是受R和Q的比值影响的，在调节参数的时候要同时比较两个协方差的大小。

③卡尔曼增益的值越大，意味着越相信测量值的输出，收敛速度越快，最优估计值震荡越明显；卡尔曼增益的值越小，意味着测量值的可信度越低，越相信系统本身的估计值，最优估计值收敛速度越慢，输出越平稳。

好了，终于完结，再撒花。

有啥不太对的地方欢迎讨论哈，谢谢大噶！

只收藏不点赞，生娃没有屁股蛋！