7.卡尔曼滤波之发散的抑制方法

       几乎任何递推和迭代算法都有发散的可能，卡尔曼滤波也不例外。造成卡尔曼滤波发散的原因无外乎以下两点：
       状态方程描述的动力学模型不准确，或者噪声的统计模型不准确，这样会使模型和量测值不匹配，导致发散。其实模型不准确的情况很少，除非原理上错误。更多的是由于模型太过粗糙而导致的。
       递推过程计算机摄入误差累积，使得方差阵失去正定性或者失去对称性，导致增益计算失去加权效果，从而导致滤波器发散。
       先说模型不准确导致的发散处理技术，累积误差导致发散的处理后面有机会再提。
       首先还是要先理解一下，卡尔曼滤波器的本质是线性最小方差估计，它虽然是递推形式的，可是每一步递推获得的估计值其实是根据当前时刻之前的所有量测值而获得的最优估计结果。也就是说它实际是有记忆的。
       而量测值其实是完全不会发散的东西，因为它会始终在它的数学期望周围分散，我们的思路就是，在滤波器中，我们应该更加相信新鲜的量测值，而减小对陈旧量测值的信任程度，也就是增大新鲜量测值的权重，减少陈旧量测值的权重。根据这一思路，我们可以重新设计卡尔曼滤波器。其设计思路有两种，分别是衰减记忆法和限定记忆法

一、衰减记忆法

       再来看看量测方程：$$Z_k=H_k{X}_k+V_k$$       根据上面的思路，我们认为，当前时刻$Z_N$是最准的，而之前的时刻的$Z_k(k<N)$要稍微不准一些，离N越远，说明量测的数据陈旧，量测值也越不准。
       不准是什么概念，就是那个$R_k$大，也就是说，我们希望离当前时刻越远的$R_k$最好越大。这好办，把$R_k$乘个系数好了$$R_N=R_k{s}^{N-k}$$       其中s>1，这样不就实现，在当前时刻R最小，之前的时刻R都要大一些。
       基于这个思路，重新对卡尔曼滤波器进行推导，就会得出下面的滤波方程（推导过程不想写了！）：
$$P_{k/k-1}^{\ast }={\Phi }_{k,k-1}(s{P}_{k-1}^{\ast }){\Phi }_{k,k-1}^T+Q_{k-1}$$
$$K_k^{\ast }=P_{k/k-1}^{\ast }{H}_k^T[H_k{P}_{k/k-1}^{\ast }{H}_k^T+R_k{]}^{-1}$$
$$P_k^{\ast }=(I-K_k^{\ast }{H}_k)P_{k/k-1}^{\ast }$$
$${\hat{X}}_k^{\ast }={\hat{X}}_{k-1}^{\ast }+K_k^{\ast }[Z_k-H_k{\Phi }_{k,k-1}{\hat{X}}_{k-1}^{\ast }]$$
       对比这套滤波方程跟卡尔曼滤波器的基本方程，其实只有第一个方程里多了个s。由于s>1，所以这样计算出来的$P_{k/k-1}^{\ast }$总比根据一般方程计算出来的$P_{k/k-1}$要大，对应这$K_k^{\ast }$也比基本方程里的$K_k$大。根据前面实例分析那篇文章里的分析结论，采用这套方程对新的量测值的利用权重要比基本方程里的权重大。也就是量测的权重增加而预测的权重减小，这样就达到了降低陈旧量测值对估计结果的影响了。而且越陈旧的量测，影响越小，所以这种方法叫做衰减记忆法

二、限定记忆法

       前面提到的衰减记忆法思路是随着滤波次数的增加，陈旧量测的权重会逐渐减小，从而达到增加新量测的权重的效果。而限定记忆法则更直接，它的思路是：我只对当前时刻之前的N次量测感兴趣，之前的量测不要考虑。这话怎么说？
       咱们不是说用卡尔曼滤波的基本方程时，${\hat{X}}_k$是由$Z_1,Z_2,\dots ,Z_k$这些量测获得的估计值吗。而现在，我们要求的是由$Z_d,Z_{d+1},\dots ,Z_k$这些量测来获得对X的估计，量测数量固定有N+1个（当前时刻之前的N个量测加上当前时刻的量测）。为了区分，把这个估计值定义成${\hat{X}}_k^N$吧。
       滤波方程推导有些复杂冗长，这里为了简洁，假定系统能够为0，也就是Q=0。可即使这样，得到的滤波方程也不简单：
$${\hat{X}}_k^N={\Phi }_{k,k-1}{\hat{X}}_{k-1}^N+K_k[Z_k-H_k{\Phi }_{k,k-1}{\hat{X}}_{k-1}^N]-{\overline{K}}_k[Z_d-H_d{\Phi }_{d,k}{\Phi }_{k,k-1}{\hat{X}}_{k-1}^N]$$$$K_k=P_k^N{H}_k^T{R}_K^{-1}$$$${\overline{K}}_k=P_k^N{\Phi }_{d,k}^T{H}_d^T{R}_d^{-1}$$$$(P_k^N{)}^{-1}={\Phi }_{k-1,k}^T(P_{k-1}^N{)}^{-1}{\Phi }_{k-1,k}+H_k^T{R}_k^{-1}{H}_k-{\Phi }_{d,k}^T{H}_d^T{R}_d^{-1}{H}_d{\Phi }_{d,k}$$
       这些式子的参数有些是需要说明的。
       首先，这是在$k>N$时才能使用的。道理也好理解，N是预计要限定记忆的长度，当k比N小的时候就直接用一般方程好了。
       其次，这里有好几个$\Phi$，也就是好几个状态转移矩阵，要注意状态转移矩阵的定义哦，${\Phi }_{k,k-1}$表示的是从k-1时刻到k时刻的状态转移矩阵，我们可从没有说过转移矩阵只能从前向后转移，从后向前也一样的。
       所以，${\Phi }_{k-1,k}$是从k时刻到k-1时刻的转移矩阵。而${\Phi }_{d,k}$则表示k时刻到k之前的N个周期的d时刻的转移矩阵。这些倒着转移的矩阵其实可以认为是正着转移矩阵的逆矩阵。
       还剩一个问题，就是这套滤波器的初值问题。一定要注意，这套算法并不是从k=0开始的，而是从k>N时才开始的。所以它的初值就是在N时刻的基本方程计算结果。而且为了体现限定记忆的精神，在k=N的瞬间，还要把初值的影响的记忆也要忘掉。所以一般按下面这种方法取值。
$$P_N^N=[P_N^{-1}-{\Phi }_{0,N}^T{P}_0^{-1}{\Phi }_{0,N}{]}^{-1}$$$${\hat{X}}_N^N=P_N^N[P_N^{-1}{\hat{X}}_N-{\Phi }_{0,N}^T{P}_0^{-1}{\hat{X}}_0]$$
       这里的$P_N$和${\hat{X}}_N$就是在前N个周期内用基本方程滤波的结果。

小结

       这样一来，我们基本把模型不够准确时的发撒抑制方法解释清楚了，当然，这也只是通常的做法。而且，更通常的情况是，这两种方法都不会轻易用，一是滤波的精度会下降，再有就是会增加计算量。尤其是限定记忆法，如果考虑状态噪声的影响，计算公式就会变得非常繁琐。所以最合理的方案还是尽量把系统数学模型建立的准确，让建立的状态方程能够尽可能的反应真实的状态才是正解。