9.卡尔曼滤波之次优滤波器——降阶处理

1、降阶滤波的推导

思路

       所谓卡尔曼滤波的降阶处理，就是想办法把状态量的数量少，当然这是有条件的。减少就意味着要忽略一些量，但那些量可以忽略是值得研究的。最直观的想法是忽略我们不关心的量，而且忽略后还得对滤波不会造成太大的影响。什么量可以忽略呢？当然是噪声。
       还记得前面提到的有色噪声处理问题吧。当状态方程的噪声有色噪声时，我们把有色噪声当成状态量，放到状态方程里一起估计。也就是我们把状态方程和量测方程进行了扩展，扩展成这个样子：
$$\left(\begin{array}{c}{X}_{k+1}\\ W_{k+1}\end{array}\right)=\left(\begin{array}{cc}{\Phi }_{k+1,k}& {\Gamma }_k\\ 0& {\Pi }_{k+1,k}\end{array}\right)\left(\begin{array}{c}{X}_k\\ W_k\end{array}\right)+\left(\begin{array}{c}0\\ I\end{array}\right){\zeta }_k$$
$$Z_k=\left(\begin{array}{cc}{H}_k& 0\end{array}\right)\left(\begin{array}{c}{X}_k\\ W_k\end{array}\right)+V_k$$
       我们其实对有色噪声里的有色的部分并不感兴趣，也就是说$W_k$是多少我们并不想知道。但是滤波的时候是把$W_k$一起进行最优估计的。我这里就讨论一种在一定约束条件下，忽略$W_k$来实现对滤波器进行降阶的处理思路。
       上面两个方程我们把参数名换一下，以便描述更直接一些。方程写成下面这个样子：
$$\left(\begin{array}{c}{X}_{k+1}^1\\ X_{k+1}^2\end{array}\right)=\left(\begin{array}{cc}{\Phi }_{k+1,k}^{11}& {\Phi }_{k+1,k}^{12}\\ 0& {\Phi }_{k+1,k}^{22}\end{array}\right)\left(\begin{array}{c}{X}_k^1\\ X_k^2\end{array}\right)+\left(\begin{array}{cc}0& 0\\ 0& {\Gamma }_k^2\end{array}\right)\left(\begin{array}{c}0\\ W_k^2\end{array}\right)$$
$$Z_k=\left(\begin{array}{cc}{H}_k^1& 0\end{array}\right)\left(\begin{array}{c}{X}_k^1\\ X_k^2\end{array}\right)+V_k$$
       注意哦！上标的1和2可不是1次方和平方，而是单纯的编号，都写下标太长了。
       其实上面两个方程和状态方程是有色噪声的扩展方程是一回事。我只是把变量名给统一了一下。

推导过程

       按刚才咱们提到的思路，我们滤波是不考虑或者尽量少考虑$X_k^2$的影响。也就是在计算$X_{k+1}^1$的估值${\hat{X}}_{k+1}^1$时不考虑$X_k^2$。那么根据卡尔曼滤波器的基本方程：
$${\hat{X}}_{k+1}^1={\Phi }_{k+1,k}^{11}{\hat{X}}_k^1+K_{k+1}(Z_{k+1}-H_{k+1}^1{\Phi }_{k+1,k}^{11}{\hat{X}}_k^1)$$
好了，我们来看一看这个估值的误差$X_{k+1}^1-{\hat{X}}_{k+1}^1$。把上面的那些关系都带进去，直接减就好了,适当化简一下
$${\tilde{X}}_{k+1}^1=X_{k+1}^1-{\hat{X}}_{k+1}^1$$
$${\tilde{X}}_{k+1}^1=(I-K_{k+1}{H}_{k+1}^1)({\Phi }_{k+1,k}^{11}{\tilde{X}}_k^1+{\Phi }_{k+1,k}^{12}{X}_k^2)-K_{k+1}{V}_{k+1}$$
       发现了吧。这个估值的误差跟$X_k^2$有关了，当然会这样了，因为$X_k^2$本身就是噪声的一部分。增益矩阵是啥来着，表征估计误差的协方差矩阵，所以，在计算滤波增益矩阵的时候，我们还是需要考虑$X_k^2$的影响的。
       好吧，看来那个估计方差阵$P_k$的方程要重写一下了。推导过程可能繁琐了写，但其实只是单纯的计算，直接写结果了：
$$P_{k+1}^1=(I-K_{k+1}{H}_{k+1}^T){\Sigma }_{k+1/k}(I-K_{k+1}{H}_{k+1}^1{)}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T$$
或者：
$$P_{k+1}^1=(I-K_{k+1}{H}_{k+1}){\Sigma }_{k+1/k}$$
       那个${\Sigma }_{k+1/k}$跟预测协方差阵$P_{k+1/k}$是一个性质。不过因为$X_k^2$的存在，所以它的方程要比$P_{k+1/k}$的方程复杂些。
