捷联惯导系统学习6.6(Sage-Husa自适应滤波 )

原理作用

只有准确的获得系统的结构参数和噪声统计特性参数，才能获得最优值的状态估计，实际中往往是不够准确的
可以使用量测输出（输出隐含了系统模型的某些信息）对系统系统模型进行重新估计。

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

系统状态空间
$X_k:n维状态向量$
$Z_k:m维测量向量$
${\Phi }_{k/k-1}:已知的系统结构参数$
${\Gamma }_{k/k-1}:已知的系统结构参数，分别为n\times l阶系统分配噪声$
$H_k:已知的系统结构参数，分别为m\times n阶测量矩阵$
$V_k:m维测量噪声，高斯白噪声，服从正太分布$
$W_{k-1}:m维系统噪声向量，高斯白噪声，服从正太分布$
$V_k与W_{k-1}互不相关$
$$\begin{gathered} \{\begin{array}{l}{X}_k={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array} \\ st. \\ \{\begin{array}{ll}E[W_k]=0,E[W_k{W}_j^T]=Q_k{\delta }_{kj}& Q_k\ge 0\\ E[V_k]=0,E[V_k{V}_j^T]=R_k{\delta }_{kj},E[W_k{V}_j^T]=0& R\ge 0\end{array} \end{gathered}$$

$E[V_k{V}_j^T]=R_k{\delta }_{kj}$这里的$R_k$是未知的，求解$R_k$

观测误差方程
$\hat{Z}:量测预测值$
$Z:量测实际值$
$\tilde{Z}:量测误差：预测-实际值$
$E[{\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T]:随机序列集的总平均值，实际使用时用时间平均代替$
$${\tilde{Z}}_{k/k-1}=Z_k-{\hat{Z}}_{k/k-1}=H_k{X}_k+V_k-H_k{\hat{X}}_{k/k-1}=H_k{\tilde{X}}_{k/k-1}+V_k$$
在初始状态选取无偏状态下，状态一步预测${\tilde{X}}_{k/k-1}$是无偏的，量测噪声$V_k$的均差为0，得到量测预测误差${\tilde{Z}}_{k/k-1}$的均值也为0，${\tilde{X}}_{k/k-1}$与$V_k$互不相关。
$$E[{\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T]=H_k{P}_{k/k-1}{H}_k^T+R_k$$
移项得到：
$$R_k=E[{\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T]-H_k{P}_{k/k-1}{H}_k^T$$
$R_k$的等加权递推估计方法如下：
${\hat{R}}_k:测量噪声方差阵的表示,R_0:根据实际大小选举$
$$\begin{gathered} {\hat{R}}_k=\frac{1}{k}\sum _{i=1}^k({\tilde{Z}}_{i/i-1}{\tilde{Z}}_{k/k-1}^T-H_i{P}_{i/i-1}{H}_i^T) \\ =\frac{1}{k}[\sum _{i=1}^{k-1}({\tilde{Z}}_{i/i-1}{\tilde{Z}}_{i/i-1}^T-H_i{P}_{i/i-1}{H}_i^T)+({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T)] \\ =\frac{1}{k}[(k-1){\hat{R}}_{k-1}+({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T)] \\ =(1-\frac{1}{k}){\hat{R}}_{k-1}+\frac{1}{k}({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T) \end{gathered}$$
但是当：$k\to \infty ;\frac{1}{k}\to 0$,自适应能力减弱
$$\begin{gathered} {\hat{R}}_k=(1-{\beta }_k){\hat{R}}_{k-1}+{\beta }_k({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T) \\ st:{\beta }_k=\frac{{\beta }_{k-1}}{{\beta }_{k-1}+b}（{\beta }_0=1,0<b<1） \end{gathered}$$

1. 当$k\to \infty$时，${\beta }_k\approx 1-b$,b常取$b=0.9-0.999$,b值不能太小，否则会使估计结果剧烈跳变
2. 以下两种情况会造成$({\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T-H_k{P}_{k/k-1}{H}_k^T)<0$,使${\hat{R}}_k$失去正定性
   （1）：$H_k{P}_{k/k-1}{H}_k^T$过大：初始噪声设置够大
   （2）：${\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T$过小:实际系统噪声噪声与理论建模值项比较小
   （3）：解决办法：采用序贯滤波对${\hat{R}}_k$对角线每个元素进行限制：
   假设${\hat{R}}_k$为对角矩阵，使用序贯滤波对第i个标量采用序贯量测更新时，标量量测更新为：
   $$Z_k^{(i)}=H_k^{(i)}{X}_k+V_k^{(i)}$$
   令：$p_k^{(i)}=({\tilde{Z}}_{k/k-1}^{(i)}{)}^2-H_k^{(i)}{P}_{k/k-1}^{(i)}(H_k^{(i)}{)}^T$
   $${\hat{R}}_k^{(i)}=\{\begin{array}{ll}(1-{\beta }_k){\hat{R}}_{k-1}^{(i)}+{\beta }_k{R}_{min}^{(i)}& p_k^{(i)}<R_{min}^{(i)}\\ R_{max}^{(i)}& p_k^{(i)}>R_{max}^{(i)}\\ (1-{\beta }_k){\hat{R}}_{k-1}^{(i)}+{\beta }_k{p}_k^{(i)}& 其他\\ R_{min}^{(i)}保证{\hat{R}}_k^{(i)}为正& \\ R_{max}^{(i)}快速降低Z_k^{(i)}的可信度并判断量测是否异常，若异常放弃本次更新& \end{array}$$
   （4）：自适应滤波的增益计算回路是非线性的，需要进行客观测性和稳定分析，实际中应尽量减少自适应个数。