卡尔曼滤波——卡尔曼滤波推导

卡尔曼滤波的基本推导与实例

目录

前言

一、基本数学概念

二、卡尔曼滤波推导

1.卡尔曼增益推导

2.    推导

总结

---

---

前言

该笔记是在学习up主DR_CAN的关于卡尔曼滤波视频后做的归纳与总结。
up主主页：https://space.bilibili.com/230105574

---

一、基本数学概念

均值：将数据集中所有数据相加，然后除以数据集中数据的个数得到的结果。尝用于描述数据集的中心趋势，均值通常用  来表示。均值计算公式为：

期望：对随机变量可能取值的加权平均值，其中权重是取值的概率。期望是对随机变量整体分布情况的描述，期望通常用  表示。期望计算公式为：

均值期望是两个相似而不同的概念。简而言之，均值是对数据集中的数值进行求和平均，而期望是对随机变量按照其概率进行加权平均。

方差：用来衡量数据与其均值的的偏离程度，用  来表示。 越大，表示数据的分布越分散，反之越集中。方差计算公式为：

协方差：用于衡量两个变量之间关系的统计量。它描述的是两个变量的联合变化程度 ，即一个变量偏离了其均值，另一个变量是否也偏离其均值 。协方差为正表示两个变量正相关，协方差为负表示两个变量负相关，协方差为0表示两个变量不相关。

协方差矩阵：协方差矩阵是描述两个或多个随机变量之间关系的矩阵，其中每个元素是两个随机变量之间的协方差。

标准差：是方差的算术平方根，用  来表示。标准差也反应数据的分散程度，且与原始数据的单位相同，相同情况下，标准差比方差更为直观。

方差和标准差都是描述数据分散程度的统计量。

正态分布：也叫高斯分布(Gaussian Distribution)，它的形状呈钟形曲线，分布在均值  附近，随着偏离均值越远，分布越稀疏。其概率密度函数表达式为：

                                                                                               

二、卡尔曼滤波推导

1.卡尔曼增益推导

现有一状态空间方程如下：

                                                                                                （1）

其中  是过程噪声， 是测量噪声， 表示 估计值， 表示测量值 ， 表示系统状态转移矩阵， 表示控制输入矩阵， 为测量矩阵  。

假设这些噪声满足正态分布，则有 ：

                                                                                                                    （2）

其中  和  分别是协方差矩阵，该正态分布的期望为 0 且      。

通常测量噪声和过程造成是零均值噪声。这是因为噪声是由许多不同来源产生的随机扰动所组成的，它们可以是正的或负的，而且通常是无偏的，也就是说它们在平均意义下相互抵消，期望为零。因此，假设噪声为零均值可以更好地描述实际情况。对于正态分布而言，均值为0，也符合这种假设。

这里使用协方差矩阵的原因是，要考虑不同维度间的相关性。通常一维变量用方差，多为变量用协方差矩阵。

因为  和  两种噪声都是未知的，那么已知的如下：

                                                                                                             （3）

如公式（3）就是去掉了噪声的值，因为少了噪声，所以这里的  只能是个估计值写作 ，​​​​表示先验估计，它是在给定当前时刻的观测数据和前一时刻状态向量的情况下，通过卡尔曼滤波算法计算得到的最优估计值。在卡尔曼滤波过程中， ​ 不断更新，以反映对系统状态的更准确估计。

我们将模型直接计算得到的称为先验变量，观测器测量得到的称为测量量，二者的表达式如下：

                                                                                            （4）

由于少了对噪声计算，这两者都是不准确的值。引入系数  ，通过数据融合的方法，带入公式(4)我们可以得到后验变量的表达式为：

                                                              (5)

由公式(5)可以看出当  时，，更倾向于相信计算得到的结果  ；当  时，

，此时则更相信测量得到的结果 。那么令带入式子（5）就能得到卡尔曼滤波中重要的方程之一，状态更新方程，也叫后验估计方程，也是书籍中卡尔曼滤波器的表达式：

                                            且                                （6）

于是我们的目标变为寻找合适的  使得后验估计与实际值的误差尽可能最小，将  趋近于 

即引入误差  使得  最小。

我们认为其满足正态分布即  ，同理  是一个协方差矩阵。

                                                                             （7）

