文章目录
原文链接：https://blog.csdn.net/varyshare/article/details/95065650
注：如果你从我前面的文章一点一点看到这里，当你把本篇看完，我敢肯定，你对卡尔曼滤波已经理解的很深刻了
接触过传感器数据的同学一定不可避免见到一个名字“卡尔曼滤波”。这是何方神圣？请看后面分晓。很多时候看不懂一个算法是因为里面很多概念上的问题你没了解，就直接看细节了当然看不懂。最关键的事就是你得先了解卡尔曼滤波到底有啥用，它的初衷是什么？接下来我就是想讲讲破解卡尔曼滤波的一些概念上的认知障碍这个事
破解概念上的认知枷锁：卡尔曼滤波做的事
卡尔曼滤波做的事就是：举个例子，已知上个时刻飞机的位置，知道现在这个时刻收到的雷达测量的飞机的位置。用前面两个数据来估计此时飞机的位置。精简的说就是知道上个时刻状态，又知道测量数据，融合这两个数据来求当前状态。
你一定会问现在知道当前时刻的测量数据那么我认为当前状态就是测量数据不就好了么？换句话说：“你一定会觉得雷达测量到的飞机位置不就是当前飞机的位置嘛？为何要用卡尔曼滤波来估计飞机当前的位置？”。
答：现在这个时刻收到的雷达信号测量的飞机的位置还真不一定是飞机当前的真实位置。首先雷达信号测量有误差。其次你想想我现在收到雷达信号，那是之前发射过去然后返回的信号。这个过程是不是要时间？这段时间飞机说不定以超2倍音速飞行，说不定直接坠机，这些都有可能**。也就是说即使收到测量数据但是还是不确飞机位置在哪**。于是我得需要根据前一个时刻的位置估计出当前时刻的飞机位置 结合 测量数据 综合考虑来 估计当前飞机位置。这就是卡尔曼滤波的作用。
那么你一定会问根据前一个时刻估计的飞机位置怎么就可以估计现在这个时刻的飞机位置了？
答：**卡尔曼认为所有的状态变化（位置变化）都是线性的。**什么叫做线性？上个时刻位置是0.3，速度是0.2。那么我估计下个时刻的位置就是0.5。这就叫做线性。
接下来你一定会问那并不是所有的状态变化都是线性的怎么办？你像风速变化它就不是线性的。
答：恭喜你发明了新的算法。事实上别人已经为这个算法命名了叫做扩展卡尔曼滤波。现在我们要学习的是卡尔曼滤波。你只需要记住卡尔曼滤波就是认为所有变化都是线性的。
那么现在我知道了怎么用上个时刻飞机的位置估计当前时刻的飞机位置，也知道了还得借助当前时刻收到的测量数据来综合考虑来估计当前飞机的位置。那么怎么综合考虑呢？这就涉及到一个比例。到底这两个数据占比多少？这就是卡尔曼滤波的核心精髓。卡尔曼滤波算法要动态的调这个比例。（有种中庸之道的调调，既不只信测量数据，也不只信上个时刻的位置所估算的当前时刻位置。）
滤波算法的思路发展？
如果不理解滤波思路那么今天学会了一个卡尔曼滤波明天还有一个通尔曼滤波要你学。如果学会这些滤波算法的思路，把他们联系在一起然后记忆那就简单多了。滤波算法本质上就是利用多个数据来融合估计真实状态。下面举两个从浅到深的例子。
如何结合“飞机的速度和雷达测量的飞机的位置 ”来估计飞机在t2时刻的位置？
但是飞机的速度怎么估计啊？因为飞机不一定是匀速走。我们得根据
现在我想估计飞机的速度。
速度估计的方式有两种：
如何结合“飞机的速度，加速度，雷达测量的飞机的位置” 来估计飞机的位置？
卡尔曼滤波怎么进行滤波的？
由上面这段话可以看出，卡尔曼滤波最关键的是需要求出两种方差。一是纯粹依赖估计的这种方法的方差。二是测量仪器的方差。只要知道了这两个值那就可以知道估计值和测量值在最终结果中的占比。算方差的方法在不同应用场景是不同的需要以实际情况而定。
注意：根据上个状态估计当前时刻的位置方法不是只有根据速度和加速度来估计这一种方法。还有其他很多种。比如我可以根据飞机发动机此时的推动力和这段时间内燃烧了的汽油来估计飞机飞行了的距离（能量守恒定律，推动力×距离=做功）。
程序实现
