卡尔曼滤波-理论专题
1. 卡尔曼滤波简介
卡尔曼滤波器，可以从一组不断输入到系统的数据中，非常快速的推断出真实中（几乎），即使初始值偏差很大也没关系，卡尔曼滤波的收敛速度非常快。
2. 流程图
- 卡尔曼滤波器的流程如图所示，外部输入用红色表示，其中输入的数据data是不断加入系统的，而初始的估计误差和初始估计值，仅输入一次。
- 我们通过初始输入的误差，和数据误差，来计算卡尔曼增益。卡尔曼增益实际上就是考虑如何对输入的估计误差和数据误差进行加权，这取决于我们更信任哪个数据（误差越小越信任）
- 通过初始估计值和输入数据，计算新的估计值。
- 通过卡尔曼增益和新的估计值，计算新的误差。
- 将新的误差和作为输入，用于进行下一次卡尔曼增益的计算。
- 讲新的估计值作为更新数据，输出系统。同时作为下一次输入的估计值
3. 卡尔曼增益
卡尔曼增益KG是一个0-1之间的小数，公式如上图。我们可以看到，KG越接近1，说明EMEA越小，反正说明EMEA越大。EMEA是测量误差，也就是测量误差越大，KG越小，测量误差越小，KG越大。
ESTt 即当前最新估计值，它等于上一次的估计值ESTt-1 + KG*[MEA-ESTt-1]
这么构造KG的原因是，我们把KG看作是一个权重，我们希望，测量误差大的时候，减少对测量值的信任权重。
所以，测量误差比较大时，KG*[MEA-ESTt-1]这部分的权重会小一些，使我们可以 减少测量值对估计值的影响。
最终，卡尔曼增益要像越来越小的方向发展，既估计值误差越来越小，越来越不受测量值影响（趋于稳定）。
4. 卡尔曼滤波3大计算公式
本节我们主要关注的是公式3，即如何计算最新的估计误差，估计误差可以化简成EESTt = (1-KG)EESTt-1的形式，观察该公式我们可以得到如下结论：
- 最新的估计误差总是小于之前的估计误差，因为0<(1-KG)<1，误差是趋向于0的。
- 如果测量误差大，则KG小，系数1-KG就大，则估计误差收敛的速度也就比较慢。及测量误差大的时候，我们希望误差减小的速度慢一些，防止误判。
- 如果测量误差小，则KG大，系数1-KG就小，则估计误差收敛的速度也就比较快。及测量误差小的时候，我们希望误差减小的速度快一些
5. 计算样例
6. 多维卡尔曼滤波
- X状态矩阵
- Xm 测量得到的状态矩阵
- Xkp 预测的状态矩阵
- Xk 经过更新的状态
- P处理协方差矩阵（估计误差）
- K卡尔曼增益
- k状态序列序号，0为初始值
- R传感器误差协方差矩阵（测量误差）
- Y测量值矩阵
- Z测量噪声（不确定性）
- Q 处理误差协方差矩阵
- u 运动控制矩阵（表明如何达到新状态）
- w 预测误差
- A，B，C ，H变换矩阵
- A，B 状态变换矩阵（看作常系数）
- C，H 测量变换矩阵
算法流程
- 前序状态（previous state）中有两个变量Xk-1和Pk-1，我们对其进行预测，得到新状态Xkp和Pkp，用于计算KG
- 预测阶段
- 预测方程1：$X_kp = AX_k-1 + Bu_k + w_k$，其中uk表示的是控制变量矩阵，也就是系统如何控制我们的前序状态向新状态转移。例如uk可以是某个物理定律。wk是我们在预测时的误差。
- 预测方程2: $Pkp = AP_k-1A^T+Q_k$
- 更新阶段（更新测量值和KG）
- 基于预测值Pkp和测量误差R计算KG
- 新测量数据 $Y = CX_km+Z_k$
- 基于预测值Xkp和新测量数据Y，结合KG计算估计值Xk
- 输出最新估计值
- $X_k$
- $P_k =(1-KG)P_kp$
- 新估计值作为输入，从新进入预测阶段。
结合课程中的表述来看，C，H是一个值，F是A，B的结合。u=w表示运动向量
7. 计算过程
- 根据X维度不同，A和B也不尽相同。
- Xk的计算方法，即之前的状态加上在某个运动规律下，t时间间隔内状态变化值，再加上一个误差。上图中，uk=1/2*at^2
- A矩阵用于计算Xk-1在$\Delta t$时间内，状态Xk-1的改变
- AX即对X进行变换，新的位移为$y+\Delta t\dot y$，新的速度为$\dot y$，加速度的情况在uk中体现
- u在此处即为加速度
- 三种情况加速分别为,0,g,a
- B是如何确定的？根据运动公式的系数。要考虑到加速度对速度和位移的影响
预测的当前状态矩阵即为Xk
- C 矩阵表示我们一次可以测量状态矩阵的几个维度
- 例如某传感器只能测量位置，则C=[1,0]
- 例如某个传感器可以同时测量位置和速度，则C为单位矩阵
- Yk是基于我们的传感器能力C，我们可以测量并输入到设备中的测量值状态矩阵。
