看了B站up主DR_CAN讲的卡尔曼滤波（链接）。up讲的非常好，强烈推荐，看完终于明白了卡尔曼滤波的奥秘。下面是我对其中内容的注解，或者说自己的理解。大部分推导省略了，但保留了算法的思想脉络。

引入

首先看算法是干什么的。

我们假设一种场景，雷达跟踪飞机的运动。我们在各个离散的时间点测量飞机的位置。我们假设知道了飞机在时刻的位置，要预测它在时刻的位置。为什么要预测呢，因为方便我们对它进行跟踪。我们有两种办法得到位置。

• 我们通过某种经验假设认为飞机符合某种运动规律，比如匀加速运动，也就是说，我们可以建立某种运动模型。这样，假设模型是完美的，那么根据，我们就可以算出时刻的位置。然而，模型里面存在一定的不确定性，里面存在随机的因素，这样，我们就不能准确得到位置，而是得到一个位置的估计。
• 我们可以直接测量时刻的位置，记为。然而，测量也存在随机的因素，不能准确测量，因此，也是真实位置的一个估计。

可以看到，我们可以得到真实位置的两个估计，一个是通过运动模型计算出来，一个是测量出来的。而二者都不等于真实位置，而是存在一定的误差。我们接下来的任务是，根据这两个估计来进行某种运算，得到一个最终的、更准确的估计值。

一个最直接的思路是对二者取平均。但是，假设我们知道，我们测量的准确度高于模型的准确度，那么，我们更应该相信测量的值，反之同理。也就是说，我们需要一个加权平均，如下：

转换一下得到：

                                                      （1）

那么问题就成了如何选择G，使得我们的估计效果最好，或者说在某种意义下最优。这正是卡尔曼滤波所要完成的任务。

状态空间方程

（一）假设k时刻的状态与k-1时刻的状态、k-1时刻的控制量存在如下关系：

                                                （2）

其中，状态就是我们关注的变量，如上面的飞机位置。由于存在多个状态变量，因此它一般是一个向量。控制量则是外界施加的某种作用量，是一个确定的量。A和B都是矩阵。而是过程噪声，用于刻画模型的不确定性。它是一个随机过程，或者说对某个确定的k，它是一个随机变量。

（二）假设k时刻的测量值与k时刻的状态的关系如下：

                                                                         （3）

其中，是测量噪声，用于刻画测量的不确定性。

估计过程

（一）我们先根据式（2）求出的一个估计：

我们省去了过程噪声，因为它是个随机变量，我们没办法知道它的准确值，所以不考虑它。称为先验估计，可以理解为一种初步的估计。需要注意的是，上面也有一个尖尖，它也是一个估计值，因为真实值是永远不知道的。

（二）根据式（3）求出的另一个估计：

这里也省去了测量噪声。

（三）接下来根据式（1），做加权平均：

通常，我们会做个变量代换，令，得到：

其中，称为卡尔曼增益，其实它就是一个权重，用于控制我们对于模型估计量和测量估计量之间的倾向程度。称为后验估计，也就是我们依据测量值做的进一步估计，是最终的估计值。

卡尔曼增益

接下来，就是要找到一个合适的。我们自然希望使得估计误差最小，即最接近于。估计误差为：

注意，是一个零均值的随机变量，它的方差越小，它就越接近0，就越接近于。由于实际上是一个向量，所以我们的目标就是使得的协方差矩阵的迹最小，也就是各个方差之和最小。

接下来，先求出的协方差矩阵，这个过程就不写了，DR_CAN的视频里给出了详细的推导。的公式看着很复杂，其实涉及到的无非是线性代数和概率论的一些基本知识。如下：

     （4）

其中，是先验估计误差的协方差矩阵，后面会推导；R是测量噪声的协方差矩阵，是已知的。

接下来求的迹，并对求导，令导数=0，就得到最优的：

                                       （5）

误差的协方差矩阵

式（5）中，目前还不知道。可以求出，

其中，Q是过程噪声的协方差矩阵，也是已知的。

可见，我们还需要求出，用于下一次计算。我们把式（5）带入到式（4）中，就可以求出：

总体算法流程

于是，我们得到了总的算法流程。

（1）计算先验估计

（2）计算先验误差的协方差矩阵

（3）计算卡尔曼增益

（4）计算后验估计

（5）更新误差协方差矩阵

以上5步不断进行迭代，就是我们根据先验估计和测量值计算后验估计值，来逼近真实值的过程。

在迭代刚开始，需要初始的状态和误差协方差矩阵。可以以0时刻的测量值作为初始状态。