一、卡尔曼滤波器的应用场景卡尔曼滤波器之前被广泛用来做动态系统的状态估计、预测。主要的目的就是用来从带噪声的观测量，比如各种传感器的观测（IMU、GPS、里程计等）估计出最优的系统状态（state）。不过要明确强调的是，由于测量都带有噪声，也就是随机性，所以真正准确的状态是无法获知的。 最小二乘法可以从一长串的测量值回归出一个最为匹配的模型。卡尔曼滤波相比于最小二乘法，采用了一种递归的计算方式，也就是每一时刻只需要保存上一时刻的状态。因此可以被用来处理实时任务。 虽然卡尔曼滤波器从美国阿波罗登月计划到如今已有几十年，且目前有更先进的因子图（Factor Graph）算法，但是在很多领域依旧可以看到卡尔曼滤波器的身影，特别是在自动驾驶的各个模块，比如：从IMU的数据（三轴加速度，三轴角速度）计算出运动物体的当前位置。 二、卡尔曼滤波器的两个步骤——宏观认识卡尔曼滤波包含两个步骤
所谓预测，就是用一个数学模型，根据当前的传感器输入，直接计算此时系统的状态。可以理解为一个方程的计算就行。 所谓的更新，就是在某些时刻或者每一时刻，获取一些系统的状态输入（可以同样是传感器的值），甚至是预测阶段中同样的传感器的值，将其当作真值，我们将这个值叫做测量值。比较此刻预测的系统状态和测量的系统状态，对预测出的系统状态进行修正，因此也叫测量更新（measurment update）。  三、卡尔曼滤波的几大概念1、状态向量State Vector系统的状态向量包含了系统中我们关心的状态变量，比如速度，距离，加速度等。我们用表示系统向量 因为卡尔曼滤波有两个步骤，我们先以每个时刻都进行测量和修正这两个步骤作为讲解。那么，每一时刻的两个步骤的输出对应两个系统向量，一个是预测的系统向量，一个是修正后的系统向量。 ps:大写字母表示矩阵；小写加粗字母表示向量；小写字母表示变量 2、状态方程&&预测方程状态方程状态方程描述了将上一时刻的状态向量映射到当前的系统状态 其中，矩阵称为转换矩阵（Transition Matrix），是当前时刻的系统输入，矩阵称为控制矩阵（Control Matrix）反映了系统输入到系统状态的映射关系，是过程噪声，我们假定其符合均值为0，协方差矩阵为的高斯噪声。这里要注意的有几点：
预测方程预测方程跟状态方程基本一样，要强调的一点是，噪声是均值为0的高斯噪声，因此最大概率对应的值为0，因此我们在预测状态向量的时候其实不用管最后一项 这里对系统状态向量上面加上一个帽子表示这是一个估计量，也就是我们实际输出的量。其中表示上一时刻执行完步骤二更新后的状态向量，表示当前时候只执行了步骤1预测的状态向量。 理解预测方程预测方程（公式2）从形式上看只是把状态方程（公式1）去掉了噪声项，但其中包含了很多含义。其中，要时刻记得的是我们把系统状态当作一个随机向量处理，严格的表述需要引入随机过程的相关知识，但其实没太大必要。由于公式（1）中状态向量是一个随机向量，且噪声项是均值为0的高斯噪声，那么状态向量也满足高斯分布，其最大概率的地方就对应这噪声为0的地方，因此我们有理由将这个概率最大处对应的值当作我们的预测。  3、测量方程&&测量估计方程测量方程测量方程反映了系统的测量值和系统状态向量之间的关系 其中，是当前时刻的测量值，称为测量矩阵（Measurement Matrix），描述了从系统状态到测量值的转换关系（举一个最简单的关系，系统状态是物体的直线距离，测量值是使用激光笔测出来的光从原点到物体的时间，那么就是光速的倒数）, 是测量噪声，我们假定其符合均值为0，协方差为的高斯噪声。注意这里无论是状态向量还是测量向量都没有加帽子（），表示这是个'准确'的表达式。 测量估计方程同样，我们如果忽略噪声项，就变成了对此时测量量的估计（注意状态向量的上标和角标）： 4、测量更新方程上面给出了对测量量的估计，但是现在的情况是，我们有了一个测量值（通过某些测量方式或者传感器数据），这是一个不需要计算得到的量，我们如何用这个测量得出的量来更新我们对状态向量的估计？这就涉及到测量更新方程，这也是卡尔曼滤波里最难的部分，这里先给出更新方程的形式。 其中，叫做卡尔曼增益；是系统测量值的误差协方差矩阵。 四、误差和协方差矩阵根据公式（1）减去公式（2），我们可以得出误差的协方差矩阵的表达形式 把公式（1）和（2）带入误差表达式，可以推导出系统状态的协方差矩阵的递归表达形式 其中，从第二个等号到第三个等号利用了状态向量和随机噪声 的无关性（协方差为0）。因此我们可以得到下面的公式（7） 五、更新方程的推导现在我们进行到了这样的情况，我们使用预测方程公式（2）得到了时刻的系统状态预测量；同时，我们在这个时刻得到了一个测量值，且根据测量方程，我们认为它与此时的状态向量满足公式（3）： 。那么其实我们可以从公式（3）也得到此时系统状态的另一个估计，由公式（3）将 移动到等式左侧，可以得到： 这里之所以没有把噪声表示出来，是因为这里将 当作一个随机向量处理，其均值为，也就是当前时刻测量得出的量；其方差为（在公式3中给出） 我们的状态方程也给出了状态向量的一个等式 这里用上标1和2表示状态向量的两个来源。要强调的是，这两个等式里面都包含了一个随机噪声，因此都是随机向量。同时，他们的分布都符合高斯分布。它们的均值和方差分别是： 现在有了两个关于状态向量的概率分布，那接下来的事情就简单了。因为这两个状态向量的来源我们可以认为是独立的，因此他们的联合概率分布是各自概率分布的乘积。重点是，高斯分布的乘积依旧是高斯分布！！！！新的高斯分布的均值和方差有如下表达形式：  用上面的式子代入，可以得到联合概率分布的均值和方差：均值 方差 这个就是更新后的系统状态误差的协方差矩阵，也就是得到下面的式子 六、总结预测方程 更新方程 |
|
|
