卡尔曼滤波递归算法

卡尔曼滤波新奇的地方在于其递归算法，它是以最小均方差为准则的最佳线性估计，核心在于采用系统的理论动态模型进行状态值预估，并利用量测信息进行校正。

上一篇的介绍中我们已经知道，标准卡尔曼滤波的适用条件是线性系统，再次描述一下：

X(k)=A X(k-1)+B U(k)+W(k)

Z(k)=H X(k)+V(k)

从离散控制过程系统的角度看，以上两式中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量；A和B是系统参数，对于多模型系统，它们为矩阵。Z(k)是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵；W(k)和V(k)分别表示过程和测量的噪声，假设为高斯白噪声(White Gaussian Noise)，它们的covariance （协方差）常用Q、R表示（为简单常假设其不随系统状态变化而变化）。

对于满足上面条件(线性随机微分系统，过程和测量都是高斯白噪声)的问题，卡尔曼滤波器是最优的信息处理器，下面用它们结合其covariances 来估算系统的最优输出。

首先要利用系统的过程模型，来预测系统的下一个状态，假设现在的系统状态是k，根据系统的模型，可知现在的状态为：

1X(kk-1)=A X(k-1k-1)+B U(k)

式1中，X(kk-1)是利用上一状态预测的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。系统结果已经更新了，可是对应于X(kk-1)的协方差还没更新，用P表示协方差：

2P(kk-1)=A P(k-1k-1) A’+Q

式2中，P(kk-1)是X(kk-1)对应的协方差，P(k-1k-1)是X(k-1k-1)对应的协方差，A’表示A的转置矩阵，Q是系统过程的协方差。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

至此有了「现在状态」的预测结果，然后需要再收集此刻的测量值，结合预测值和测量值，就可以得到现在状态(k)的最优化估算值X(kk)：

3X(kk)= X(kk-1)+Kg(k) (Z(k)-H X(kk-1))

其中Kg为卡尔曼增益(Kalman Gain)：

4Kg(k)= P(kk-1) H’/(H P(kk-1) H’ + R)

到现在为止，已经得到了k状态下最优的估算值X(kk)。但是为了要使卡尔曼滤波器不断的运行下去直到系统过程结束，还要更新k状态下X(kk)的协方差：

5P(kk)=(I-Kg(k) H) P(kk-1)

其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(kk)就是式子2的P(k-1k-1)，这样，算法就可以自回归的运算下去。根据这5个公式，可以很容易的实现计算机的程序。

油位监控项目回顾

之前的油位监控项目中，我假设油位是线性减少的，每次加完油是一次新的周期（加油很容易判断，油位有明显的上升），利用加油后油位传感器的前500个数据点进行线性回归，得到「系统的状态方程模型」，而油位传感器本身的油位数据构成「系统的量测方程」。假设线性回归方程和量测方程中噪声成分为白噪声且符号高斯分布，白噪声指在整个频率上都有相同强度频率特性的噪声，实际应用中将频率设为常值、带宽大大超过系统带宽的噪声称为「白噪声」，白噪声意味着噪声值和时间不相关，用高斯白噪声来模拟，可以大大简化模型。

经线性回归得到系统的状态方程（假如是Y）后，对每一个油位上报时刻，有两个关于油位的油位数据：根据Y的预测值和油位传感器的测量值，现在要用这两个值结合它们各自的噪声来估算出油位的真实值。

假如我要估算k时刻的实际油位值。首先要根据k-1时刻的油位值，来预测k时刻的油位值，因为相信油位是方程Y线性下降的，所以k时刻的预测值是Y(k)，假设是0.9，同时该值的高斯噪声的偏差是0.05（0.05是这样得到的：如果k-1时刻估算出的油位的偏差是0.03，我对自己预测的不确定度是0.04度，它们的平方相加再开方，就是0.05）。然后，我从油位传感器那里得到了k时刻的油位，假设是0.92，同时该值的偏差是0.04。

现在k时刻有两个油位值，分别是0.9和0.92。究竟实际油位是多少呢？相信线性回归函数的预测还是相信油位传感器呢？究竟相信谁多一点，可以用它们的协方差来判断。因为Kg^2=0.05^2/(0.05^2+0.04^2)，所以Kg=0.61，这样估算出k时刻的实际油位值是：0.9+0.61*(0.92-0.9)=0.912，可以看出，因为油位传感器的协方差比较小（比较相信油位传感器），所以估算出的最优油位值偏向传感器的值。

现在已经得到k时刻的最优油位值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么递归的东西出现。在进入k+1时刻之前，还要算出k时刻那个最优值（0.912）的偏差，算法如下：((1-Kg)*5^2)^0.5=0.031。这里的0.05就是上面的k时刻预测的那个0.9油位值的偏差，得出的0.031就是进入k+1时刻以后k时刻估算出的最优油位值的偏差（对应于上面的0.03）。

就是这样，卡尔曼滤波器就不断的把协方差递归，从而估算出最优的油位值。它运行的很快，而且它只保留了上一时刻的协方差。