关于卡尔曼滤波中协方差矩阵Q,R的一些思考,卡尔曼原理讲解

　　首先声明：本文的两个主旨是①“如果将卡尔曼滤波看做一个系统，那么Q,R先验地决定了这个系统对来自状态预测和来自观测预测的置信度（通过K矩阵体现，K矩阵像一个系数一样对两个预测进行加权得到最优估计（见下面图4的公式四））” ②在做仿真任务时，注意对效果的评估是受相对误差影响的，即同样大小的误差用在不同的模型上表现的效果不同。例如同样的精度的传感器，在工程道路测量上的表现令人可以接受，但是在精细的测绘中就令人不满意了。

　　本文的参考资料我大力推荐，真的很棒！ 文章中对参考资料的引用可能可以通过ctrl+f在网页中搜索一些关键词搜索到，阅读本文时建议同步阅读参考资料。

1 前言，故事背景：

　　故事背景是我为了学习卡尔曼滤波预测器，而做”卡尔曼滤波算法的仿真练习”（下称练习或作业），如下：

1.1 题目

 

 

 

 

 

（作业要求做直线轨迹和圆弧轨迹，以下叙述均只叙述与直线轨迹有关内容）

1.2 初步解题

　　我是在没有理论基础的前提下写这个练习的，以至于对一些矩阵的意义不明白（例如K,P等），更别说卡尔曼的五个公式的推导了；另外我一些方面基础较差，在刚开始写这个练习的时候我不清楚协方差（包括单随机变量和随机向量）的定义和计算方法，一些矩阵乘法的技巧也不熟悉。

　　我初步清晰地自定义了一个如下运动模型，然后直接用精准观测值（即真实值）作为五个公式公式四的y(k)，套用五个公式，得到了与题目中参考仿真结果相近的可喜结果（图1）

物体摆在世界坐标系下（30,0,0)处,手眼相机坐标系三轴指向始终保持与世界坐标系相同，

% 一、物体摆在世界坐标系下（30,0,0)处,手眼相机坐标系三轴指向始终保持与世界坐标系相同，
% 二、手眼相机从(0,0,0)开始以2m/s匀速沿着x轴靠近物体
% 三、注：迭代过程中的观测量y采用了精准观测值（即真实值），
% 例如在k=4时，是第3拍，当前时刻为0.75s，相机已前进1.5m，那么观测值y即为(28.5,0,0,0,0,0)'

图1

 

1.3 问题出现

　　然后，有同学抱着“不加噪声，卡尔曼滤波的优化如何体现？”的疑问问我如何在这个仿真作业中加入噪声。我思考了一下，首先认为确实需要加入噪声，因为①这位同学的质疑很有道理； ② 毕竟我们声明了“观测方程噪声V的协方差是单位矩阵”，虽然不清楚协方差的定义，但是还是知道噪声不会全为0，否则协方差矩阵就是0矩阵了； ③观测量是通过传感器得到的，传感器的测量量存在误差人尽皆知，在做仿真作业不应该忽略传感器的测量量这么明显的要素。

　　然而，在给y的第一维（即x方向位移的观测量）添加了rand产生的高斯分布噪声后，整个拟合结果非常辣眼睛（见图2，位移预测还能接受，速度预测偏差太大，甚至可以描述为“天女散花”，速度预测的偏差在预测值画散点图时更加明显），于是产生了“到底要不要加噪声”和“为什么加了噪声效果后卡尔曼似乎没有正确预测”的疑问

　　图2（左：位移，右：速度）

　　这时，为了解决第一个疑问，我去网上找了卡尔曼滤波的示例代码，请转到参考1：https://zhuanlan.zhihu.com/p/29191795进行查看。这个链接里的代码写得非常清楚，是非常典型的卡尔曼滤波仿真代码。请读者注意看他的三段实测的代码（实际上只是改变了Q的值而已）及结果：①“% 【1】预测误差比较大的时候，Q = 200;”②“% 【2】预测误差适中的时候，Q = 10^-1;”，③“% 【3】预测误差很小的时候，Q = 10^-6;”。　　可见参考1的卡尔曼滤波示例不仅加了噪声，而且得到了同样“差劲”的预测结果。所以，我更加相信要加噪声。

　　但是，第二个疑问还没有解决，尤其是看着来自老师的优秀参考仿真结果，不禁想：为什么加了噪声效果后卡尔曼似乎没有正确预测？是我哪里写错了吗？

2 对卡尔曼滤波原理的学习：

