android 卡尔曼滤波库 详解卡尔曼滤波

卡尔曼滤波

• 个人理解
• 两种信号

• 状态变量（按照人类经验预测的状态）
• 观测变量（根据生活工具测量的实际状态）
• 噪声

• 噪声分布
• 噪声表达

• 滤波器的计算原型

• 先验估计、后验估计与测量状态变量
• 计算增益系数K

• 数据融合基础变量（先验估计、测量状态变量）
• 一、列表达式变形（后验估计）
• 二、引入误差
• 三、计算最小协方差矩阵等式
• 增益矩阵特点

• 计算先验误差协方差P^-

• 推算未知变量
• 先验误差协方差 P^-表达式

• 滤波循环过程

• 基本循环过程
• 完善循环过程

个人理解

  在卡尔曼滤波中，有两种变量。
  第一种是我们预测的状态变量，是我们从日常的生活经验中总结出来的普遍公式来进行预测的，就像一辆小车在镜面上行驶，速度 2m/s,我们通过 s = v * t，预测下一秒会前进两米，但实际结果与预测有差异，因为我们的公式没办法将实际的情况全部体现出来（像摩擦力，风速等未在公式中体现）。
  第二种就是我们实际测量的观察变量，我们用尺子测量小车在一秒内行进的距离。这个变量因为尺子本身的局限性也无法测量出小车的实际前进距离。
  那么在生活中我们就没法得到小车1 s内的实际前进距离了吗，的却是这样的，我们得到的数据都是满足可接受误差的数据。卡尔曼滤波就是让得到数据不停逼近实际数据的算法。其大体思想就是将状态变量与观测变量进行数据融合得到接近真实数据的数据，然后作为下一过程的状态变量，再与下一过程的观测变量进行融合，如此往复，最后的数据就会非常接近真实数据。

两种信号

  我们得到的都是接近实际数据的数据，那么如何表示实际数据。这里引入状态变量公式，在公式中加入不断变换的噪声，来弥补我们无法预测的误差。

状态变量（按照人类经验预测的状态）

x为状态变量（预测得来的状态变量），u为控制变量，A为状态矩阵，B为控制矩阵，w为过程噪声。下角标 k表示第几个过程。

观测变量（根据生活工具测量的实际状态）

z观测变量，H增益矩阵，x状态变量（测量得来的状态变量），v观测噪声。下角标 k表示第几个过程。

z为工具直接测量的数据，不可直接使用，需经增益矩阵转化后得到测量的状态变量 x进行使用。增益矩阵为 I（单位矩阵）时，z与 x近乎相同。

噪声

噪声分布

  变量分布就是该变量符合的密度函数。

过程噪声和观测噪声相互独立，遵循正态分布。在实际系统中，过程噪声协方差矩阵 Q和观测噪声协方差矩阵 R可能随着每次迭代计算而改变。

噪声表达

  假设w为两阶
$w=\left[ \begin{matrix} w1\\ w2 \end{matrix} \right]$

$Q = E\left[ \begin{matrix} w,\mathbf{w}^\intercal \end{matrix} \right] =E\left[ \begin{matrix} \left[ \begin{matrix} w1\\w2 \end{matrix} \right] \left[ \begin{matrix} w1,w2 \end{matrix} \right] \end{matrix} \right] =\left[ \begin{matrix} E(w1²) ，E(w1w2)\\ E(w2w1),E(w2²) \end{matrix} \right]$

故σ²(x)= E²(x)，σ(x)= E(x)

$Q =\left[ \begin{matrix} σ²(w1) ，σ(w1)σ(w2)\\ σ(w2)σ(w1),σ²(w2) \end{matrix} \right]$

滤波器的计算原型

先验估计、后验估计与测量状态变量

$\hat{x}$_k^- :第 k个过程的先验状态估计，就是去掉噪声的预测得来的状态变量。
$\hat{x}$_k测:第 k个过程的测量状态变量，就是去掉噪声，由测量变量 z经增益矩阵 H转化来的状态矩阵。
$\hat{x}$_k:第 k个过程的后验状态估计，就是去掉噪声的融合数据变量。
(- 表示先验。^表示估计。因为ht元素与Latex公式不融合，下面公式中由x^代替 $\hat{x}$。)

计算增益系数K

数据融合基础变量（先验估计、测量状态变量）

先验估计：

x^ _k^- = (A * x^ _k-1) + (B * u^ _k-1) .

测量状态变量：

Z _k= H * x _k测  ➡ x _k测 = H ^- * Z _k .

一、列表达式变形（后验估计）

表达式1：

x^ _k = x^ _k^- + G * (H ^- * Z _k - x^ _k^-).   G∈[0,1] G = 0时，x^ _k = x^ _k^-

G = 1时，x^ _k = x _k测

令G = K_k * H,得表达式2（后验估计公式）：

x^ _k = x^ _k^- + K _k* (Z _k - H * x^ _k^-).   K _k∈[0,H ^-] K _k = 0时，x^ _k = x^ _k^-

