11.卡尔曼滤波扩展应用——固定点平滑

       挑几个卡尔曼滤波比较典型的应用扩展，希望能够帮助大家在设计卡尔曼滤波器时，对状态方程的建立和量测方程建立的基本方法提供一些思路

最优线性平滑

       前面一系列文章，我觉得我已经基本说明白了一件事，卡尔曼滤波是一种最优估计算法。这回，我们先重新回到最优估计的个概念上来。其实最优估计还可以大致分成三种，滤波、预测和平滑。这三个的概念理解起来不难。
       滤波——就是根据1到k时刻的量测$Z_1$ ~ $Z_k$来对k时刻的状态$X_k$进行最优估计，之前讨论卡尔曼滤波的基本方程主要就是干这个事的。.
       预测——就是根据1到k时刻的量测$Z_1$ ~ $Z_k$来对k时刻之后的某一时刻的状态$X_{k+N}$进行最优估计。卡尔曼滤波的基本方程里有涉及，还记得一步预测${\hat{X}}_{k+1/k}$吗？
       平滑——就是根据1到k时刻的量测$Z_1$ ~ $Z_k$来对k时刻之前的某一时刻的状态$X_{k-N}$进行最优估计。
       滤波和预测我们在之前的卡尔曼滤波基本方程里都用到了，那么就剩平滑了。
       说到平滑，还要分三种情况。
       固定点平滑——只想知道0~k时刻中的某一固定时间点j的状态，对它进行最优估计。一般就是某项实验或某个过程中的某一时刻的状态特别重要，就会用固定点平滑。比如利用人造卫星的轨道数据来估计它入轨时刻的状态。
       固定滞后平滑——根据1到k时刻的量测$Z_1$ ~ $Z_k$估计$X_{k-N}$，N是一个固定的值。也就是说估计的输出与输入之间有一个时间延迟。
       固定区间平滑——就是利用固定时间区间内的量测值$Z_1$、$Z_2$、…$Z_M$来对这个区间内的每一个状态$X_k(k=1,2,\dots M)$进行估计。
       我们先来看看固定点平滑的最优估计方法。

固定点平滑

       还是那个系统。
       系统状态方程：
$$X_{k+1}=X_k+W_k$$
       量测方程：
$$Z_k=X_k+V_k$$
       固定点平滑就是要对某一固定时刻的$X$进行最优估计。比如那个固定时刻是j时刻，那么，我们要求的就是${\hat{X}}_{j/k}$。
       记住一句话，要估计什么，就想办法把要估计的量放到状态量里面去。所以我们就在状态量X里面增加一个状态$X_k^{\alpha }$。它的初值就是$X_j$。而且，由于是固定时刻，所以这是一个常量，也就是：
$$X_{k+1}^{\alpha }=X_k^{\alpha }$$
       那么，我们要求的那个${\hat{X}}_{j/k}$，实际上就是${\hat{X}}_{k+1/k}^{\alpha }$。也就是说，我们把状态方程和量测方程扩展了。
       状态方程变成：
$$\left(\begin{array}{c}{X}_{k+1}\\ X_{k+1}^{\alpha }\end{array}\right)=\left(\begin{array}{cc}{\Phi }_{k+1,k}& 0\\ 0& I\end{array}\right)\left(\begin{array}{c}{X}_k\\ X_k^{\alpha }\end{array}\right)+\left(\begin{array}{c}{\Gamma }_k\\ 0\end{array}\right)W_k$$
       量测方程变成：
$$Z_k=\left(\begin{array}{cc}{H}_k& 0\end{array}\right)\left(\begin{array}{c}{X}_k\\ X_k^{\alpha }\end{array}\right)+V_k$$
       以上方程当k=j时，
$$\left(\begin{array}{c}{X}_j\\ X_j^{\alpha }\end{array}\right)=\left(\begin{array}{c}{X}_j\\ X_j\end{array}\right)$$
       好了，所有条件具备了，带入卡尔曼滤波的基本方程，状态方程和量测方程的阶数翻了一倍，算去吧！
       这计算量也太大了吧。所以我么可以把它分解成两套系统，也就是一套原系统的滤波方程和一套固定点的平滑方程。从0时刻开始，当k小于j时，我们只需要跑原系统的滤波方程估计${\hat{X}}_{k+1/k}$，当k大于等于j时，跑原系统滤波方程的同时，在跑下面这套方程就好了。
$${\hat{X}}_{j/k}={\hat{X}}_{j/k-1}+K_k^{\alpha }(Z_k-H_k{\hat{X}}_{k/k-1})$$
$$K_k^{\alpha }=P_{k/k-1}^{\alpha }{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}$$
$$P_{k+1/k}^{\alpha }=P_{k/k-1}^{\alpha }({\Phi }_{k+1,k}-K_k^{\ast }{H}_k{)}^T$$
       式中$K_k^{\ast }={\Phi }_{k+1,k}{K}_k$。
       最后有几点要说明一下。

1. 既然是平滑，那么就会有平滑的样子，也就是说量测值越多，估计的精度越高。这句话的意思是，k-j越大，获得的${\hat{X}}_{j/k}$越好。这很好理解吧。
2. 从平滑方程里来看，$K_k^{\alpha }$和$P_{k+1/k}^{\alpha }$与量测无关，所以，这两个量是可以单独计算的。其实卡尔曼滤波基本方程也有这个性质。
3. 如果系统是定常系统，那么对于原系统滤波稳定后的${\hat{X}}_{k+1/k}$是趋于常值的，但是，扩展后估计的${\hat{X}}_{j/k}$确是时变的。这是因为每一步平滑，都会让平滑的方差减小。滤波的时间越长，估计的方差就会越小，但是要注意，随着时间的延长，实际改善方差的作用会越来越小。所以，很多时候没必要一直在那里进行平滑计算，精度够了就停吧。
4. 量测噪声越小，则平滑效果越好。如果量测噪声很大，那么上一条说的随着时间增加改善估计方差的效果也不会太大。（测都测不准还平滑个什么）。