　　前面提到我一些方面基础较差，我不清楚协方差（包括单随机变量和随机向量）的定义和计算方法，不熟悉一些矩阵乘法的技巧，也不理解卡尔曼滤波中K,P这些关键矩阵的意义。实际上，为了弄懂原理，我必须得清楚知道计算流程和每一步计算的含义。另外，我还需要弄明白，卡尔曼滤波是用来做什么的，他是怎么得到最优估计的。这里插上一句，协方差相关的知识缺少的话还是建议补习的，因为用处挺大的。

2.1 基本原理

　　Thank to 一些优秀的网上资源，参考2：https://zhuanlan.zhihu.com/p/137235479（在阅读这段时，请同步打开这个链接进行参考） 清晰地讲解了卡尔曼滤波是怎么得到最优估计的：卡尔曼滤波做估计的两个依据来源是“理论预测”和“实际测量"。

　　“理论预测”是通过对系统进行运动建模得到的，这个运动模型的信息提供给卡尔曼滤波系统（可以想象这个信息存储在系统的"理论预测"模块中），例如参考2中的运动模型就是匀加速直线运动，即通过x(t-1)估计x(t)时，由于系统的"理论预测"模块认为汽车完全是在进行匀加速直线运动，所以x(t)=x(t-1)+v(t-1)*Δt+½*a*(Δt)²。而在上述练习中，系统的理论预测对应A矩阵，系统的"理论预测"模块认为手眼相机完全是在进行匀速直线运动，所以展开计算卡尔曼滤波的公式一就会发现：x(t)=x(t-1)+dx(t-1)*T。

　　“实际测量"就比较简单了，只需要直接读取测量量，经过一个矩阵将测量空间映射到真实状态空间就可以告知卡尔曼滤波系统："总部！根据测量，当前状态为xx，请参考！"（注：卡尔曼的公式中没有直接将测量空间映射到真实状态空间的测量矩阵，做这种映射的是K矩阵，相反倒是有“将真实状态空间映射到测量空间”的矩阵H(参考2中)/C(练习中)，关于这一点的思考见本节的第6段“附注一点”）。

　　于是得到了这两个依据来源后，就可以进行“2.3 理论预测与实际测量融合”了，融合的依据是所谓的求出卡尔曼增益K “使得每时刻的估计误差方差最小”然后对这两个依据来源线性加权，其实更新P,K才是卡尔曼滤波的精髓灵魂，涉及较多数学推导，想具体了解请参考参考2和其他资料。

　　参考2的这篇文章非常清晰地进行了公式推导，解释了P,K矩阵的来源和意义，完全解释了卡尔曼五个公式每个公式的作用和意义，非常赞！尤其是我能把参考2中的公式符号与练习中的对应起来，对我理解卡尔曼滤波的公式原理非常有帮助！

　　附注一点，关于测量量的映射矩阵，在练习中对应C矩阵（这里没有怎么映射，因为是直接测得xyz三轴位置等，所以用6*12的C矩阵进行直接提取（参见C矩阵的定义））。这样做似乎不太直观，为什么不是直接提取测量信息的到“测量对状态的估计”呢？可以按下面两点理解：①用这样的H/C矩阵可以在测量空间中导出“理论估计值与测量值之间的误差的协方差矩阵”，虽然与”理论估计值与真实状态之间的误差”不在同一个空间。但是我们的最终目的是“估计值与系统的真实值之间的误差最小”（见参考2式(11)）,既然是“估计值与系统的真实值之间的误差”，那我们有哪些与“理论预测”和“实际测量"相关的真实值呢？最直接的就是真实状态量（真实状态空间）和真实测量量（测量空间）“理论预测”——状态估计和“实际测量"——测量估计对应。通过参考2的推导，我们发现空间不同并不影响推导的正确性，从某种程度上说，这样推导可能更简洁。② 据我猜测，先将真实测量量映射到状态空间再计算误差也是可以的，有待对其他资料考察和来自它们的支持。

　　另外的补充说明：上述只是讲解了一次估计的原理，虽然参考2和练习给出的公式涉及了迭代，但是下述资料可能可以帮助你理解得更好：参考3：https://www.jianshu.com/p/d3b1c3d307e0 （推荐关注其中的“3.卡尔曼滤波的通俗理解”），以具体的数值例子，通俗地从置信度角度切入，说明卡尔曼滤波的线性加权本质。另外，他对于置信度（通过均方差描述(因为参考3中的例子是一维的，所以是均方差，他文章里也说了“如果状态向量是多维，那么方差就该变为协方差”）的数值更新直观地体现了卡尔曼滤波对"所有历史值均有考察的特点"（见“于是我们可以把绿色分布当做第一张图中的红色分布对t=k+1时刻进行预测”所在段落)。