K _k = H ^-时，x^ _k = x _k测

二、引入误差

  不同于单个过程，直接以融合数据变量方差越小越精确。多个过程引入了误差，以误差方差或协方差矩阵越小越精确。

误差公式：

e _k = x _k -x^ _k .   p(e _k)~(0, P) e _k^- = x _k -x^ _k^- .   p(e _k)~(0, P)

  数据为一维时，误差方差越小越精确.
  数据为多维时，误差协方差越小越精确，即误差协方差矩阵的迹越小越精确。（协方差矩阵大小由协方差矩阵的迹来反映。）

以二维数据为例:
$e=\left[ \begin{matrix} e1\\ e2 \end{matrix} \right]$
$P =E\left[ \begin{matrix} e,\mathbf{e}^\intercal \end{matrix} \right] =\left[ \begin{matrix} σ²(e1) ，σ(e1)σ(e2)\\ σ(e2)σ(e1),σ²(e2) \end{matrix} \right]$
协方差矩阵迹最小，即是 σ²(e1) + σ²(e2)最小。

三、计算最小协方差矩阵等式

1. 计算误差协方差 P表达式

P_k = P_k^- - (K_k * H * P_k^-) - (P_k^- * $\mathbf{H}^\intercal$ * $\mathbf{K}^\intercal$_k) + (K_k * H * P_k^- * $\mathbf{H}^\intercal$ * $\mathbf{K}^\intercal$_k) + (K_k * R * $\mathbf{K}^\intercal$_k) .

计算过程如图：

2. 计算误差方差的迹 tr(P_k)表达式

tr(P_k) =tr(P_k^-) - 2tr(K_k * H * P_k^-) + tr((K_k * H * P_k^- * $\mathbf{H}^\intercal$ * $\mathbf{K}^\intercal$_k) + tr(K_k * R * $\mathbf{K}^\intercal$_k) .

计算过程如图：

3. 使协方差矩阵的迹 tr(P_k)对 K_k求导，并等于0. 得到迹结果最小时的等式

令 $\frac{d[tr(P_k)]}{d(K_k)}=0 .$
得
$(P_k^- * \mathbf{H}^\intercal) + K_k * (H * P_k^- * \mathbf{H}^\intercal + R) = 0 .$

进行化简，得增益系数表达式：

P_k是误差的协方差，R是测量噪声，H为增益矩阵。计算过程如图：

增益矩阵特点

  当测量噪声 R很大时，意味着测量不准确。我们选择相信预测变量。
  当测量噪声 R很小时，意味着测量很准确。我们选择相信测量变量。

在公式中体现：
  测量噪声 R很大，K_k→0，$\hat{x}$_k→$\hat{x}$_k^- 。
  测量噪声 R很小，K_k→H^-，$\hat{x}$_k→$\hat{x}$_k测 。

计算先验误差协方差P^-

推算未知变量

  目前要求出后验估计，需要对先验估计公式，卡尔曼增益公式与后验估计公式进行运算。
1 .
先验估计公式：

x^ _k^- = (A * x^ _k-1) + (B * u^ _k-1) .

其中，状态矩阵 A已知，上一过程状态变量 $\hat{x}$_k-1已知，控制矩阵 B已知，上一过程控制变量 $\hat{u}$_k-1已知。则先验估计 $\hat{x}$_k^-可求出。
2 .
后验估计公式：

x^ _k = x^ _k^- + K _k* (Z _k - H * x^ _k^-).   K _k∈[0,H ^-]

其中，先验估计 $\hat{x}$_k^-已知，观测变量 Z_k已知，增益矩阵 H已知。仅增益系数 K_k未知。

3 .

卡尔曼增益系数公式：

其中，增益矩阵 H已知，观测噪声 R已知。仅先验误差协方差 P^-未知。

  综上，只需要求出先验误差协方差 P^-，即可得出增益系数 K_k，进而求出后验估计 $\hat{x}$_k。

先验误差协方差 P^-表达式

4 .

先验误差协方差公式：

A为状态矩阵， Q为观测噪声，P_k-1为上一过程误差协方差。

计算过程如图：

滤波循环过程

基本循环过程

$\hat{x}$_k^-）、后验估计公式（$\hat{x}$_k）、卡尔曼增益系数公式（K_k）与先验误差协方差公式(P^-)即可推出一个循环过程。

完善循环过程

  在循环过程中，可以发现，在赋予初始估计，循环完第一遍后。除先验误差协方差公式 (P^-)外，每个公式中所没有提供的变量，都由其它公式的结果提供。只有先验误差协方差公式 (P^-)中上一过程误差协方差 P_k-1没有提供，所以要在一个过程最后加上该过程误差协方差 P_k的公式（进入下个过程时就变成了 上一过程的误差协方差P_k-1）。
5 .
误差协方差公式：

计算过程如图：

最后得到完整的循环过程