卡尔曼滤波与组合导航原理（九）Sage-Husa自适应滤波

一、自适应滤波基本思想

函数模型
{ X k = Φ k / k − 1 X k − 1 + Γ k − 1 W k − 1 Z k = H k X k + V k \left\{\begin{array}{l} \boldsymbol{X}_{k}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\ \boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k} \end{array}\right. {Xk​=Φk/k−1​Xk−1​+Γk−1​Wk−1​Zk​=Hk​Xk​+Vk​​
随机模型
{ E [ W k ] = q k , E [ W k W j T ] = Q k δ k j E [ V k ] = r k , E [ V k V j T ] = R k δ k j E [ W k V j T ] = 0 \left\{\begin{array}{lc}{\color{red}\mathrm{E}\left[\boldsymbol{W}_{k}\right]=\boldsymbol{q}_{k}}, & \mathrm{E}\left[\boldsymbol{W}_{k} \boldsymbol{W}_{j}^{\mathrm{T}}\right]=\boldsymbol{Q}_{k} \delta_{k j} \\ {\color{red}\mathrm{E}\left[\boldsymbol{V}_{k}\right]=\boldsymbol{r}_{k}}, & \mathrm{E}\left[\boldsymbol{V}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\boldsymbol{R}_{k} \delta_{k j} \\ \mathrm{E}\left[\boldsymbol{W}_{k} \boldsymbol{V}_{j}^{\mathrm{T}}\right]=\mathbf{0} & \end{array}\right. ⎩ ⎨ ⎧​E[Wk​]=qk​,E[Vk​]=rk​,E[Wk​VjT​]=0​E[Wk​WjT​]=Qk​δkj​E[Vk​VjT​]=Rk​δkj​​
系统噪声和量测噪声都不是零均值，且噪声的均值和方差都未知，用标准Kalman滤波没法做。

基本原理：噪声参数不准会影响系统输出，利用输出边作状态估计边作噪声辨识，需要系统的可观测新比较强。

• 噪声均值均可等效于状态增广。把噪声分解成零均值部分和非零均值部分，非零均值的部分作为状态一并估计。所以对系统噪声和量测噪声的均值用自适应方法没有必要，真的未知又想估计它，就把它扩维增广为状态。
• 系统噪声方差难以自适应，代表系统的状态，一般来说系统比较稳定，做自适应比较难。
• 量测噪声方差相对容易自适应，代表仪器和环境的状态，有可能时好时坏，用自适应效果比较好。
• 应尽量减少噪声自适应参数的数目，可以只对容易变化的量测做自适应。

二、量测噪声方差阵自适应算法

新息和新息的方差
Z ~ k / k − 1 = Z k − Z ^ k / k − 1 = H k X k + V k − H k X ^ k / k − 1 = H k X ~ k / k − 1 + V k E [ Z ~ k / k − 1 Z ~ k / k − 1 T ] = H k P k / k − 1 H k T + R k \begin{array}{l} \tilde{\boldsymbol{Z}}_{k / k-1}=\boldsymbol{Z}_{k}-\hat{\boldsymbol{Z}}_{k / k-1}=\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}=\boldsymbol{H}_{k} \tilde{\boldsymbol{X}}_{k / k-1}+\boldsymbol{V}_{k} \\ \mathrm{E}\left[\tilde{\boldsymbol{Z}}_{k / k-1} \tilde{\boldsymbol{Z}}_{k / k-1}^{\mathrm{T}}\right]=\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k} \quad \\ \end{array} Z~k/k−1​=Zk​−Z^k/k−1​=Hk​Xk​+Vk​−Hk​X^k/k−1​=Hk​X~k/k−1​+Vk​E[Z~k/k−1​Z~k/k−1T​]=Hk​Pk/k−1​HkT​+Rk​​
想估计 R R R ，就认为左边都已知，就是新息量测值的方差减去预测量测值方差
R k = E [ Z ~ k / k − 1 Z ~ k / k − 1 T ] − H k P k / k − 1 H k T \boldsymbol{R}_{k}=\mathrm{E}\left[\tilde{\boldsymbol{Z}}_{k / k-1} \tilde{\boldsymbol{Z}}_{k / k-1}^{\mathrm{T}}\right]-\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}} Rk​=E[Z~k/k−1​Z~k/k−1T​]−Hk​Pk/k−1​HkT​
这种的满足只是统计上整体的满足，不是说每一次测量都应该满足。

实际做Kalman滤波研究的往往只有一个样本，用时间平均来算，多个时刻的数据得出 R k R_k Rk​ 。并且写成递推的形式如下：
R ^ k = 1 k ∑ i = 1 k ( Z ~ i / i − 1 Z ~ i / i − 1 T − H i P i / i − 1 H i T ) = 1 k [ ∑ i = 1 k − 1 ( Z ~ i / i − 1 Z ~ i / i − 1 T − H i P i / i − 1 H i T ) + ( Z ~ k / k − 1 Z ~ k / k − 1 T − H k P k / k − 1 H k T ) ] = ( 1 − 1 k ) R ^ k − 1 + 1 k ( Z ~ k / k − 1 Z ~ k / k − 1 T − H k P k / k − 1 H k T ) \begin{aligned} \hat{\boldsymbol{R}}_{k} & =\frac{1}{k} \sum_{i=1}^{k}\left(\tilde{\boldsymbol{Z}}_{i / i-1} \tilde{\boldsymbol{Z}}_{i / i-1}^{\mathrm{T}}-\boldsymbol{H}_{i} \boldsymbol{P}_{i / i-1} \boldsymbol{H}_{i}^{\mathrm{T}}\right) \\ & =\frac{1}{k}\left[\sum_{i=1}^{k-1}\left(\tilde{\boldsymbol{Z}}_{i / i-1} \tilde{\boldsymbol{Z}}_{i / i-1}^{\mathrm{T}}-\boldsymbol{H}_{i} \boldsymbol{P}_{i / i-1} \boldsymbol{H}_{i}^{\mathrm{T}}\right)+\left(\tilde{\boldsymbol{Z}}_{k / k-1} \tilde{\boldsymbol{Z}}_{k / k-1}^{\mathrm{T}}-\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\right)\right] \\ & =\left(1-\frac{1}{k}\right) \hat{\boldsymbol{R}}_{k-1}+\frac{1}{k}\left(\tilde{\boldsymbol{Z}}_{k / k-1} \tilde{\boldsymbol{Z}}_{k / k-1}^{\mathrm{T}}-\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\right)\end{aligned} R^k​​=k1​i=1∑k​(Z~i/i−1​Z~i/i−1T​−Hi​Pi/i−1​HiT​)=k1​[i=1∑k−1​(Z~i/i−1​Z~i/i−1T​−Hi​Pi/i−1​HiT​)+(Z~k/k−1​Z~k/k−1T​−Hk​Pk/k−1​HkT​)]=(1−k1​)R^k−1​+k1​(Z~k/k−1​Z~k/k−1T​−Hk​Pk/k−1​HkT​)​
上式每个时刻的权重都相同，为等加权平均。统计很长时间后 1 k \frac{1}{k} k1​ 会趋于 0 0 0 ，时间越长，自适应的能力越差。

为避免此问题，可以把 K K K 值限定，到了一定数目之后，就不让它再增加。如采用指数渐消记忆加权平均：
R ^ k = ( 1 − β k ) R ^ k − 1 + β k ( Z ~ k / k − 1 Z ~ k / k − 1 T − H k P k / k − 1 H k T ) \hat{\boldsymbol{R}}_{k}=\left(1-\beta_{k}\right) \hat{\boldsymbol{R}}_{k-1}+\beta_{k}\left(\tilde{\boldsymbol{Z}}_{k / k-1} \tilde{\boldsymbol{Z}}_{k / k-1}^{\mathrm{T}}-\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\right) R^k​=(1−βk​)R^k−1​+βk​(Z~k/k−1​Z~k/k−1T​−Hk​Pk/k−1​HkT​)
其中 β k = β k − 1 β k − 1 + b β 0 = 1 , β ∞ = 1 − b , 0 < b < 1 \beta_{k}=\frac{\beta_{k-1}}{\beta_{k-1}+b} \quad \beta_{0}=1, \beta_{\infty}=1-b, 0<b<1 βk​=βk−1​+bβk−1​​β0​=1,β∞​=1−b,0<b<1 称为渐消因子。 b b b 一般取的接近于 1 1 1，如 0.9 、 0.99 、 0.999 0.9、0.99、0.999 0.9、0.99、0.999

式子后半部分有矩阵相减，当实际噪声比较小时，容易出现量测方差负定，可改用“序贯标量量测+方差受限”自适应方法加以解决：

令 ρ k ( i ) = ( Z ~ k / k − 1 ( i ) ) 2 − H k ( i ) P k / k − 1 ( i ) ( H k ( i ) ) T \rho_{k}^{(i)}=\left(\tilde{Z}_{k / k-1}^{(i)}\right)^{2}-\boldsymbol{H}_{k}^{(i)} \boldsymbol{P}_{k / k-1}^{(i)}\left(\boldsymbol{H}_{k}^{(i)}\right)^{\mathrm{T}} ρk(i)​=(Z~k/k−1(i)​)2−Hk(i)​Pk/k−1(i)​(Hk(i)​)T ， R max  ( i ) , R min  ( i ) R_{\text {max }}^{(i)}, R_{\text {min }}^{(i)} Rmax (i)​,Rmin (i)​ 人为设置的方差上下限，有：
R ^ k ( i ) = { ( 1 − β k ) R ^ k − 1 ( i ) + β k R min ⁡ ( i ) ρ k ( i ) < R min ⁡ ( i ) R max ⁡ ( i ) ρ k ( i ) > R max ⁡ ( i ) ( 1 − β k ) R ^ k − 1 ( i ) + β k ρ k ( i )  others  \hat{R}_{k}^{(i)}=\left\{\begin{array}{cc}\left(1-\beta_{k}\right) \hat{R}_{k-1}^{(i)}+\beta_{k} R_{\min }^{(i)} & \rho_{k}^{(i)}<R_{\min }^{(i)} \\ R_{\max }^{(i)} & \rho_{k}^{(i)}>R_{\max }^{(i)} \\ \left(1-\beta_{k}\right) \hat{R}_{k-1}^{(i)}+\beta_{k} \rho_{k}^{(i)} & \text { others }\end{array}\right. R^k(i)​=⎩ ⎨ ⎧​(1−βk​)R^k−1(i)​+βk​Rmin(i)​Rmax(i)​(1−βk​)R^k−1(i)​+βk​ρk(i)​​ρk(i)​<Rmin(i)​ρk(i)​>Rmax(i)​ others ​
流程如下：

左边的系统对右边有作用，右边对左边也有作用，比较复杂，需要对其做稳定性分析和可控性分析很困难。判断工程上能不能用得做仿真。