卡尔曼滤波的介绍与使用
一开始理解卡尔曼滤波花了不少时间，一旦了解了以后就没有开始时那种困难的感觉了。基本上下面这张图就概括了卡尔曼滤波。
可见就是我们可以具体知道的信息。其中u是我们预先知道的东西，Z是通过传感器等得到的观测值。
模型
模型是反映现实情况的，图中的箭头方向是事情的因果顺序。（我当时就卡在混淆了模型和计算的区别）
x表示系统的真实状态，它是不可知的。比如说室内的温度，其实我们没有办法知道确切的室内温度，我们总是通过感觉或者传感器（温度计）来获取温度值。而Z就是那个我们能够得到的温度值。这个模型很好地反映了现实中的情况。在外部因素u的作用下x发生了改变，而我们通过方法H去测量x得到z。然而现实中总是不可避免地发生一些不确定因素，所以引入误差w与v。
需要注意的是卡尔曼滤波是一个线性的模型，并不是所以场景下都能使用。所引入的误差w与v服从正态分布。计算
接下来我们要根据模型中的内容，来得到我们想要的东西。也就是已知u与z，求x。
- 预测$${\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}}（预测估计协方差矩阵）$$ - 更新首先要算出以下三个量：
$${\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
-
预测
X=X+1.3;P=P+Q;
-
更新
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 那么最后会稳定到原函数。
后续
- 协方差矩阵R,Q对平衡观测数据与预测结果有至关重要的影响。R,Q的值如何确定？这个我暂时没能力解释。
- 卡尔曼滤波只能处理线性模型。对于非线性模型可以使用扩展卡尔曼滤波。