卡尔曼滤波原理（十）量测故障检测与强跟踪滤波：量测故障检测与隔离、量测方差自适应滤波、单重渐消因子自适应滤波、多重渐消因子自适应滤波、基于序贯算法的多重渐消因子自适应滤波

函数模型
$$\{\begin{array}{l}{\mathbf{X}}_k={\mathbf{\Phi }}_{k/k-1}{\mathbf{X}}_{k-1}+{\mathbf{\Gamma }}_{k-1}{\mathbf{W}}_{k-1}\\ {\mathbf{Z}}_k={\mathbf{H}}_k{\mathbf{X}}_k+{\mathbf{V}}_k\end{array}$$
随机模型
$$\{\begin{array}{lc}\mathrm{E}\left[{\mathbf{W}}_k\right]=0,& \mathrm{E}\left[{\mathbf{W}}_k{\mathbf{W}}_j^{\mathrm{T}}\right]={\mathbf{Q}}_k{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{V}}_k\right]=0,& \mathrm{E}\left[{\mathbf{V}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]={\mathbf{R}}_k{\delta }_{kj}\\ \mathrm{E}\left[{\mathbf{W}}_k{\mathbf{V}}_j^{\mathrm{T}}\right]=0& \end{array}$$
新息序列方差阵： $\mathrm{E}\left[{\tilde{\mathbf{Z}}}_{k/k-1}{\tilde{\mathbf{Z}}}_{k/k-1}^{\mathrm{T}}\right]={\mathbf{H}}_k\left({\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\right){\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k$

经典滤波模型中左边应该等于右边，但实际计算过程中由于粗差，两边不相等，可以进行处理

前面部分 $\mathrm{E}\left[{\tilde{\mathbf{Z}}}_{k/k-1}{\tilde{\mathbf{Z}}}_{k/k-1}^{\mathrm{T}}\right]$ 按照Sage-Husa滤波的方法，记为 ${\hat{\mathbf{C}}}_k$ ：
$${\hat{\mathbf{C}}}_k\approx {\mathbf{H}}_k\left({\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\right){\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k$$
${\hat{\mathbf{C}}}_k$ 的计算方法与Sage-Husa相同，迭代算时间平均：
$${\hat{\mathbf{C}}}_k=\left(1-{\beta }_k\right){\hat{\mathbf{C}}}_{k-1}+{\beta }_k{\tilde{\mathbf{Z}}}_{k/k-1}{\tilde{\mathbf{Z}}}_{k/k-1}^{\mathrm{T}}$$
针对这个式子，可以检测两边是否相等，如果不相等可以做相应的处理。

1、量测故障检测与隔离

对式子两边求迹，再比较，如果左边特别大，很可能就出现了粗差，可以对剔除。
$$\mathrm{tr}\left({\hat{\mathbf{C}}}_k\right)\gg \mathrm{tr}\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)$$

2、量测方差自适应滤波

有些值差别比较大，但并非完全不能用，想保留，就在其量测方差前面乘上一个 ${\alpha }_k$ 减小估计时的权重
$${\alpha }_k{\mathbf{R}}_k\approx {\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\alpha }_k=\frac{\mathrm{tr}\left({\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{P}}_{kkk-1}{\mathbf{H}}_k^{\mathrm{T}}\right)}{\mathrm{tr}\left({\mathbf{R}}_k\right)}$$

与Sage-Husa: ${\hat{\mathbf{R}}}_k=\left(1-{\beta }_k\right){\hat{\mathbf{R}}}_{k-1}+{\beta }_k\left({\tilde{\mathbf{Z}}}_{k\mid k-1}{\tilde{\mathbf{Z}}}_{k\mid k-1}^{\mathrm{T}}-{\mathbf{H}}_k{\mathbf{P}}_{k\mid k-1}{\mathbf{H}}_k^{\mathrm{T}}\right)$ 相比

• Sage-Husa用迭代的方式来修改 $R$，量测方差自适应滤波直接根据当前历元来修改 $R$

3、遗忘滤波（单重渐消因子自适应滤波）

$${\hat{\mathbf{C}}}_k\approx {\mathbf{H}}_k\left({\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\right){\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k$$

两边如果不相等，就在 $P_{k-1}$ 前面乘上一个遗忘因子 ${\lambda }_k$ ，放大前一时刻的方差，化为：
$${\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}-{\mathbf{R}}_k\approx {\mathbf{H}}_k{\mathbf{\Phi }}_{klk-1}\left({\lambda }_k{\mathbf{P}}_{k-1}\right){\mathbf{\Phi }}_{kkk-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}$$
其中，$\lambda$ 的取值应该使左右两边尽量相等，但没法通过乘一个标量使两个矩阵相等，只能使两矩阵求迹相等：
$$\begin{array}{ll}{\lambda }_k=\max \left(1,\frac{\mathrm{tr}\left({\mathbf{N}}_k\right)}{\mathrm{tr}\left({\mathbf{M}}_k\right)}\right)& \\ {\mathbf{M}}_k={\mathbf{H}}_k{\mathbf{\Phi }}_{klk-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}& \text{ 以前的信息 }\\ {\mathbf{N}}_k={\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}^{\mathrm{T}}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}-{\mathbf{R}}_k& \text{ 当前的信息 }\end{array}$$
滤波公式：
$$\{\begin{array}{l}{\hat{\mathbf{X}}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\hat{\mathbf{X}}}_{k-1}\\ {\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}\left({\lambda }_k{\mathbf{P}}_{k-1}\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{I}}_{k-1}^{\mathrm{T}}\\ {\mathbf{K}}_k={\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\\ {\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)\\ {\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}\end{array}$$

• 与遗忘滤波非常相似，只是遗忘滤波的遗忘因子 $s$ 是定值，而这里的 $\lambda$ 实时变化。
• 此方法也称单重渐消因子自适应滤波；所谓“单重”，就是值 $\lambda$ 是标量。

4、多重渐消因子自适应滤波

单重渐消因子自适应滤波中的 $\lambda$ 是标量，把矩阵相等简化为求迹相等，会损失信息。只是把所有观测值作为整体处理，可以处理的更细致一点，用多重渐消因子自适应滤波。

单重：${\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}-{\mathbf{R}}_k\approx {\mathbf{H}}_k{\mathbf{\Phi }}_{k/k-1}\left({\lambda }_k{\mathbf{P}}_{k-1}\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}$

多重是乘矩阵，为保证对称性，前后都要乘，且乘的 $\Lambda$ 是对角阵：
$${\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}-{\mathbf{R}}_k\approx {\mathbf{H}}_k{\mathbf{\Phi }}_{k/k-1}\left({\mathbf{\Lambda }}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{\Lambda }}_{\mathbf{k}}\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}$$
其中
$$\begin{array}{l}{\mathbf{\Lambda }}_k=\mathrm{diag}\left(\sqrt{{\lambda }_k(1)},\sqrt{{\lambda }_k(2)},\cdots ,\sqrt{{\lambda }_k(n)}\right)=\sqrt{c_k}\mathbf{\alpha }\\ {\lambda }_k(i)=\max \left(1,c_k,{\alpha }_i\right)c_k=\frac{\mathrm{tr}\left({\mathbf{N}}_k\right)}{\mathrm{tr}\left({\mathbf{M}}_k\right)}\\ {\mathbf{M}}_k={\mathbf{H}}_k{\mathbf{\Phi }}_{k/k-1}\left(\mathbf{\alpha }{\mathbf{P}}_{k-1}\mathbf{\alpha }\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}\\ {\mathbf{N}}_k={\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}-{\mathbf{R}}_k\end{array}$$
滤波公式
$$\{\begin{array}{l}{\hat{\mathbf{X}}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\hat{\mathbf{X}}}_{k-1}\\ {\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}\left({\mathbf{\Lambda }}_k{\mathbf{P}}_{k-1}{\mathbf{\Lambda }}_k\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\\ {\mathbf{K}}_k={\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\\ {\hat{\mathbf{X}}}_k={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{Z}}_k-{\mathbf{H}}_k{\hat{\mathbf{X}}}_{k/k-1}\right)\\ {\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}\end{array}$$

5、基于序贯算法的多重渐消因子自适应滤波

多重：${\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}-{\mathbf{R}}_k\approx {\mathbf{H}}_k{\mathbf{\Phi }}_{k/k-1}\left({\mathbf{\Lambda }}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{\Lambda }}_{\mathbf{k}}\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}$

改写成标量量测，左右两边都是变量，就好处理多了。直接处理不太方便，先进行变换：
$$\begin{array}{l}{\hat{\mathbf{C}}}_k-{\mathbf{H}}_k{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}-{\mathbf{R}}_k\approx {\mathbf{H}}_k{\mathbf{\Phi }}_{k/k-1}\left({\mathbf{\Lambda }}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{\Lambda }}_{\mathbf{k}}\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}\\ {\hat{\mathbf{C}}}_k-{\mathbf{R}}_k\approx {\mathbf{H}}_k\left[{\mathbf{\Phi }}_{k/k-1}\left({\mathbf{\Lambda }}_k{\mathbf{P}}_{k-1}{\mathbf{\Lambda }}_k\right){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\right]{\mathbf{H}}_k^{\mathrm{T}}\\ \approx {\mathbf{H}}_k\left[{\mathbf{\Lambda }}_k\left({\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\right){\mathbf{\Lambda }}_k\right]{\mathbf{H}}_k^{\mathrm{T}}\end{array}$$
序贯，左右两边变成两个标量相等：
$$\begin{array}{l}{\hat{C}}_k^{(j)}-R_k^{(j)}\approx {\mathbf{H}}_k^{(j)}\left({\mathbf{\Lambda }}_k^{(j)}{\mathbf{P}}_k^{(j-1)}{\mathbf{\Lambda }}_k^{(j)}\right){\left({\mathbf{H}}_k^{(j)}\right)}^{\mathrm{T}}\\ c_k^{(j)}=\frac{N_k^{(j)}}{M_k^{(j)}}\begin{array}{l}{N}_k^{(j)}={\hat{C}}_k-R_k^{(j)}\\ M_k^{(j)}={\mathbf{H}}_k^{(j)}{\mathbf{\alpha }}^{(j)}{\mathbf{P}}_k^{(j)}{\mathbf{\alpha }}^{(j)}{\left({\mathbf{H}}_k^{(j)}\right)}^{\mathrm{T}}\end{array}\end{array}$$
滤波公式
$$\{\begin{array}{l}{\hat{\mathbf{X}}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\hat{\mathbf{X}}}_{k-1}\\ {\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\\ {\mathbf{K}}_k^{(j)}={\mathbf{P}}_k^{(j-1)}{\mathbf{\Lambda }}_k^{(j)}{\left({\mathbf{H}}_k^{(j)}\right)}^{\mathrm{T}}{\left[{\mathbf{H}}_k^{(j)}{\mathbf{\Lambda }}_k^{(j)}{\mathbf{P}}_k^{(j-1)}{\mathbf{\Lambda }}_k^{(j)}{\left({\mathbf{H}}_k^{(j)}\right)}^{\mathrm{T}}+{\mathbf{R}}_k^{(j)}\right]}^{-1}\\ {\hat{\mathbf{X}}}_k^{(j)}={\hat{\mathbf{X}}}_{k/k-1}+{\mathbf{K}}_k^{(j)}\left(Z_k^{(j)}-{\mathbf{H}}_k^{(j)}{\hat{\mathbf{X}}}_{k/k-1}\right)\\ {\mathbf{P}}_k^{(j)}=\left(\mathbf{I}-{\mathbf{K}}_k^{(j)}{\mathbf{H}}_k^{(j)}\right){\mathbf{P}}_k^{(j-1)}\end{array}$$
量测更新要做很多次，对每一个量测分量都要处理

6、强跟踪滤波（string tracking filter）

具有以下优良特性：

• 较强的关于模型参数失配的鲁棒性；主要是量测模型，
• 较低的关于噪声及初值统计特性的敏感性；
• 极强的关于突变状态的跟踪能力，并在滤波器达到稳态时仍保持这种能力；
• 适中的计算复杂性。
• 理论上，强跟踪滤波可以在线自适应调整Kalman滤波增益使得输出残差序列正交（所谓正交性原理）。

然而实际应用中，“理想是很理想的，但现实是很现实的”，特别是对于高维复杂系统而言，强跟踪滤波器的想法几乎不具有可操作性。