捷联惯导系统学习6.5(遗忘滤波 )

原理作用

当系统建模存在偏差时，滤波器往往出现过度拟合，不能反映系统真实状态，均方误差阵$P_k$并不能反映状态估计精度。
解决办法是修改系统噪声$Q_k,$和量测噪声$R_k$的权重，从而减少历史信息的权重，相对提高了新信息的权重，减小系统惯性，在建模不准情况下改善滤波性能。

系统状态空间

$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[W_k{W}_j^T]$和量测噪声$E[V_k{V}_j^T]$

$s>1:s为遗忘因子,s越大遗忘速度越大$
$$\begin{gathered} E[W_k{W}_j^T]=s^{N-k+1}{Q}_k{\delta }_{kj} \\ E[V_k{V}_j^T]=s^{N-k}{R}_k{\delta }_{kj}(i,j\le N) \end{gathered}$$

kalman滤波公式（n时刻遗忘滤波模型下的）

n时刻的滤波公式如下：
$$\{\begin{array}{ll}{\hat{X}}_{k/k-1}^N={\Phi }_{k/k-1}{\hat{X}}_{k-1}^N& 状态预测/时间更新\\ P_{k/k-1}^N={\Phi }_{k/k-1}{P}_{k-1}^N{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{s}^{N-k}{Q}_{k-1}{\Gamma }_{k-1}^T& 状态预测均方差阵/量测更新\\ K_k^N=P_{k/k-1}^N{H}_k^T(H_k{P}_{k/k-1}^N{H}_k^T+s^{N-k}{R}_k{)}^{-1}& 滤波增益\\ {\hat{X}}_k^N=(I-K_k{H}_k){\hat{X}}_{k/k-1}^N+K_k^N{Z}_k& 状态估计\\ P_k^N=(I-K_k^N{H}_k)P_{k/k-1}^N& 状态估计均方误差阵\end{array}$$
简化：
令${\hat{X}}_{k/k-1}^{\ast }={\Phi }_{k/k-1}{\hat{X}}_{k-1}^{\ast };{\hat{X}}_k^{\ast }={\hat{X}}_k^N;{\hat{X}}_{k/k-1}^{\ast }={\hat{X}}_{k/k-1}^{\ast };K_k^{\ast }=K_k^N$
$$\{\begin{array}{ll}{\hat{X}}_{k/k-1}^{\ast }={\Phi }_{k/k-1}{\hat{X}}_{k-1}^{\ast }& 状态预测/时间更新\\ P_{k/k-1}^{\ast }={\Phi }_{k/k-1}(s{P}_{k-1}^{\ast }){\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T& 状态预测均方差阵/量测更新\\ K_k^{\ast }=P_{k/k-1}^{\ast }{H}_k^T(H_k{P}_{k/k-1}^{\ast }{H}_k^T+R_k{)}^{-1}& 滤波增益\\ {\hat{X}}_k^{\ast }=(I-K_k{H}_k){\hat{X}}_{k/k-1}^{\ast }+K_k^{\ast }{Z}_k& 状态估计\\ P_k^{\ast }=(I-K_k^{\ast }{H}_k)P_{k/k-1}^{\ast }& 状态估计均方误差阵\end{array}$$