卡尔曼滤波器原理

1.卡尔曼滤波作用

    卡尔曼滤波实现简单而且是纯时域的滤波器，不需要进行频域变换。在任何含有不确定信息的动态系统中使用卡尔曼滤波，可以对系统下一步的走向做出有根据的预测。优点：占用内存小（只需保留前一个状态量），速度快，适用于实时问题和嵌入式系统）。

2.状态预测公式

        假设有一辆车在路上行驶，我们用位置和速度表示当前行驶状态，设状态变量(u表示加速度）：

                                                                        

    其中  ,                    

                                                

    观察可得，它们的输出变量都只是输入变量的线性组合，所以卡尔曼滤波器是最佳的线性滤波器，它只能描述状态与状态之间的线性关系。就可以把它写成矩阵的形式：

                                                

    进一步提取两个状态矩阵：

                                                

    所以可将公式简化成：

                                                             --------------------------------------------（1）

    这就是卡尔曼滤波器中的第一个公式，状态预测公式，其中F叫状态转移矩阵，表示如何从上一时刻的状态来推测当前时刻的状态，B叫控制矩阵，表示控制量u如何作用于当前状态。表示是对x的估计量，而不是真实值。因为真实状态不可知，我们只能根据观测来尽可能地估计x的值。等号左边有一个减号上标，表示这一时刻的值是通过上一时刻的值推测而来的。待会还会通过观测量修正x的值，修正之后才算最佳估计值，减号上标会去掉。

3.协方差矩阵

    有了状态预测公式，我们就可以预测当前状态。但是所有的推测都是包含噪声的，噪声越大，不确定性越大，如何来表示这次推测带来了多少不确定性，我们用协方差矩阵来表示。

    一维情况：假设我们有一个一维的包含噪声的数据，每次测量的值都不同，但都是围绕在一个中心值周围。

                                

    我们表示它们分布状况的最简单的方法就是记下它的中心值和方差，实际上是高斯分布。

                                    

    二维情况：这里有一个二维包含噪声的数据分别对两个坐标轴进行头晕，在两个坐标轴上都是高斯分布：

                                                                

                                                                                             图一

        在表示分布的时候，如果两个维度是独立的时候，我们分别记下两个坐标轴的中心值和方差，但如果两个维度有相关性，比如在一个维度上噪声增大另一个维度相应地噪声也增大，如下图所示：

                                                                    

                                                                                             图二

        或比如一个维度噪声增大另一个维度噪声减小，如下图所示：

                                                                    

                                                                                                  图三

        这时候在两个坐标轴上的投影跟第一个图完全没有变化，仍然是高斯分布，所以为了表示这两个维度之间的相关性，除了要记录两个维度之间的方差之外，还要用协方差来表示两个维度之间的相关程度。把它们写成矩阵的形式如下：

                                                    

        对角线上两个位置的值是两个维度的方差，副对角线上两个位置的值是相等的，是它们的协方差。上面三个图中，第一个图的，第二个图的，第三个图的。在卡尔曼滤波器中，所有关于不确定性的表述都要用到这个协方差矩阵。

4.噪声协方差矩阵的传递

    在小汽车的例子中，每一个状态的不确定性都是由协方差矩阵P来表示，在状态转换矩阵左右两边乘上协方差矩阵便可以让这个不确定性在每个时刻之间传递：

                                                            

    因为有协方差矩阵的性质：

                                                    

    但这时候我们考虑到预测模型不是百分百准确地，它本身也是包含噪声的，所以我们还在后面加上一个协方差矩阵Q来表示预测模型本身带来的噪声：

                                                     --------------------------（2）

    这是卡尔曼滤波器的第二个公式，表示不确定性在各个时刻间的传递关系。

5.观察矩阵

    对小汽车模型来说，假设我们在公路的一端放一个激光测距仪，在每个时刻都可以观测到汽车的位置，观测到的值我们记为Zt，汽车本身状态Xt与观测状态Zt之间有一个变换关系，我们记为H，当然这个变换关系也只能是线性关系（因为卡尔曼滤波器是线性滤波器），所以我们把H写成矩阵的形式，也叫观测矩阵。X和Z的维度不一定是相同的，在我们的汽车模型例子是，X是二维列向量，Z只是一个标量，所以H是一个一行两列的矩阵，里面的元素分别是1和0（因为测距仪测得的是距离，它和X的第一个元素相等）。所以例子里：

                                                            

    后面加小v是因为，我们的观测值也不是百分百可靠，所以后面也要加上v来表示观测的噪声，观测噪声的协方差矩阵我们用R来表示。因为在我们的例子里，观测值是一个一维的值，所以R的形式也不是一个矩阵，而是一个单独的值，仅仅表示Z的方差。假设除了激光测距仪外，我们还有其他测量方法，可以观测到小汽车的某项特征，Z就会变成多维的列向量，会包含每一种测量方式的测量值，而每一个测量值都是真实状态的一种不完全表现。我们可以从几种不完整的表述里面，推断出真实的状态, 而卡尔曼滤波器的数据融合的功能，正是在这个测量矩阵中体现出来。

6.状态更新

    我们有了观测量Z和它的噪声的协方差矩阵R，那么我们如何把它们整合进我们对状态X的估计，我们前面得到了带减号上标的Xt，现在我们只要在它后面加上一项，来修正它的值，就可以得到我们的最佳估计值，

                                                    ----------------------=------（3）

    加号后面括号里的值表示实际的观测值和我们预期观测值的残差，残差乘上一个系数K，就可以用来修正Xt的值，K十分关键，叫卡尔曼系数，实际上它也是一个矩阵，它的公式是：

                                                    -------------------------（4）

    卡尔曼系数K的作用主要有两个方面：

        （1）权衡预测状态协方差P和观察量协方差矩阵R的大小，来决定我们是相信预测模型多一点还是观察模型多一点。如果相信预测模型多一点，残差的权重就会小一点，如果相信观察模型多一点，残差的权重就会大一点

        （2）把残差表现形式从观察域转换到状态域。比如刚才的观察值Z是一维的向量，状态量X是二维向量，它们所用的单位甚至描述的特征都有可能不同，如何用观察值的残差更新状态值，这个卡尔曼系数K就是做这样一个转换。在我们的例子里，我们只观察到了汽车的位置，但K里面已经包含了协方差矩阵P的信息，所以它利用位置和速度这两个维度的相关性，从位置的残差里面推算出了速度的残差，从而让我们可以对状态X的两个维度同时进行修正

7.噪声协方差矩阵更新

                                                        ------------------------------（5）

    最后一步就是更新最佳估计值的噪声分布，这个值是留给下一轮迭代时用的，在这一步里，状态的不确定性是减小的，而在下一轮迭代中，由于传递噪声的引入，不确定性又在增大，卡尔曼滤波器就是在这种不确定性变化中寻求一种平衡的。

8.五个公式

    到目前为止，我们已经有了卡尔曼滤波的五个公式，在这五个公式中，前两个是通过上一时刻的状态来预测当前时刻的状态：

    预测：                                                     

    通过这两个公式，我们得到的是带减号上标的X和P，表示这并不是最佳的估计值，还欠缺一些东西，这些欠缺的东西需要从观测值里面的信息带来，因为我们还没有考虑当前时刻的观测值。

    后面上个公式就是用当前的观测值来更新后面的X和P，经过更新后的值就是最佳观测值，所以它们不带减号上标。