捷联惯导系统学习5.2(递归最小二乘法 )

采用递推方式求解最小二乘法（Recursive Least Square RLS）可以有效减小数据存储量。

测量方程

$${\overline{Z}}_k={\overline{H}}_{k}X+{\overline{V}}_k$$
$$\begin{gathered} {\overline{Z}}_k=\left[\begin{array}{c}{Z}_1\\ Z_2\\ Z_3\\ ...\\ Z_i\end{array}\right],{\overline{H}}_k=\left[\begin{array}{c}{H}_1\\ H_2\\ H_3\\ ...\\ H_i\end{array}\right],{\overline{V}}_k=\left[\begin{array}{c}{V}_1\\ V_2\\ V_3\\ ...\\ V_i\end{array}\right] \\ E[{\overline{V}}_i]=0,E[{\overline{V}}_i{\overline{V}}_i^T]={\overline{R}}_i=diag(R_1,R_2,R_3,...) \end{gathered}$$
最小二乘估计
$${\hat{X}}_{WLS}=(H^{T}WH{)}^{-1}{H}^{T}WZ$$
对测量方程做最小二乘估计
$k-1$时最小二乘估计
$$\begin{gathered} {\hat{X}}_{k-1}=({\overline{H}}_{k-1}^T{\overline{R}}_{k-1}^{-1}{\overline{H}}_{k-1}{)}^{-1}{\overline{H}}_{k-1}^T{\overline{R}}_{k-1}^{-1}{\overline{Z}}_{k-1}=P_{k-1}{\overline{H}}_{k-1}^T{\overline{R}}_{k-1}^{-1}{\overline{Z}}_{k-1} \\ {P}_{k-1}=({\overline{H}}_{k-1}^T{\overline{R}}_{k-1}^{-1}{\overline{H}}_{k-1}{)}^{-1} \end{gathered}$$
$k$时最小二乘估计
$$\begin{gathered} {\hat{X}}_k=({\overline{H}}_k^T{\overline{R}}_k^{-1}{\overline{H}}_k{)}^{-1}{\overline{H}}_k^T{\overline{R}}_k^{-1}{\overline{Z}}_k \\ =P_k(\left[\begin{array}{cc}{\overline{H}}_{k-1}^T& H_k^T\end{array}\right]\left[\begin{array}{cc}{\overline{R}}_{k-1}^{-1}& 0\\ 0& R_k^{-1}\end{array}\right]\left[\begin{array}{c}{\overline{Z}}_{k-1}\\ Z_k\end{array}\right]) \\ =P_k({\overline{H}}_{k-1}^T{\overline{R}}_{k-1}^{-1}{\overline{Z}}_{k-1}+H_k^T{R}_k^{-1}{Z}_k) \\ ={\hat{X}}_{k-1}+P_k{H}_k^T{R}_k^{-1}(Z_k-H_k{\hat{X}}_{k-1}) \\ ={\hat{X}}_{k-1}+K_k(Z_k-H_k{\hat{X}}_{k-1}) \end{gathered}$$
$$P_k={\overline{H}}_k^T{\overline{R}}_k^{-1}{\overline{H}}_k=(\left[\begin{array}{cc}{\overline{H}}_{k-1}^T& H_k^T\end{array}\right]\left[\begin{array}{cc}{\overline{R}}_{k-1}^{-1}& 0\\ 0& R_k^{-1}\end{array}\right]\left[\begin{array}{c}{\overline{Z}}_{k-1}\\ Z_k\end{array}\right]{)}^{-1}=(P_{k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k{)}^{-1}$$
$$K_k=P_k{H}_k^T{R}_k^{-1}$$
最小二乘估计递推公式
$P_k^{-1}$左乘$P_k$右乘$P_{k-1}$
$$P_k{P}_k^{-1}{P}_{k-1}=P_k(P_{k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k)P_{k-1}$$
得到$P_k$
$$P_k=P_{k-1}^{}-P_k{H}_k^T{R}_k^{-1}{H}_k{P}_{k-1}$$
再将$P_k$右乘$H_k^T$整理可得：
$$K_k=P_k{H}_k^T{R}_k^{-1}=P_{k-1}{H}_k^T(H_k{P}_{k-1}{H}_k^T+R_k{)}^{-1}$$
得到递推公式为：
$$\{\begin{array}{l}{K}_k=P_{k-1}{H}_k^T(H_k{P}_{k-1}{H}_k^T+R_k{)}^{-1}\\ P_k=P_{k-1}^{}-P_k{H}_k^T{R}_k^{-1}{H}_k{P}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k-1}+K_k(Z_k-H_k{\hat{X}}_{k-1})\end{array}$$