其中cov是计算协方差的，例如上式子可以转化为

                                                                                           （8）

当  越小即 的方差越小，便越接近期望值 0 。所以想要  最小，则只要协方差矩阵的迹  最小。因为迹等于方差的平方和，所以只要迹越小，那么方差也就越小。

式子（7）的证明：

由  得

因为方差 ，且对于  来说 期望是 0 , 所以 

故而 ：  即  

对于方差来说有 ：

所以有：

 

而协方差   :

                                                                           （8） 

由式子（6）可以得到

 

进一步推导得：

                                                                             （9）

将（1）式子中的   可得

                        ​​​​​​​        ​​​​​​​        ​​​​​​​         

即         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                                         （10） 

其中  是单位矩阵， 是先验误差，即是噪声  。将式（10）带入式（8）可得：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​            (11)

 因为  是常数，于是上式子可化简为：

        ​​​​​​​        ​​​​​​​         （12）            

上面用到了线性代数中  ，

由于  和  相互独立，于是有  

这是因为  且   

于是乎，由  带入式（12）可得:           

                                                               (13)

 由式子   和式子（2） 可得：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                                                                             (14)

 是先验误差的协方差矩阵，将式（14）带入式（12）可得：

        ​​​​​​​        ​​​​​​​        ​​​​​​​                  (15)

因为  和 互为转置，而协方差矩阵的转置等于它本身。

因为互为转置的协方差矩阵，它们的迹是相同的。

 所以对上式（15）两侧分别取迹，则得到 式（16）：

因为我们的目的是寻找合适的  使  尽可能小，因为  符合正态分布，所以想让  越小，只要让其期望接近0，即   的方差尽可能的小。则只要协方差矩阵的迹  取极小值。要求的极小值，则需求

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                                                          （17）

所以对式（16）求导得：

  (18)

由于  ， 证明如下：

                ​​​​​​​

                 

以及

因为这里  是协方差矩阵，所以 , 故而再此案例中，

于是式（18）可以变为：

        ​​​​​​​                     (19) 

由于  和  都是协方差矩阵，是对称矩阵，所以有：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                                        （20）

所以最终变换式子求得卡尔曼增益  为：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​           ​​​​​​​                            （21）

通过推导，我们得出了卡尔曼滤波中三个重要方程，先验估计方程，卡尔曼增益方程，后验估计方程：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        

2.    推导

根据上面的结果，我们发现三大方程中，先验值  的我们并不知道，于是乎这一小节将求 

 由于  满足  ，所以带入式（1）和式（8）可得：

        ​​​​​​​        ​​​​​​​                                                                    （1）

        ​​​​​​​        ​​​​​​​                                                （8）

        ​​​​​​​        ​​​​​​​                    (22)

因为  和  相互独立，且  所以带入得到：

                                         

则式（22）可化简为：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                                 （23）

因为  且  所以上式可以变化成：

由式（15）可以得到 ：

        ​​​​​​​        ​​​​​​​        ​​​​​​​     

带入卡尔曼增益方程可得：

        ​​​​​​​        ​​​​​​​        

化简得到:

 至此卡尔曼滤波五大方程推导完毕，其表达式为：

预测：

先验估计：

先验协方差：

矫正（卡尔曼增益）：

后验估计：

更新后验协方差：

注意：

矩阵  和  是状态转移矩阵和控制输入矩阵，它们描述了系统状态随时间的变化规律和外部控制输入对系统的影响。在实际问题中，这些矩阵通常需要通过对系统进行建模来确定，可以通过物理模型、经验公式、数据拟合等方法来获得。

矩阵  是过程噪声的协方差矩阵，描述了系统在运动过程中由于各种因素引起的噪声影响。通常情况下，它也需要通过对系统进行建模来确定，可以通过测量数据、统计分析等方法来获得。

矩阵  是观测矩阵，它描述了系统状态与观测量之间的关系。在实际问题中，这个矩阵通常也需要根据具体问题来确定，可以通过物理模型、测量数据、经验公式等方法来获得。         

---

这是南叔先生写的卡尔曼滤波实际运用例子。

https://blog.csdn.net/seek97/article/details/120012667

---

总结

以上是卡尔曼滤波的五大方程的基本推导与示例应用。