卡尔玛滤波的原理说明

卡尔曼滤波的原理说明

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，它是最优、效率最高甚至是最有用的。

卡尔曼滤波的介绍

这里先根据下面的例子对卡尔曼滤波的5条公式进行一步一步地探索。
假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分配。另外，我们在房间里放一个温度计，但这个温度计也是不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
好了，现在对于谋一分钟我们有两个关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面要用这两个值结合它们各自的噪声来估算出房间的实际温度值。
假如我们要估算$k$时刻的实际温度值。首先你要根据$k$-1时刻的温度值，来预测$k$时刻的温度。因为你相信温度是恒定的，所以你会得到$k$时刻的温度预测值是跟$k$-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果$k$-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4，它们的平方相加再开方，就是5）。然后，你从温度计那里得到了$k$时刻的温度值，假设是25度，同时该值的偏差是4度。
由于我们用于估算$k$时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计？究竟相信谁多一点，我们可以用它们的covariance来判断。因为$K{g}^2=5^2/(5^2+4^2)$，所以$Kg$=0.78，我们可以估算出$k$时刻的实际温度值是：$23+0.78\ast (25-23)=24.56$度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
现在我们可以得到$k$时刻的最优温度值了，下一步就是要进入$k$+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入$k$+1时刻之前，我们还要算出$k$时刻那个最优值（24.56度）的偏差。算法如下：$((1-Kg)\ast 5^2{)}^{0.5}=2.35$。这里的5 就是上面的$k$时刻你预测的那个23度温度值的偏差，得出的2.35就是进入$k$+1时刻以后$k$时刻估算出的最优值的偏差（对应于上面的3）。
就是这样，卡尔曼滤波就不断地把covariance地柜，从而估算出最优的温度值。它运行的很快，而且它只保留了上一时刻的covariance。上面的$Kg$，就是卡尔曼增益。它可以随不同的时刻改变它自己的值。

卡尔曼滤波算法

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：
$$X(k)=AX(k-1)+BU(k)+W(k)$$
再加上系统的测量值：
$$Z(k)=HX(k)+V(k)$$
上两式中，$X(k)$是$k$时刻的系统状态，$U(k)$是$k$时刻对系统的控制量。$A$和$B$是系统参数，对于多模型系统，它们为矩阵。$Z(k)$是$k$时刻的测量值，$H$是测量系统的参数，对于多测量系统，$H$为矩阵。$W(k)$和$V(k)$分别表示过程和测量的噪声。它们被假设成高斯白噪声，它们的covariance分别是$Q$，$R$（这里我们假设它们不随系统状态变化而变化）。
对于满足上面的条件（线性随机微分系统，过程和测量都是高斯白噪声），卡尔曼滤波是最优的信息处理器。下面我们来用它们的covariances来估算系统的最优化输出。
首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是$k$，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
$$X(k|k-1)=AX(k-1|k-1)+BU(k)\text{\hspace{1em}(1)}$$
上式中，$X(k|k-1)$是利用上一状态预测的结果，$X(k-1|k-1)$是上一状态最优的结果，$U(k)$为现在状态的控制量，如果没有控制量，它可以为０．
到现在为止我们的系统结果已经更新了，可是，对应于$X(k|k-1)$的covariance还没更新。我们用$P$表示covariance：
$$P(k|k-1)=AP(k-1|k-1)A^{\prime }+Q\text{\hspace{1em}(2)}$$
上式中，$P(k|k-1)$是$X(k|k-1)$对应的covariance，$P(k-1|k-1)$是$X(k-1|k-1)$对应covariance，$A^{\prime }$表示$A$的转置矩阵，$Q$是系统过程的covariance。式子1，2就是卡尔曼滤波５个公式中的前两个，也就是对系统的预测。
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态$(k)$和最优化估算值$X(k|k)$：
$$X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))\text{\hspace{1em}(3)}$$
其中$Kg$为卡尔曼增益：
$$Kg(k)=P(k|k-1)H^{\prime }/(HP(k|k-1)H^{\prime }+R)\text{\hspace{1em}(4)}$$
到现在为止，我们已经得到了$k$状态下最优的估算值$X(k|k)$。但是为了要卡尔曼滤波器不断地运行下去知道系统过程结束，我们还要更新$k$状态下$X(k|k)$的covariance：
$$P(k|k)=(I-Kg(k)H)P(k|k-1)\text{\hspace{1em}(5)}$$
其中$I$为单位矩阵，对于单模型单测量，$I=1$。当系统进入$k$+1状态时，$P(k|k)$就是式２的$P(k-1|k-1)$。这样，算法就可以自回归的运算下去.
卡尔曼滤波器的原理基本描述了，式子１，２，３，４和５就是它的５个基本公式。根据这５个公式，可以很容易地实现计算机的程序。