捷联惯导系统学习6.4(奇异值分解滤波 )

奇异值（singular Value Decomposition,SVD）分解滤波

将三角阵的求逆变为对角阵(plotter平方根滤波)的求逆,大大降低计算量
<奇异值分解:>
$A:当为n阶实对称矩阵,可分解为：$
$Q{Q}^T=I,Q为标准正交阵，Q的列坐标为特征向量$
$\Gamma :为特征值构成的对角矩阵$
$$\begin{gathered} A=Q\Gamma Q^T(特征值分解EVD) \\ \Gamma =\left[\begin{array}{cccc}{\lambda }_1& ...& ...& ....\\ ...& {\lambda }_2& ...& ....\\ ...& ...& ....& ....\\ ...& ...& ...& {\lambda }_n\end{array}\right] \end{gathered}$$
$A:当为n阶实矩阵，可分解为：$
$U{U}^T=I,U为单位正交阵，左奇异矩阵,U\in R_{m\times m}$
$V{V}^T=I,V为单位正交阵，右奇异矩阵,V\in R_{n\times n}$
$\Sigma :奇异值，除了对角元素，其他元素为0,\Sigma \in R_{m\times n}$
$$\begin{gathered} A=U\Sigma V^T \\ \Sigma ={\left[\begin{array}{ccccc}{\sigma }_1& 0& ...& 0& 0\\ 0& {\sigma }_2& ...& 0& 0\\ 0& 0& ...& 0& 0\\ 0& 0& ...& {\sigma }_n& 0\end{array}\right]}_{m\times n} \end{gathered}$$
求解：
$$A{A}^T=U\Sigma V^{T}V{\Sigma }^T{U}^T=U\Sigma {\Sigma }^T{U}^T$$
已知kalman滤波均方误差阵更新公式如下:
$$\begin{gathered} P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T \\ {P}_k^{-1}=P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k \end{gathered}$$
已知$P_{k/k-1},P_k^{-1}$:是对称正定的，那么总可以进行奇异值分解：
$U_k,U_{k/k-1}:单位正交阵$
${\Lambda }_k,{\Lambda }_{k/k-1}:n阶对角阵且对角线元素均为正$
$$P_k=U_k{\Lambda }_k{U}_k^T,P_{k/k-1}=U_{k/k-1}{\Lambda }_{k/k-1}{U}_{k/k-1}^T$$
$P_{k/k-1}$的推导:
带入均方误差阵：
$$\begin{gathered} P_{k/k-1}=U_{k/k-1}{\Lambda }_{k/k-1}{U}_{k/k-1}^T={\Phi }_{k/k-1}{U}_{k-1}{\Lambda }_{k-1}{U}_{k-1}^T{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T \\ =\left[\begin{array}{cc}{\Phi }_{k/k-1}{U}_{k-1}{\Lambda }_{k-1}^{1/2}& {\Gamma }_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]\left[\begin{array}{c}({\Lambda }_{k-1}^{1/2}{)}^T{U}_{k-1}^T{\Phi }_{k/k-1}^T\\ (Q_{k-1}^{1/2}{)}^T{\Gamma }_{k-1}^T\end{array}\right] \end{gathered}$$
再对$\left[\begin{array}{cc}{\Phi }_{k/k-1}{U}_{k-1}{\Lambda }_{k-1}^{1/2}& {\Gamma }_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]$进行奇异值分解为：
$S_{k/k-1}:n阶单位正交阵$
$V_{k/k-1}:n+1阶单位正交阵$
$$\left[\begin{array}{cc}{\Phi }_{k/k-1}{U}_{k-1}{\Lambda }_{k-1}^{1/2}& {\Gamma }_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]=S_{k/k-1}{D}_{k/k-1}{V}_{k/k-1}^T$$

带入均方误差阵：
$$\begin{gathered} P_{k/k-1}=U_{k/k-1}{\Lambda }_{k/k-1}{U}_{k/k-1}^T=\left[\begin{array}{cc}{\Phi }_{k/k-1}{U}_{k-1}{\Lambda }_{k-1}^{1/2}& {\Gamma }_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]\left[\begin{array}{c}({\Lambda }_{k-1}^{1/2}{)}^T{U}_{k-1}^T{\Phi }_{k/k-1}^T\\ (Q_{k-1}^{1/2}{)}^T{\Gamma }_{k-1}^T\end{array}\right] \\ =(S_{k/k-1}{D}_{k/k-1}{V}_{k/k-1}^T)(S_{k/k-1}{D}_{k/k-1}{V}_{k/k-1}^T{)}^T \\ =S_{k/k-1}(D_{k/k-1}{D}_{k/k-1}^T)S_{k/k-1}^T \end{gathered}$$
其中$D_{k/k-1}{D}_{k/k-1}^T$是n阶正定的对角阵，令：
$$U_{k/k-1}=S_{k/k-1},{\Lambda }_{k/k-1}^{1/2}=(D_{k/k-1}{D}_{k/k-1}^T{)}^{1/2}$$
取${\Lambda }_{k/k-1}^{1/2}$为长方对角阵$D_{k/k-1}$的对角元素，重新构成方阵，即：
$$D_{k/k-1}=\left[\begin{array}{cc}{\Lambda }_{k/k-1}^{1/2}& 0_{n\times l}\end{array}\right]$$
$P_k$的推导（与$P_{k/k-1}$相同）:
$S_k:n阶单位正交阵$
$V_k:n+m阶单位正交阵$
$D_k:n\times (n+m)阶单位正交阵$
$$\begin{gathered} P_{k/k-1}=U_{k/k-1}{\Lambda }_{k/k-1}^{-1}{U}_{k/k-1}^T+H_k^T{R}_k^{-1}{H}_k \\ =\left[\begin{array}{cc}{U}_{k/k-1}{\Lambda }_{k/k-1}^{-1/2}& H_k^T{R}_k^{-1/2}\end{array}\right]\left[\begin{array}{c}({\Lambda }_{k/k-1}^{-1/2}{)}^T{U}_{k/k-1}^T\\ (R_k^{-1/2}{)}^T{H}_k\end{array}\right] \\ =(S_k{D}_k{V}_k^T)(S_k{D}_k{V}_k^T{)}^T \end{gathered}$$
同理得到：
$$\begin{gathered} U_k=S_k,{\Lambda }_k^{-1/2}=(D_k{D}_k^T{)}^{1/2} \\ {D}_k=\left[\begin{array}{cc}{\Lambda }_k^{-1/2}& 0_{n\times m}\end{array}\right] \end{gathered}$$
奇异值分解滤波算法流程如下:
$\left[\begin{array}{cc}{\Phi }_{k/k-1}{U}_{k-1}{\Lambda }_{k-1}^{1/2}& {\Gamma }_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]{\to }^{SVD}\to (S_{k/k-1},D_{k/k-1}^{})\to (U_{k/k-1},{\Lambda }_{k/k-1}^{1/2})$
$\left[\begin{array}{cc}{U}_{k/k-1}{\Lambda }_{k-1}^{-1/2}& H_k^T{R}_{k-1}^{-1/2}\end{array}\right]{\to }^{SVD}\to (S_k,D_k^{})\to (U_k,{\Lambda }_k^{-1/2})$
$K_k=U_k{\Lambda }_k{U}_k^T{H}_k^T{R}_k^{-1}$