卡尔曼滤波学习记录

1.简介：

卡尔曼滤波是一种数学工具/算法，用来在存在不确定信息的情况下，​估计一个动态系统的内部状态​（即使你无法直接观察到它），并且能让这个估计更加精确。

2例子：

想象你在一个雾天开船（系统状态未知）：

1. ​你知道大致方向和速度（预测）：​ 你知道之前的位置，船的马力（系统模型），可以预测一下船现在大概在哪里。但这个预测有误差（比如风浪影响）。
2. ​你有仪表读数（测量）：​ 你有不精确的GPS定位（测量），它会显示你当前的位置，但这个读数也有误差（比如GPS信号漂移）。

你并不知道哪一个是正确的，这个时候单一使用你自己预测的位置或者读出的位置都不是特别准确，因为他们各自都有各自的偏差，那么如果我们把两个用某种方法把他们的优点结合起来是不是就能获得更准确的位置了？事实也如此，而这个方法就是卡尔曼滤波算法。

它的作用如下：

1. ​结合预测和测量：​ 把你的预测（可能有误差）和GPS测量（也有误差）结合起来。
2. ​衡量谁更可靠：​ 判断哪个信息（预测或测量）在当前更可信、误差更小。
3. ​给出最优估计：​ 给出一份​“融合后”的最优估计位置。它比单独依靠预测或单独依靠测量更精确、更可靠。
4. ​循环进行：​ 这个助手会不断重复这个过程（预测下一步 -> 测量 -> 结合 -> 得到新的最优估计 -> 再预测下一步…）。

核心思想就是：

1. ​预测：​
   • 根据你建立的系统数学模型​（描述系统怎么变化）和你上一次的最优估计，​预测系统当前的状态会是怎样的。
   • 同时，预测的不确定性/误差是多少？因为你模型不完美，这个预测肯定有误差。
2. ​更新：​
   • 获取实际测量值​（传感器读数）。
   • 比较预测的状态和测量的状态之间的差异。
   • 计算测量值的可信度（权重）​ 和预测的可信度（权重）​：谁的误差小，谁就更可信，赋予更大的权重。
   • 将预测和测量按最优方式（加权平均）融合在一起，形成一个新的、当前时刻的最优估计状态，并且计算出这个新估计的不确定性/误差。

原理推导：

首先（线性）卡尔曼滤波的应用基于以下三个假设前提：

1. 当前时刻状态只和上一时刻状态有关。
2. 模型和系统均满足线性关系。
3. 引入的噪声符合高斯分布。

因此会有开头的两条方程，第一条为状态模型，第二条为过程模型。测量值Z可以和状态矩阵X的维度不相同（因为有可能存在观测不到的分量），比如一辆小车，它在移动的过程中同时具有速度和位置两个状态分量，但是我们可以测量得到的可能就只有它的速度，H是观测转移矩阵，就是将原本二维的状态转化为一维的测量值。

A就是状态转移矩阵，通过这个矩阵把上一时刻的状态转化为下一时刻的状态。B就是控制矩阵，将控制输入量（比如加速度）转化为状态量（比如速度）

总结的五个公式就是卡尔曼滤波算法要用到的具体公式，只要给出X的初始后验估计值和误差协方差矩阵P的初始值就可以不断自动矫正得到越来越准确的状态值，在不准确之中得到最准确的估计值，这就是卡尔曼滤波的作用。下面补充误差协方差矩阵P的化简。