　　更为重要的是参考3说明卡尔曼原理的时候回到了本质上的“概率”问题，在本博客下文，你将看到我对卡尔曼中概率(协方差，置信度)的解释。

2.2 一个错误想法

　　因为图1中的初始状态(坐标(0,0,0))与真实状态(坐标(30,0,0))差别很大而卡尔曼滤波能修正回来，我曾经高估了卡尔曼滤波的智能度,甚至极端地，我曾想象卡尔曼滤波在面对偏差较大的测量值时，会根据历史条件非常智能地对测量值的采纳进行调整。具体地举例说：例如系统平稳运行一段时间，在第k时刻突然测量到状态突变，我想象系统会自动对这个异常值进行修正，以使状态整体波动不大。

　　以上想法过于空想，理想主义，一方面没有依据，另一方面：我们不能假想状态突变就是测量异常而且系统运行平稳，假设是系统真的状态突变（即测量量是准确度）而系统错误地修正了，那么状态估计就是错误的了。以上想法给卡尔曼系统过多的预知知识了，实际上，在第k时刻前，卡尔曼系统根本不知道第k时刻测到的状态突变是测量失误还是系统真实状态突变。如果不纠正上述对卡尔曼系统的错误崇拜，就可能不会去从原理解释为什么加了噪声后卡尔曼似乎没有正确预测，而无法理解出错的本质原因。