$${\Sigma }_{k+1/k}={\Phi }_{k+1,k}^{11}{P}_k^1{\Phi }_{k+1,k}^{11T}+{\Phi }_{k+1,k}^{12}{C}_k{\Phi }_{k+1,k}^{11T}+{\Phi }_{k+1,k}^{11}{C}_k^T{\Phi }_{k+1,k}^{12T}+{\Phi }_{k+1,k}^{12}{A}_k{\Phi }_{k+1,k}^{12T}$$

       $A_k$是$X_k^2$的自相关矩阵，也就是$X_k^2{X}_k^{2T}$，它也有递推关系：
$$A_{k+1}={\Phi }_{k+1,k}^{22}{A}_k{\Phi }_{k+1,k}^{22T}+{\Gamma }_k^2{Q}_k^2{\Gamma }_k^{2T}$$
       $C_k$是${\tilde{X}}_k^1$和$X_k^2$的协方差矩阵，也就是${\tilde{X}}_k^1{X}_k^{2T}$，也有递推关系：
$$C_{k+1}^T=I_f{\Phi }_{k+1,k}^{11}{C}_k^T{\Phi }_{k+1,k}^{22T}+I_f{\Phi }_{k+1,k}^{12}{A}_k{\Phi }_{k+1,k}^{22T}$$
$$I_f=I-K_{k+1}{H}_{k+1}^1$$
       同样，增益矩阵$K_k$也跟着重写一下。$$K_{k+1}={\Sigma }_{k+1/k}{H}_{k+1}^T(H_{k+1}{\Sigma }_{k+1/k}{H}_{k+1}^T+R_{k+1}{)}^{-1}$$
       总结一下，降阶处理的次优滤波器全套方程如下：
$${\Sigma }_{k+1/k}={\Phi }_{k+1,k}^{11}{P}_k^1{\Phi }_{k+1,k}^{11T}+{\Phi }_{k+1,k}^{12}{C}_k{\Phi }_{k+1,k}^{11T}+{\Phi }_{k+1,k}^{11}{C}_k^T{\Phi }_{k+1,k}^{12T}+{\Phi }_{k+1,k}^{12}{A}_k{\Phi }_{k+1,k}^{12T}$$
$$K_{k+1}={\Sigma }_{k+1/k}{H}_{k+1}^T(H_{k+1}{\Sigma }_{k+1/k}{H}_{k+1}^T+R_{k+1}{)}^{-1}$$
$${\hat{X}}_{k+1}^1={\Phi }_{k+1,k}^{11}{\hat{X}}_k^1+K_{k+1}(Z_{k+1}-H_{k+1}^1{\Phi }_{k+1,k}^{11}{\hat{X}}_k^1)$$
$$P_{k+1}^1=(I-K_{k+1}{H}_{k+1}){\Sigma }_{k+1/k}$$
$$I_f=I-K_{k+1}{H}_{k+1}^1$$
$$C_{k+1}^T=I_f{\Phi }_{k+1,k}^{11}{C}_k^T{\Phi }_{k+1,k}^{22T}+I_f{\Phi }_{k+1,k}^{12}{A}_k{\Phi }_{k+1,k}^{22T}$$
$$A_{k+1}={\Phi }_{k+1,k}^{22}{A}_k{\Phi }_{k+1,k}^{22T}+{\Gamma }_k^2{Q}_k^2{\Gamma }_k^{2T}$$
       顺序计算递推即可。注意$H_k$和$H_k^1$的区别。
       再次强调一下，这个估计是次优估计，不是最优估计。这个推导过程难道不能保证估值最优吗。很遗憾，不能。从前面的文章中，我说过了，$K_k$时根据$P_k$最优而选择的，从而保证了卡尔曼滤波是最优估计。但在这套降阶处理的过程中，$K_k$的选择并不是根据$P_k$最优进行选择，而是根据$P_k^1$最优而进行的选择。但却不保证$C_k$最优。也就是说，选择$K_k$时虽然考虑了$X_k^2$的影响，但$X_k^2$的影响并没有完全消除，因此并不能保证$P_k^1$。所以这种滤波方法是一种次优滤波。不过话说回来，它毕竟在一定程度上考虑了一下$X_k^2$，所以在一般情况下，滤波的精度还是可以的。

实例分析

       还是举个例子说明一下把。有个系统，状态方程和量测方程长下面的样子：
$$\left(\begin{array}{c}{X}_{k+1}^1\\ X_{k+1}^2\end{array}\right)=\left(\begin{array}{cc}1& 1\\ 0& 0.5\end{array}\right)\left(\begin{array}{c}{X}_k^1\\ X_k^2\end{array}\right)+\left(\begin{array}{c}0\\ W_k^2\end{array}\right)$$
$$Z_k=\left(\begin{array}{cc}1& 0\end{array}\right)\left(\begin{array}{c}{X}_k^1\\ X_k^2\end{array}\right)+V_k$$
       $X_k^1$和$X_k^2$不相关，$W_k^2$和$V_k$的方差分别为：
