卡尔曼滤波3：使用条件和迭代过程

本文目的

卡尔曼算法在应用中如何建模，做了哪些近似，有哪些假设，如何迭代。

便于理解，本文用信号代替系统状态，所谓系统状态，其实也就是需要求得的信号

前置知识点

1. 基本概率论知识
2. 卡尔曼滤波1：状态空间方程
3. 卡尔曼滤波2：数据融合datafusion

卡尔曼方程个人理解

卡尔曼滤波是一种data fusion的过程，本质上使用了有噪声的信号数学模型，以及一个传感器观测模型，通过两个数据的融合得到更高精度的系统状态（信号），同时是被证明的最优线性滤波。

卡尔曼滤波使用过程

从信号建模，前提条件，到迭代过程，最后到初值设置来进行讲解。

1. 状态空间方程建模

系统的状态（信号）可以被建模成如下两个式，“卡尔曼滤波1：状态空间方程”解释了为什么两个方程要建模成这样。

• 系统状态方程，表征从系统的下一个状态可以在一定程度上有系统的上一个状态进行预测。
  $$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$$
• 观测状态方程，表征了系统当前时刻状态可以由传感器观测得到
  $$z_k=H{x}_k+V_k$$

各个参数含义如下表
$x_k$：状态变量
$A$：状态矩阵
$B$：控制矩阵
$u_{k-1}$：输入控制矢量
$w_{k-1}$：过程噪声，无法建模
$V_k$：测量噪声
$H$：观测矩阵

• 理想情况
  在理想模型的情况下，只要有一个公式就可以求解出系统变量
• 实际情况
  由于有无法建模的噪声，两个公式求出来的都不太准
• 数据融合
  如何用系统方程和观测方程这两个不太准确的方程，最终测出一个相对来说比单个方程测量都准确的数据，并且是通过线性加权进行的数据融合，这就是卡尔曼滤波。

2. 使用卡尔曼滤波的前提条件和假设

1. 无噪情况下，信号的当前值可以由历史值过一个线性系统得到，也就是具有系统状态方程。
2. 另外有一个传感器，无噪声情况下，其观测到的数据和信号的当前值是一个线性关系，也即是观测状态方程。
3. 事实上有噪声，系统状态方程里会有过程噪声，观测状态方程有观测噪声，需要知道或者通过参数估计得到过程噪声和观测噪声的协方差矩阵
   1. 假设$V_{k-1}～N(0,R)$，是一个随机矢量，$R$是$w$的协方差矩阵，$R$已知或者可以参数估计估出来。
   2. 假设$w_{k-1}～N(0,Q)$，是一个随机矢量，$Q$是$w$的协方差矩阵，$Q$已知或者可以参数估计估出来。

3. 迭代过程

3.1 每次迭代的已知内容

这里假设k-1时刻已经完成了状态估计，此时新数据到来，要进行第k时刻的迭代。
下表列出已知的变量以该变量是如何获取的（从使用角度可以不用理解具体含义）

$A$：状态矩阵，系统状态方程建模得到。
${\hat{x}}_{k-1}$：上一个过程的卡尔曼估计状态输出。
$B$：控制矩阵，系统状态方程建模得到。
$u_{k-1}$：输入控制矢量，应当在系统中作为外部输入已知的。
$P_{k-1}$：上一时刻的后验误差协方差，上一个过程计算出来的。
$Q$：过程噪声协方差，系统状态方程已知或者更常是由数据数据估计得出。
$R$：观测噪声协方差，观测状态方程已知或者更常是由数据数据估计得出。
$H$：观测矩阵，观测状态方程建模得到。
$z_k$：传感器观测信号（也就是观测状态）。

3.2 预测过程

1. 由系统状态方程对系统状态进行估计，称之为先验估计（未使用观测结果的估计）
   $${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$

${\hat{x}}_k^{-}$就是k时刻系统状态先验估计。
${\hat{x}}_{k-1}$是k-1时候后验估计，同时也是k时刻卡尔曼滤波输出的估计状态。

2. 更新先验误差协方差
   $P_k^{-}=A{P}_{k-1}{A}^T+Q$

$P_k^{-}$用于计算误后验误差协方差$P_k$

3.3 更新/校正过程

在先验估计和观测数据进行数据融合，得到后验估计（未使用观测结果后最优的估计）

1. 计算卡尔曼增益
   $$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$
2. 计算后验估计
   $${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$$

同时也是卡尔曼的输出估计状态，基本上所有的计算都是为了这一步服务的
这个公式本质上就是状态方程得出的先验估计，和传感器观测状态做数据融合
目标是寻找最合适的$K_k$使得${\hat{x}}_k$最准确
数据融合的公式长成这样，可以看卡尔曼滤波2：数据融合datafusion

3. 更新误差协方差
   $$P_k=(I-K_{k}H)P_k^{-}$$

4. 初值设置

迭代过程中有一些内容是基于上一个时刻的迭代过程的，在第一次迭代的时候，这些内容应当如何设置呢？
主要包括两个
参考《随机信号处理》陈芳炯 5.4卡尔曼滤波 p294

${\hat{x}}_0=E(x_0)$
$P_0=E((\hat{x}-x)(\hat{x}-x{)}^T)$
服了，垃圾书看不明白

• 以下部分是个人对初始化的一些理解
  如果且系统初始状态没有先验，但是状态转移方程能保证初始系统状态不变的情况下：
  1. 先不使用状态转移方程，采集n组传感器数据，计算修正样本协方差和期望。
  2. 对于${\hat{x}}_0$，由于$E(H^{-}z)=x$，可以通过系统状态不变时的多次测量取平均值得出，当然具体情况也应该具体分析
     $${\hat{x}}_0=\frac{1}{n}\sum _i^n{H}^{-}{z}_i$$

  这里的$z_i$是初始化的时候多次测量的数值，后续迭代的时候要重新从$z_1$算起

  3. $P_0=E(({\hat{x}}_0-x)({\hat{x}}_0-x{)}^T)=\frac{1}{n}{H}^{-}R(H^{-}{)}^T$
  4. 如果这个初始数据采集（n）够多的时候，可以近似认为对于$P_0=\frac{1}{n}R=0$

参考文献

B站DR_CAN-卡尔曼滤波器4
《随机信号处理》陈芳炯 5.4卡尔曼滤波 p294

后续内容

算法推导过程，为什么数据融合采用这样的卡尔曼增益是最合理的