卡尔曼滤波

卡尔曼滤波的介绍与使用

一开始理解卡尔曼滤波花了不少时间，一旦了解了以后就没有开始时那种困难的感觉了。基本上下面这张图就概括了卡尔曼滤波。

可见就是我们可以具体知道的信息。其中u是我们预先知道的东西，Z是通过传感器等得到的观测值。

模型

模型是反映现实情况的，图中的箭头方向是事情的因果顺序。（我当时就卡在混淆了模型和计算的区别）

x表示系统的真实状态，它是不可知的。比如说室内的温度，其实我们没有办法知道确切的室内温度，我们总是通过感觉或者传感器（温度计）来获取温度值。而Z就是那个我们能够得到的温度值。这个模型很好地反映了现实中的情况。在外部因素u的作用下x发生了改变，而我们通过方法H去测量x得到z。然而现实中总是不可避免地发生一些不确定因素，所以引入误差w与v。
需要注意的是卡尔曼滤波是一个线性的模型，并不是所以场景下都能使用。所引入的误差w与v服从正态分布。

计算

接下来我们要根据模型中的内容，来得到我们想要的东西。也就是已知u与z，求x。

1. 预测
   $${\hat {{\textbf {x}}}}_{{k|k-1}}={\textbf {F}}_{{k}}{\hat {{\textbf {x}}}}_{{k-1|k-1}}+{\textbf {B}}_{{k}}{\textbf {u}}_{{k}}(预测状态)$$

   $${\displaystyle {\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}} {\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}（预测估计协方差矩阵）$$

2. 更新
   首先要算出以下三个量：

   $${\displaystyle {\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1}} {\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-{\textbf {H}}_{{k}}{\hat {{\textbf {x}}}}_{{k|k-1}}(测量余量，measurement residual)$$

   $${\displaystyle {\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}} {\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}(测量余量协方差)$$

   $${\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}} {\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}（最优卡尔曼增益）$$
   然后用它们来更新滤波器变量x与P：

   $${\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}} {\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}(更新的状态估计)$$

   $${\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}} {\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}（更新的协方差估计）$$

   其中\({x}_{k|k}\) 就是我们希望获取的系统真是值。\({P}_{k|k}\) 是需要不断迭代的预测估计。
   需要注意的是\({Q}_{k}\),\({R}_{k}\) 并不能从前面建立的模型中取得。它们是协方差矩阵，代表了模型中的误差w与v的可行度。也就是说值越大就越不可信。因此在实际使用中需要对\({Q}_{k}\),\({R}_{k}\)进行测量估计，调参来使得提升滤波效果。它们间的取值反映了当我有传感器数据，有模型预测数据的时候，如何对它们进行权衡。

实例

下面我将通过软件模拟数据来解释如何使用卡尔曼滤波。
假设我们在烧开水，其真实情况是温度关于时间的线性函数 \(x=1.3 t\).我们的温度计存在均值为0，标准差为5

在这个简单模型下F、H均为1. R=25,Q=1

1. 预测

   X=X+1.3;
   P=P+Q;

2. 更新

   S=P+R;
   K=P/(S);
   X=X+K(Z-X);
   P=(1-K)P;

进阶

如果只能处理精准模型下的预测，那功能就太局限了。现在把前面温度计的例子做一点修改。\(x=K t\) 其中K是一个未知数。接下来不仅要对数据进行滤波，还要确定参数K！
新的模型为X=X+K
$$F=\left(\begin{array}{cc} 1 & 1 \\ 0 & 1 \\\end{array}\right)$$
$$H=\left(\begin{array}{cc} 1 & 0 \\\end{array}\right)$$

协方差矩阵Q,R：
$$Q=\left(\begin{array}{cc} 0 & 0. \\ 0. & 0.0001 \\\end{array}\right);R=25;$$

代入数据：

fun[{X_, P_, i_}] := Module[{X1 = F .X,
   P1 = F . P.Transpose[F] + Q},
  y = z[i] - H . X1;
  S = H .P1. Transpose[H] + R;
  K = P1.Transpose[H].Inverse[S];
  {X1 + K . y, N[(IdentityMatrix[2] - K.H).P1], i + 1}]

代入不正确的初值(0,0.6)依然得到比较接近的结果。

如果调小Q为0 那么最后会稳定到原函数。

后续

1. 协方差矩阵R,Q对平衡观测数据与预测结果有至关重要的影响。R,Q的值如何确定？这个我暂时没能力解释。
2. 卡尔曼滤波只能处理线性模型。对于非线性模型可以使用扩展卡尔曼滤波。