零阶多项式的卡尔曼滤波器

[首发：cnblogs    作者：byeyear    Email：byeyear@hotmail.com]

本文讲述的是一个最简单的卡尔曼滤波器实现。

先插个题外话：卡尔曼滤波器中的一些术语来自于自动控制行业，对于搞电子的小伙伴们有些名词可能比较陌生。比如“过程”这个词，一般情况下，你可以简单将其等效为电子信息专业里的术语“系统”（拒绝抬杠）。对，就是“信号与系统”这门课里的“系统”。

OK，let's go。

想象你捡到一台外星科技直流信号源，这个信号源的直流输出非常纯净没有任何波动（至少其波动远小于你所用的测量仪器能察觉到的量级），以机械按钮操作，但按钮上的字已经完全磨掉了，所以当你按下按钮时，你不知道这个信号源的输出到底是多少。你随便按了几个键，然后接了一个万用表想要测量此时这个信号源的输出，但捉襟见肘的你买不起agilent的八位半万用表，而你手头的手持式万用表的观测噪声分布为$N(0, \sigma^2)$。说人话就是，万用表示值的标准差为$\sigma$。

让我们将这个信号源看做一个过程，因为这个信号源是用外星科技制造的，它的输出噪声为0，所以过程噪声方差$Q_k$也是0。将万用表的示值作为观测值，那么我们有：

$x=a_0$    （信号源输出$x$为定值$a_0$）

$y=x$        （理想万用表的示值等于信号源输出）

（插一句，我在这里没有用黑体$\mathbf{x}$，因为这是一个零阶系统，卡尔曼滤波器方程中的所有矩阵在这个例子里都是标量。）

为了描述系统状态的变化，我们装模作样地算一下系统状态$x$的一阶导数：

$\frac{dx}{dt}=0$

嗯，当信号源输出恒定时，系统状态木有变化。

按状态空间的标准记法：

$\frac{dx}{dt}=Fx$

于是：

$F=0$

由此可以得到状态转换矩阵：

$\Phi=1$

对于外星科技恒定输出直流源，系统状态确实不会发生变化。嗯，这很科学。

我们的万用表直接接信号源输出，因此系统状态==信号源输出==理想万用表示值==观测值。注意红字，因此，从状态空间到观测空间的观测模型为：

$H=1$

因为我们不知道信号源的输出是多少，所以我们设定如下的初始条件：

$\hat{x}_0=0$         （初值为0）

$P_0=\infty$           （初始误差无穷）

现在我们可以开始卡尔曼滤波循环：

啊，且慢，让我先叨叨两句。对于卡尔曼滤波娘这种高大上的神兽而言，研究她和使用她完全是两回事。

研究她，就是研究她的理论基础，比如我前两篇文章里列出的那些云遮雾罩让我推导到吐的公式；

使用她，就是将书上的公式和算法流程抄在纸上，然后将数据按流程代入公式，但完全不必管为啥是这个公式以及为啥是那个流程。只要这张纸在，你大可以恬不知耻地说，我也会卡尔曼滤波器了。事实上，不单卡尔曼滤波，其他数字信号处理神兽也可以这么玩，比如小波娘。我作为神秘编号3xx354下的一名技术员，偶尔（常常？）也是这么干的，忽悠了不少不明真相的吃瓜群众。但我自己心里清楚，其实自己也就半瓶子水，啊不，一瓶~盖~水晃悠。比如这篇文章吧，虽说是个卡尔曼滤波器最小实现，但你要是把我的小抄纸收走了，我还真写不出来。

好，回到正题。卡尔曼滤波分预测和更新两个过程。预测，就是根据前值预测现值；更新，也可以理解为修正，就是用观测值去修正预测值。

预测1：

$\hat{x}_1^-=\Phi \hat{x}_0$   （公式抄纸上）

$=0$   （代入公式）

$P_1^-=\Phi P_0 \Phi^T$  （公式抄纸上）

$=1*\infty*1=\infty$ （代入公式）

更新1：

$K_1=P_1^-H^T(HP_1^-H^T+R_1)^{-1}$ （公式抄纸上）

$=\frac{P_1^-}{P_1^-+\sigma_n^2}$  （代入公式）

$=\frac{\infty}{\infty+\sigma_n^2}=1$

$P_1=(I-K_1H)P_1^-$     （公式抄纸上）

$=\sigma_n^2$       （代入公式）

$\hat{x}_1=\hat{x}_1^-+K_1(z_1-x_1^-)=z_1$

从上面的式子可以看到，不管我们如何选取初始估计$x_0$，$x_1$总是等于第一个观测值。

按照上面的方法继续执行预测-更新过程（后续将省略一些计算过程）：

预测2：

$\hat{x_2}^-$

$=\Phi \hat{x}_1=\hat{x}_1$

$P_2^-=\sigma_n^2$

更新2：

$K_2=0.5$

$P_2=\frac{\sigma_n^2}{2}$

$\hat{x}_2$

$=\hat{x}_2^-+K_2(z_2-x_2^-)$

$=\frac{z_1+z_2}{2}$

嗯，这好像是一个递推平均……事实上如果我们继续以上卡尔曼滤波过程（预测3，更新3；预测4，更新4；etc），就可以确认这确实就是一个递推平均。

P.S. 在进行上面的卡尔曼滤波循环时，$P$和$K$的递推计算是用不上状态变量$x$的。