$$Q_k^2=1$$
$$R_k=1$$
       而且：
$$E[X_0^1]=E[X_0^2]=0$$
E { ( X ~ 0 1 X 0 2 ) ( X ~ 0 1 X 0 2 ) } = ( 10 0 0 10 ) E\{\begin{pmatrix}\widetilde X_0^1\\X_0^2\end{pmatrix}\begin{pmatrix}\widetilde X_0^1&X_0^2\end{pmatrix}\}=\begin{pmatrix}10&0\\0&10\end{pmatrix} E{(X 01​X02​​)(X 01​​X02​​)}=(100​010​)
       初始条件都有了，咱们为了做对比，先用标准的卡尔曼滤波方程估一下。那么，就带公式,然后得到。
$${\hat{X}}_{k+1}^1={\hat{X}}_k^1+{\hat{X}}_k^2+K_{k+1}^1(Z_{k+1}-{\hat{X}}_k^1-{\hat{X}}_k^2)$$
$${\hat{X}}_{k+1}^2={\hat{X}}_k^2+K_{k+1}^2(Z_{k+1}-{\hat{X}}_k^1-{\hat{X}}_k^2)$$
$$P_{k/k-1}^{11}=P_{k-1}^{11}+2P_{k-1}^{12}+P_{k-1}^{22}$$
$$P_{k/k-1}^{12}=0.5(P_{k-1}^{12}+P_{k-1}^{22})$$
$$P_{k/k-1}^{22}=0.25P_{k-1}^{22}+1$$
$$K_k^1=P_k^{11}=\frac{P_{k/k-1}^{11}}{P_{k/k-1}^{11}+1}$$
$$K_k^2=P_k^{12}=\frac{P_{k/k-1}^{12}}{P_{k/k-1}^{11}+1}$$
$$P_k^{22}=P_{k/k-1}^{22}-P_k^{12}{P}_{k/k-1}^{12}$$
       初值也有$P_0^{11}=10$，$P_0^{12}=0$，$P_0^{22}=10$。手动迭代7次，看一下$P_k$的收敛情况。

k	1	2	3	4	5	6	7
$P_k$	0.952	0.815	0.736	0.700	0.693	0.693	0.693

       然后，我们在用前面讲的降阶后的次优滤波方法在滤一次。
$${\Sigma }_{k+1/k}=P_k+2C_k+A_k$$
$$K_{k+1}={\Sigma }_{k+1/k}({\Sigma }_{k+1/k}+1{)}^{-1}$$
$$P_{k+1}=(1-K_{k+1}){\Sigma }_{k+1/k}$$
$$A_{k+1}=0.25A_k+1$$
$$C_{k+1}=0.5(1-K_{k+1})(C_k+A_k)$$
$${\hat{X}}_{k+1}^1={\hat{X}}_k^1+K_{k+1}(Z_{k+1}-{\hat{X}}_k^1)$$
       前面推导得到的方程看着复杂一些，但是真正用起来时却发现比基本方程要计算简单。
       初值也有。$P_0=10$，$C_0=0$，$A_0=10$。还是迭代七次，看估值方差的变化。

k	1	2	3	4	5	6	7
$P_k$	0.952	0.831	0.799	0.735	0.719	0.715	0.714

       看来两种方法的估计精度还是比较接近的。
       最后。想不想看看完全不考虑$X_k^2$的影响，直接将方程退化，会是什么效果？
       试一下就知道了。
$${\hat{X}}_{k+1}^1={\hat{X}}_k^1+K_{k+1}^1(Z_{k+1}-{\hat{X}}_k^1)$$
$$K_k^1=\frac{P_{k/k-1}^1}{P_{k/k-1}^1+1}$$
$$P_{k/k-1}^1=P_{k-1}^1$$
$$P_k^1=K_k$$
       然后$P_0^1=10$，还是迭代7次，看$P_k$。

k	1	2	3	4	5	6	7
$P_k$	0.909	0.476	0.323	0.244	0.196	0.164	0.141
$P_k^r$	0.938	1.248	2.215	3.200	4.157	5.150	7.810

       上表的第三行是实际误差的均方差（也就是简化后的系统跟原系统之间的误差），竟然发散了！

小结

       这个例子说明了，降阶处理的次优滤波效果其实也不错，至少比单纯的删去状态量要好。但是要注意，这种方法并不能保证任意删去多少状态都可以的。另外，这种方法其实相比卡尔曼滤波基本方程计算量减少的并不多。这套方法还有一种另类用途，就是尝试可能降阶的状态变量来分析哪些状态对系统影响较小。