卡尔曼滤波

在介绍姿态估计的具体实现之前，我们先大致了解一下卡尔曼滤波的原理。

---

什么是卡尔曼滤波

我们要估计一个系统的某一个状态，我们可以通过很多方法来得到这个状态值，但是每一种方法都会有自己的误差，有的大，有的小。卡尔曼滤波就是通过误差来融合不同方法最后得到最优的状态值，所以卡尔曼滤波也叫做最优状态估计。
举一个最简单的例子，我们要测量车子当前时刻的速度，我们可以通过GPS来测量，也可以通过加速度计来积分测量，这两种方法都存在各自的误差，现在我们可以通过卡尔曼滤波来融合这两个数据得到一个最真实的数据，这里的最真实并不是客观事实上的真实，而是我们认为的真实，有时候我们认为的真实和客观真实是不一样的。
再说简单一点，GPS测量的速度为5m/s，加速度计测量的速度为6m/s，我们相信40%的加速度数据，相信60%的GPS数据，那么根据贝叶斯公式就可以得到两者加权的速度。其实卡尔曼滤波从根本说就是贝叶斯的一种实现，其实就是一个概率问题，这个概率在卡尔曼滤波里通过卡尔曼增益来实现。

很多人就问了，这40%和60%这是哪儿来的啊？不用担心，我后面会讲到，这其实就是过程噪声和测量噪声

---

卡尔曼滤波的原理

不是任何系统都能使用卡尔曼滤波的，该系统需具备两个条件：
1、线性系统
2、系统噪声符合高斯白噪声

对于第一个条件：如果系统不是线性，我们可以使用EKF来线性化，但是会带来一些高阶损失

核心原理：
两个正态分布相乘依旧是正态分布
高斯分布的PDF函数：

两个高斯分布相乘：

化简得到：

看看图新得到的PDF函数为蓝色部分：

有何意义？
如果新的概率分布依旧是正态分布的话，那么这个系统就可以进行循环迭代了，可以不断的优化估计，这就是意义所在，在工程上就可以实现了，这就是卡尔曼先生的牛逼之处。

---

小结

总之，卡尔曼滤波其实就是通过融合多个测量系统的值，得到一个相对真实的值。

---