卡尔曼滤波算法原理

卡尔曼滤波算法的深入理解_L_smartworld的博客-CSDN博客_kalman滤波算法原理  https://blog.csdn.net/L_smartworld/article/details/82145013

以前对于这个算法只会调用，并没有深入的理解。现在碰到一个项目，没办法只能硬着头皮去理解了。网上看了很多资料，感觉写的也一般般，现在我讲自己的理解。

        卡尔曼滤波算法的核心是动态调整权值。用过互补滤波的应该知道它的权值是静态的，而卡尔曼是动态的。刚刚接触卡尔曼也不要紧张，我来一步步剖析这个东西。

       以上五条公式是卡尔曼的核心。它的本质就是通过预测结合测量来估计当前系统的状态。举个例子，假如我们要估计一架飞行器的姿态，可以通过IMU来实时测量，但是测量值有一定的风险是不准确的，所以并不能完全依赖传感器。任何一个满足物理规律的系统应当是连续的，所以我们还可以通过上一状态来预测当前状态。Kalman Filter正是结合这两条进行状态估计，到底是相信哪一个多一点，还要根据Kt来决定，我们定义Kt为卡尔曼增益，它是根据 测量和预测的协方差来计算的。

      先解释下每个公式所要表达的含义以及变量的含义：

line 2： 首先通过上一状态最优值和将要施加的控制量来预测当前状态，由假设一可以得到：

因为我们只是求均值，而高斯噪声均值为0，所以可省去最后一项。

At指当前时刻的状态转移矩阵（就是指从上一状态转变为下一状态的关系矩阵）；

Bt指当前时刻的控制矩阵（就是指影响控制量的控制矩阵）；

ut指当前时刻的控制量；

ut-1指上一时刻最优估计值；

指当前时刻估计值；

line 3： 除了预测均值之外，我们还需要预测值的协方差来计算Kalman增益。

指上一时刻的预测值协方差矩阵；

Rt指当前时刻测量值噪声矩阵；

line 4：根据预测值的协方差，测量值和状态的比例系数，测量值的协方差来计算Kalman增益。

Kt指卡尔曼增益（指的就是权值）；

Ct指的是当前时刻的测量方程；

Qt表示观察量的协方差矩阵；

line 5：这一行可以说是Kalman Filter 的精华了，现在我们有了对状态的预测值和协方差，同时也收集到了对状态的测量值。这时就可以通过kalman增益来计算状态估计值了。

增益越大，表明我们越相信测量值。

line 6： 根据 line3 ，预测当前状态需要用到上一状态的协方差，所以我们还需要计算当前状态的协方差用于下一次迭代。它同样要根据Kalman增益来计算：

相信到这里，大家应该对kalman Filter的原理有了一个大致的了解，算法中，从初始状态开始，不断计算当前状态的均值和方差来迭代，直至系统结束。

        上面解释了各个公式以及各个变量的含义。其实扩展卡尔曼的主要作用还是在不断的迭代中求出最接近真实值的那个值。卡尔曼滤波的作用有以下两种。第一，如果卡尔曼用作单种数据滤波（或者多种数据分开），那么将数据作为测量量传入模型中，卡尔曼模型会通过上一次的值估计出下一次的值，然后将此次的估计值和测量值分别取一定的权值（模型自己所计算的权值（卡尔曼增益）），求出这次的最优值。第二，是多数据的融合。可将一种数据作为测量量，另一种数据作为估计值进行融合。一般来说观测的数据要准确一些，估计的数据要不准确一些。

附上视频：https://www.bilibili.com/video/av4356232?from=search&seid=12641787542715420206

卡尔曼数据融合matalab代码：https://download.csdn.net/my/uploads/1/2