5.卡尔曼滤波之实例分析

实例一、线性定常系统

       线性定常系统，就是上一篇文章中的${\Phi }_{k,k-1}$是一个常值。为了更简单些，观测也用直接观测好了。于是这个定常系统可以这样表示：
$$X_k=\Phi X_{k-1}+W_{k-1}$$$$Z_k=X_k+V_k$$
       这里的$\Phi$是常数了，观测Z和状态X都是标量，系统噪声$W_k$和量测噪声$V_k$都是零均值的白噪声，方差矩阵用Q和R表示，Q和R也是常量。现在，我们对这个系统用卡尔曼滤波试试。
直接套公式好了。
$${\hat{X}}_{k/k-1}=\Phi {\hat{X}}_{k-1}$$$${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-{\hat{X}}_{k/k-1})=(1-K_k){\hat{X}}_{k/k-1}+K_k{Z}_k$$$$K_k=P_{k/k-1}(P_{k/k-1}+R{)}^{-1}=\frac{P_{k/k-1}}{P_{k/k-1}+R}$$$$P_{k/k-1}={\Phi }^2{P}_{k-1}+Q$$$$P_k=(1-K_k)P_{k/k-1}=(1-\frac{P_{k/k-1}}{P_{k/k-1}+R})P_{k/k-1}=\frac{R{P}_{k/k-1}}{P_{k/k-1}+R}=R{K}_k$$
       这不就是把卡尔曼滤波的五个方程拿过来套了一下吗？套公式简单，我们希望通过这个简单的例子来看看卡尔曼滤波里到底发生了什么事。
       先看第二个方程，
$${\hat{X}}_k=(1-K_k){\hat{X}}_{k/k-1}+K_k{Z}_k$$       我们发现估值${\hat{X}}_k$由两部分组成，分别是这一步的预测和这一步的观测。而这两部分的系数就好玩了，如果一个大，那另一个就小。
       先看预测部分的系数小，观测部分的系数大的情况。既然观测部分的系数大，那就是说$K_k$大，在看上面第三个方程，$$K_k=\frac{P_{k/k-1}}{P_{k/k-1}+R}$$       $K_k$大，就说明R小，而根据R的定义，R越小说明量测$Z_k$越准。所以第二个方程观测部分的系数更大。
       如果预测部分的系数大，观测部分的系数小呢？情况正好倒过来，预测部分的系数大，说明$$1-K_k=\frac{R}{P_{k/k-1}+R}$$       要大，那么就需要$P_{k/k-1}$小，而$P_{k/k-1}$的定义就是表征预测的精度的，它小说明预测的准，所以预测部分的系数要取的大一些。
       从这个例子也可以看出，卡尔曼滤波是能够识别出可用信息的质量，并自动确定这些信息的利用程度的，所以可以认为，卡尔曼滤波是具有一些智能自适应功能的算法。
       代进去一些具体数算一下试试看。
       假如$\Phi =Q=R=P_0=1$。
       第一步递推得到$$P_{1/0}=P_0+Q=2$$$$K_1=P_{1/0}(P_{1/0}+R{)}^{-1}=0.67$$$$P_1=K_1$$
       依次递推，会得到：
$$P_2=K_2=0.625$$$$P_3=K_3=0.62$$$$P_k=K_k=\frac{P_{k-1}+1}{P_{k-1}+2}$$
       会发现，随着递推次数的增加，$K_k$和$P_k$会逐渐减小，相应的$P_{k/k-1}$也会减小，虽然每次递推，大家减小的绝对量是一样的，但是由于$P_{k/k-1}$总比$K_k$大，所以$K_k$减小的相对量要比$P_{k/k-1}$大。这说明，在递推的开始阶段，估值依靠测量要多一些，随着递推次数的增加，预测的比重开始增加。最终两者达到平衡。这就说明滤波达到稳态了。

实例二、$\alpha -\beta -\gamma 滤波$

       说实话，我第一次仿真这个例子时真的感觉到这是在滤波了。它讲了这么一个情况。
       一个物体沿直线运动，可以想象，这个物体在每一个时刻都可以用位置s(t)，速度v(t)和加速度a(t)来表示它的状态。但在观测端，只能观测到位置信息$Z_k=s_k+V_k$显然，观测是由误差的。定义观测的方差$E[V_k^2]=r$，要求对这个物体的运动状态进行卡尔曼滤波。
       开始分析这个问题吧。其实就是已知每一时刻的位置$s_k$，求每一时刻的速度$v_k$和加速度$a_k$。
还有一个量，叫加加速度，咱们用$j_k$表示。但是，这个加加速度在这个系统，尤其是离散后的系统基本跟白噪声没什么区别了。信息理论中对白噪声建模方法里，就是把一个类似有色噪声的过程不断的求导，就会得到白噪声。
       加加速度相当于位置进行了三次求导，在离散领域就是做了三次差分，可以认为是白噪声了。于是系统的噪声也有了。我们定义系统的噪声方差为$E[j(t{)}^2]=q$离散后$E[j_k]=q/T$。
       虽然我们可以用上一篇文章里讲的连续系统离散化方法建立状态方程，但对于这么简单的系统，离散方程是可以直接写出来的。数据周期定义为T，那么$$s_{k+1}=s_k+v_{k}T+0.5a_k{T}^2$$$$v_{k+1}=v_k+a_{k}T$$$$a_{k+1}=a_k+j_k$$
       好啦，状态方程直接出来了。
$$X_k=\left(\begin{array}{c}{s}_k\\ v_k\\ a_k\end{array}\right),\Phi =\left(\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right),\Gamma =\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right),Q=\frac{q}{T}$$
       量测矩阵就是$$H=\left(\begin{array}{ccc}1& 0& 0\end{array}\right)$$
       直接套卡尔曼滤波的一般方程吧。大家可以自己编个程序仿真一下，比如数据周期1毫秒，如果不用卡尔曼滤波，直接把观测的位置信息求导来计算速度和加速度，即使观测误差达到微米的数量级，两次微分得到的加速度也是看不得的，噪声大得不忍直视。而如果使用卡尔曼滤波，你会发现，不管是速度，还是加速度的曲线都会变得非常的光滑平稳。
       为什么叫$\alpha -\beta -\gamma$滤波？它是这个意思：当滤波达到稳态时，$P_k$趋于稳定，稳定后是一个固定的矩阵$P$。而根据卡尔曼滤波器的增益矩阵计算的公式，$K_k$也会稳定到一个值$$K=P{H}^T{r}^{-1}$$       把这个式子乘开$$K=\left(\begin{array}{c}\frac{P_{11}}{r}\\ \frac{P_{21}}{r}\\ \frac{P_{31}}{r}\end{array}\right)$$
       再用$\alpha ，\beta ，\gamma$来定义K的三个元素
$$K=\left(\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right)$$
       于是$${\hat{X}}_k=\Phi {\hat{X}}_{k-1}+\left(\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right)(Z_k-H\Phi {\hat{X}}_{k-1})$$
       这就是$\alpha -\beta -\gamma$滤波。$\alpha ，\beta ，\gamma$分别代表位置，速度和加速度的稳态滤波增益。如果不估计加速度，只估计位置和速度，那就是$\alpha -\beta$滤波。