牢记公式，ardupilot EKF2就是纸老虎（一）！

BreederBai首次创建于2018年12月26日

版权声明：本文为博主原创文章，转载请附上博文链接！

一、初识卡尔曼滤波器

    本博客着重介绍ardupilot EKF2算法所使用的公式和思想，EKF2执行的过程我在另一篇博客中有介绍。网上关于EKF的介绍很多，百度百科也有一些介绍，我在这里就不多做介绍了。为了描述方便我从网上找了一张卡尔曼滤波器的5大公式的图片。篇幅所限，下图所示的是多维卡尔曼滤波器（因为EKF2是多维扩展卡尔曼滤波器，所以我们从多维说起），为了跟好的理解卡尔曼滤波器可以百度一下，从一维开始。

    这5个公式之外还有一个观测模型，根据你实际的观测量来确定，它的主要作用是根据实际情况来求观测矩阵。

        

    因为卡尔曼滤波器是线性滤波器，状态转移矩阵和观测矩阵是确定的。在维基百科上状态转移矩阵用表示。在ardupilot EKF2算法中，状态转移矩阵也是用表示的。下面是维基百科给出的线性卡尔曼滤波器的相关公式

    说了这么多，你只要记住一点就行了，卡尔曼滤波器的作用就是输入一些包含噪声的数据，得到一些比较接近真是情况的数据。比如无人机所使用的陀螺仪和加速度计的读值，他们的读值都是包含噪声的，比如明明真实的角速度是俯仰,陀螺仪的读值却是。通过扩展卡尔曼之后的角速度值会变得更加接近的真实值，有可能是。至于它是怎么做到的就是通过上述的公式做到的。公式为什么是这样，这，，，我目前也不大清楚，做工科的嘛，像上学时一样，先记住公式，然后可以使用公式解题（解决实际问题），至于公式为什么是这样（公式的推导），等可以熟练使用公式之后再去探索。