2.3 一个重要发现

 　　之前偶然对卡尔曼滤波五个公式里的变量做了拓扑排序（用这个术语我很得意啊，对不懂拓扑排序的同学：拓扑排序的意思就是图里"只有箭头源头被求出后才能求箭头指向的对象"），见下图。

 图3（符号参考练习文档）

             　　　　　　　　　　　图4.对预处理K的实验验证(绿线是预处理K后得到的X估计，整体上移了一个单位以作对比（重合可能不易看））

　　发现了什么？ X与y是不影响P，K的值的！ 这从计算原理上否定了2.2节中说的“卡尔曼系统的智能估计”,更进一步说，对于作业中的定常系统(A,G,C恒定，实际上很多系统都是定常系统),只要Q,R确定，我们完全可以预处理得到P(k)，K(k)（实际是只需要每时刻保存的只有K，见卡尔曼公式四）,然后再逐时刻求得X的最优估计!  并且预处理K的正确性也被实验验证.（见图4），可见预处理K后得到的X最优估计与原本直接套5个公式得到的结果 完 全 一 致 !

2.4 对Q,R本质的研究

　　好了，基于2.2节和2.3节提出的“卡尔曼系统没有那么智能”和“对于定常系统可以预处理K”，我们完全撇清了y对于卡尔曼增益K亦即加权系数的影响。从一个角度说明：你y任取，我系统都不为所动（指“笑死，P,K矩阵根本不变”），那么在这里我们已经初步得到了要不要加噪声的结论了，结论是加不加都行！这个结论可能比较抽象突兀，下面将进一步介绍。（最终回答见3.1节）

　　由图3的拓扑排序知：对于定常系统，影响P,K的只有Q,R矩阵。参考2中“那么如何度量理论估计值... 与真实状态...之间的差距呢？也就是说理论估计值的估计误差有多少？这可以用协方差矩阵来表示”和参考3中的“究竟相信谁多一点，我们可以用kalman的方法来加权，即利用他们的方差σ^2来判断”的叙述表明：卡尔曼中是用协方差来描述误差和置信度的，进一步说：协方差影响权重K（这点在K矩阵的定义中已经说明了：求出卡尔曼增益K “加权后的估计值与系统的真实值之间的误差最小”（参考2））。这一点不难理解：协方差大，说明波动大，说明你状态预测（“理论预测”）或测量预测（“实际测量”）不准、误差大，那我系统自然就对你的信任低，你置信度就低嘛。　　

　　而Q,R正好就是噪声的协方差矩阵，描述了通过状态预测和通过测量预测的不准确度，于是我断言：我们在使用卡尔曼滤波系统进行最优估计的时候，传给系统的先验知识除了初始状态外，就只有Q,R这个两个矩阵，这两个矩阵向系统声明：“在面对状态预测和通过测量预测时，你对他们的相信程度请参考我们这两个矩阵描述的噪声的方差！”

　　结合2.3节的叙述，我们发现，无论你传入什么y，我卡尔曼系统照单全收，是一个无情的计算机器（这一点在预处理K时更加明显），根本不会去动态考虑“诶，这个y偏那么多会不会是假的呀”。但是值得一提的是：不同的y会导致X(k)序列的改变,导致不同的预测结果（参见卡尔曼公式四，不同的y序列会连锁地影响X的历史值（最好在预处理K的前提下思考））。结合2.2节，以上容易理解：因为系统对未来完全未知，他能做的只有按照预设好的数学规律进行“它所认为的最优估计”，而“不同的y会导致X(k)序列的改变”则体现了卡尔曼系统对历史值的考量。从始至终，系统对状态预测和通过测量预测的相信程度（采纳度）都是相对于实际观测值和状态值静态的,它们只受实际观测值和状态值（提供状态预测和通过测量预测）的波动情况（协方差）影响。

　　了解Q,R的来源对理解上述描述有帮助。参考4：https://blog.csdn.net/u011362822/article/details/95905113 讲解了在实际中应用卡尔曼滤波如何确定Q,R矩阵：根据人们对系统的考察，对状态方程（考察运动时为运动方程）的协方差估计确定Q, 传感器的精度确定R。选取不同的Q,R就是对卡尔曼系统进行超参数设置，会深刻影响预测结果，对状态值的波动和观测值的波动的正确评估与否（是否接近波动的真实情况）决定了卡尔曼系统的表现，因为现实中状态值和观测值就是按照真实的波动情况波动的（因为噪声）（废话），这里说的波动情况等价于噪声的分布情况。

　　回首参考1中的三个测试效果，他保持R不变，设置Q越来越小，表示状态值的波动越来越小，状态值的置信度越来越高，系统由Q的不同经过主要相信观测->较综合考察->主要相信状态三个模式，得到不同的预测结果。 值得提醒的是：并不能从参考1的三个测试的预测结果直接得到第三种Q,R配置最接近真实值的结论，关于R可以说是最优的因为代码传入的R矩阵是噪声n的真实协方差。但对于Q，Q的配置是否接近真实值是比较难考察的，这里Q取得极小而系统表现良好的另一个关键原因是在这个仿真代码中X的初值的准确的，如果X的初值不准确（例如练习中的），那么去很小的Q使卡尔曼系统非常相信状态预测，那么会得出不好的结果（坏笑）。另外，背离采用真实的R的原则而去人为增大R传给卡尔曼系统效果可能也会变好，但显然人为增大的R不可能比真实的R要优。（因为根本上是不符合实际的），所以直接从预测结果断言Q,R配置的好坏是不科学的，仅能做一定的参考。　　最后对参考1的三个测试的总结：通过Q,R的设置，调整了卡尔曼系统对状态预测和测量预测分别的相信程度。

　　我们人为地告诉了卡尔曼系统Q,R以决定系统对状态预测和测量预测的采纳比，卡尔曼系统“笨笨地,不会'聪明'地去画蛇添足地"揣摩例如"突变的观测值要不要忽视"这类本质上不可揣摩的问题，因为未来完全是未知的。正因为这个“不可揣摩”，卡尔曼滤波的"最优的估计预测算法"的头衔才不会被摘掉。

 

3 对两个疑问的最终解答：

3.1 到底要不要加噪声

　　加亦可，不加亦可。

　　首先，加上噪声是一种完美的仿真：我知道测量量的真实值，而且又定义了噪声的协方差矩阵R非0，给他加一个协方差符合R的噪声，理所应当。其次，加了噪声更有助于看出卡尔曼滤波的优化效果，这不显得你枪法准吗？（开玩笑，无恶意，意思就是确实要评估一下卡尔曼的效果）

　　而不加噪声的原因呢？ 首先，不加噪声也能看出卡尔曼枪法准，图1中X的初始状态偏差真实状态很大，但是这个卡尔曼系统实现了纠正。 其次，不加噪声并不会导致卡尔曼系统运作失常，根据2.4节，卡尔曼对输入的观测值y照单全收，他也不知道这是不是真实值，反正他就心惊胆战地抱着“哇，这群y测量值的协方差是单位阵诶，拿到这些y的时候要小心谨慎！”（笑死了哈哈）的心态无情地埋头算着他的数学公式，卡尔曼滤波系统并没有知道这些是测量的真实值，也不会取巧地发现“预测密码”而实现完美预测。第三：加噪声后画出来的预测图给人的直观感受会受到运动尺度的影响（将在第四节讲解）。最后，协方差是无限时空下的统计特性，在每次运行卡尔曼系统时并不能保证测量量噪声的协方差一定就是R（虽然都接近R），但是完全取符合R分布的噪声比较脱离实际（尤其是在这里样本量较少的时候，练习中只有61个时刻需要加误差），在实际应用中我们也只是拿着观测量直接传入而不管观测量是多少，从某种程度上说，作为仿真设计者，也要设身处地地为卡尔曼系统着想，抛开一些上帝视角，无论传入什么y，QR已定，都体现了系统的特性。

　　啊这，不加噪声的第四点理由强词夺理到我自己都受不了了，太诡辩了，至少无论如何都不能证明完全不加噪声比加准确的噪声要优。其实最终结论是：应该加噪声，而且在仿真这种可以加准确的噪声的理想环境下可以加准确的噪声。同时，加一些不那么准确的，例如协方差不严格为单位阵，而是对角线元素是0.9左右，剩下元素是0.1左右的噪声也是可以接受的。见下一段。

　　下面插入一个加准确噪声和加近似噪声（即上一段说的不那么准确的噪声）的对比实验（噪声为matlab同种子下rand产生，准确噪声直接计算噪声的协方差（近似于单位阵）传给卡尔曼系统，而近似噪声直接传入单位阵，注意与上一段的“准确-近似”定义有区别），可见差别不大。同时站在统计概率的局部抽样有一定偏差性的角度，总体上说，对噪声的要求不必很严格，近似噪声也可以接受。完全不加噪声在"简单地完成练习"层面也可以接受，取决于老师想通过作业考察学生们什么。经过第四节的讲述，最终对于这个练习还是不加噪声的好。(汗)

　　

图5 准确噪声分布（蓝线）与近似噪声分布（绿线）的对比

3.2 为什么加了噪声效果后卡尔曼似乎没有正确预测

　　根据2.2-2.4节的论述，卡尔曼系统是“笨笨的”，没有人们幻想得那么精明。 具体地说：Q,R定了（作业的要求），卡尔曼系统也就定了，卡尔曼系统被设定成“比较相信观测”，那么预测结果当然就更加地跟随观测值，所以粗略地说：观测值是什么样的，预测值大概就是是什么样的。抽象地直观地说：这套Q,R参数不适合（虽然作业的这个系统中Q难以确定，R已经对应观测噪声真实分布了，我也不知道适合的Q,R参数是什么样的，所以这个结论只是泛泛的直观感受）。 从另一个角度：在保证Q不变的情况下（我们假设系统的状态量的方差就是这么大），这个系统的效果不好，可以解释的原因之一是传感器太差了！（你的速度绝对值就只有2，你测量绝对误差尺度竟然有1！（其实是协方差为单位阵，就先直观地叫误差尺度为1吧，我目前不清楚协方差为单位阵的噪声一般偏差多少））如果传感器质量高一些，绝对误差尺度降到0.1，那么看起来效果不就好起来了吗？  同样的道理，具体应用中，相对误差比绝对误差有效得多，直观地看图片中偏移的大小有时会导致错觉，因为我们可能忽略坐标轴的尺度单位。接下来的实验可以清晰地展示尺度对于误差对人的感受的直观影响的干扰！(其实基于图5能说明坐标尺度的影响：假设扩大纵轴的范围，那么误差就会“被压缩”，直观地看过去可能会感觉预测效果蛮好的)

4 运动尺度的干扰：

　　3.2节讲到卡尔曼滤波效果不好可能是因为传感器太差了，可以换更好的传感器以降低相对误差来让效果看起来好。降低相对误差的另一个方法是加大运动尺度，我运动速度是10m/s而误差上下波动不变，那么matlab的plot画出来的图看起来不就好看些了吗，那卡尔曼滤波的效果不就“好起来了”吗?

图6 运动尺度放大后“卡尔曼表现更优了”

 　　如图6，将原本2m/s共运动30m的运动更改为10m/s运动150m(其余不变)后整体看起来卡尔曼枪法又准起来了，我的聪明的卡尔曼回来了！但是真实情况是：卡尔曼还是那个笨笨的卡尔曼，他只是在无情地继续算着它的数学。效果看起来更优了一方面是因为相对误差变小了，更直接的是plot画出来的图的纵坐标范围更大了，同样大小的绝对误差被“放小”了。（实际上，将图放大，有时人也会直观地认为误差还是很大......）这也说明，对于150m这么大的运动尺度，这个误差尺度为1的传感器又够用了！所以卡尔曼效果，传感器好坏都是相对量。

　　那么关于要不要加噪声的问题，由于这个作业规定了Q,R矩阵都是单位阵，R矩阵对应的传感器又不一定够用，所以要加噪声要么使用加大运动尺度的trick，要么明白卡尔曼的原理然后向老师报告：”老师，不是我和卡尔曼的错，都是这个传感器的错！” 在运动尺度小的时候加了噪声，人又不懂卡尔曼的原理的时候反而会非常疑惑（《论本文是怎么诞生的》）,所以不加噪声也不是坏事。（误）

5 关于Q：

　　在1.3节中提到参考1中修改不同的Q值使得预测结果不同，我在2.4节中指出“并不能从参考1的三个测试的预测结果直接得到第三种Q,R配置最接近真实值的结论”,其中的原因之一就是Q采用了准确的初始值，在关于本次练习的实验中，我也做了相关内容：将Q缩小1000倍，其余参量不变，同时让X的初始值设为真实的(30,0,0,0,...,0),得到了可喜的结果。因为系统的状态初值是准确的，保证了以后的估计与真实偏差不大（因为期望的运动通过矩阵A已经预设好了，现实的运动和状态预测所基于的运动都以A为蓝本，只是现实的运动可能与理想有一定偏差而已）,同时系统被设置为“比较相信状态估计，不要太考虑观测估计”,所以系统的预测受观测估计的噪声影响就不大。　　这充分体现了Q,R参数对卡尔曼表现的影响。在现实应用中能否正确估计Q,R参数是很重要的！

 图7 让卡尔曼系统更相信状态预测

6 总结：

　　卡尔曼滤波的最优估计来源于对状态预测和观测预测的加权考量。

　　卡尔曼滤波是一个数学工具，并不会玄学地过于智能地进行“思考”从而完美预测任何状态估计，是具有一定的适用范围的。但同时他背后有科学的强大的数学原理支撑。

　　卡尔曼系统中状态&观测量对于卡尔曼对状态预测和观测预测采纳权重的单方面解耦的，即前两者不影响后两者。算法调用者向卡尔曼系统传入状态量的协方差Q和观测量的协方差R，以此来告知系统应该对两个参考进行怎样的加权采纳，优化目标是使估计量与真实量误差方差最小。系统接受Q,R后，整个系统实际上已经固定，P,K矩阵均可预处理求出，然后再逐时刻求出X(k)（依据当前时刻的K(k)对状态预测(from X(k,k-1))和观测预测（from y(k)）进行线性加权）。由于系统的这种固定性，在实际应用中传入Q,R时应力求传入准确的协方差矩阵(至少你要告诉卡尔曼滤波小宝宝比较准确的情报吧），以使卡尔曼滤波正常、科学、优质地表现出应有水平。

　　关于本次作业要不要加噪声：因为本次作业并没有准备考察噪声方面的知识，在误差的相对尺度规定上有疏忽（没有规定运动尺度而固定了R为单位阵）,所以不加噪声可以接受。但是一般来说，做仿真，当然要加噪声，并且要向系统传入正确的R矩阵（准确值和近似值均可接受），以此观测卡尔曼系统的效果，理解其数学原理(包括Q,R矩阵的意义，和设置不Q,R表示设置置信度)。

　　在做仿真时，更应关注相对误差，小心地考虑误差到底有多大，不要仅直观观测忽视坐标尺度而被画出来的图所误导。

 

7 致谢与参考资料：

　　当然要感谢问题的提出者（那位同学）和这个练习的作者啦！　　最后，还要感谢我自己！

　　参考资料：

　　1.卡尔曼滤波示例：https://zhuanlan.zhihu.com/p/29191795

　　2.卡尔曼滤波器预测原理：https://zhuanlan.zhihu.com/p/137235479

　　3.卡尔曼滤波的概率置信度角度讲解：https://www.jianshu.com/p/d3b1c3d307e0 

　　4. kalman中协方差矩阵的计算：https://blog.csdn.net/u011362822/article/details/95905113

　　5.知乎一个答主的回答，通俗地讲了卡尔曼滤波是干什么的 https://www.zhihu.com/question/23971601/answer/26254459