捷联惯导系统学习6.4(UD分解滤波 )

UD(unit upper traingular& diagonal factorization)分解滤波

UD(unit upper traingular& diagonal factorization)分解
$P:n阶正定的对称矩阵$
对矩阵P进行上三角-对角分解:
$U:上三角阵$
$D:对角阵$
$$\begin{gathered} P=UD{U}^T \\ P=\left[\begin{array}{cccc}{P}_{11}& P_{12}& ...& P_{1n}\\ P_{21}& P_{22}& ...& P_{2n}\\ ...& ...& ...& ...\\ P_{n1}& P_{n2}& ...& P_{nn}\end{array}\right],U=\left[\begin{array}{cccc}{U}_{11}& U_{12}& ...& U_{1n}\\ U_{21}& U_{22}& ...& U_{2n}\\ ...& ...& ...& ...\\ U_{n1}& U_{n2}& ...& U_{nn}\end{array}\right](U_{ii}=1,(i=1,2,3,..,n)),D=\left[\begin{array}{cccc}{D}_{11}& D_{12}& ...& D_{1n}\\ D_{21}& D_{22}& ...& D_{2n}\\ ...& ...& ...& ...\\ D_{n1}& D_{n2}& ...& D_{nn}\end{array}\right] \end{gathered}$$
可以得到：
$$\begin{gathered} P_{ij}=D_{jj}{U}_{ij}{U}_{jj}+D_{j+1,j+1}{U}_{i,j+1}{U}_{j,j+1}+...+D_{nn}{U}_{in}{U}_{jn} \\ =\sum _{k=i+j}^n{D}_{kk}{U}_{ik}{U}_{jk}+D_{jj}{U}_{ij}{U}_{jj}(1\le i\le n,i\le j\le n) \end{gathered}$$
从而得到上三角阵和对角阵各元素计算规则如下：
$$U_{ij}=\{\begin{array}{ll}(P_{ij}-{\sum }_{k=j+1}^n{D}_{kk}{U}_{ik}{U}_{jk})/D_{jj}& i<j\\ 1& i=j\\ 0& i>j\end{array}$$
UD与乔莱斯三角分解存在如下关系

$\Delta =U\sqrt{D}$
$$P=UD{U}^T=U(\sqrt{D}{\sqrt{D}}^T)U^T=(U\sqrt{D})(U\sqrt{D}{)}^T=\Delta {\Delta }^T$$
UD分解滤波
系统状态空间
$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}$$
kalman滤波
$$\{\begin{array}{ll}{\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}& 状态一步预测\\ 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& 状态一步预测均方差阵\\ K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T-R_k{)}^{-1}& 滤波增益\\ {\hat{X}}_k=(I-K_k{H}_k){\hat{X}}_{k/k-1}+K_k{Z}_k& 状态估计\\ P_k=(I-K_k{H}_k)P_{k/k-1}& 状态估计均方误差阵\end{array}$$
滤波增益带入状态估计均方误差阵
$$P_k=(I-P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T-R_k{)}^{-1}{H}_k)P_{k/k-1}$$
UD分解滤波
对非负定的均方误差阵进行UD分解如下：
$U:上三角阵,D:斜对角阵$
$$P_k=U_k{D}_k{U}_k^T,P_{k/k-1}=U_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T$$
UD分解分解后的方程带入$P_k$
$$\begin{gathered} P_k=U_k{D}_k{U}_k^T=(I-U_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T{H}_k^T(H_k{U}_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T{H}_k^T-R_k{)}^{-1}{U}_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T \\ =U_{k/k-1}[D_{k/k-1}-D_{k/k-1}{U}_{k/k-1}^T{H}_k^T(H_k{U}_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T{H}_k^T+R_k{)}^{-1}{H}_k{U}_{k/k-1}{D}_{k/k-1}]U_{k/k-1}^T \\ =U_{k/k-1}(D_{k/k-1}-{\alpha }^{-1}g{g}^T)U_{k/k-1}^T \\ st: \\ \{\begin{array}{l}\alpha =H_k{U}_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T{H}_k^T+R_k=f^{T}g+R_k\\ f=(H_k{U}_{k/k-1}{)}^T\\ g=D_{k/k-1}{U}_{k/k-1}^T{H}_k^T=D_{k/k-1}f\end{array} \end{gathered}$$
当量测矩阵为标量的情况
可以通过比较等式两端不同，得到$U_k,D_k$的直接计算公式：
$f_j;f的第j个分量,n维列向量$
$g_j:g的第j个分量,n维列向量$
$a$为标量,$a_n=\alpha$
$$\{\begin{array}{ll}{D}_{k,jj}=a_{j-1}/a_j\ast D_{k/k-1,jj}& \\ U_{k,ij}=U_{k/k-1,ij}+{\lambda }_j(g_i+{\sum }_{s=i+1}^{j-1}{U}_{k/k-1,is}\ast g_s)& \\ {\lambda }_j=-\frac{f_j}{a_{j-1}}& \\ a_{j-1}=a_j-f_j{g}_j(j=n,n-1,...,1;i=1,2,...,j-1)& 当R_k>0时，总有a>0\end{array}$$
均方误差阵的时间更新
$$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$$
进行UD分解滤波
$W_{k/k-1}=\left[\begin{array}{cc}{\Phi }_{k/k-1}{U}_{k-1}& {\Gamma }_{k-1}\end{array}\right]$
$\overline{D}=\left[\begin{array}{cc}{D}_{k-1}& 0\\ 0& Q_{k-1}\end{array}\right]$
$$\begin{gathered} P_{k/k-1}=U_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T \\ ={\Phi }_{k/k-1}{U}_{k-1}{D}_{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}& {\Gamma }_{k-1}\end{array}\right]\left[\begin{array}{cc}{D}_{k-1}& 0\\ 0& Q_{k-1}\end{array}\right]\left[\begin{array}{c}{U}_{k-1}^T{\Phi }_{k/k-1}^T\\ {\Gamma }_{k-1}^T\end{array}\right] \\ =W_{k/k-1}{\overline{D}}_{k-1}{W}_{k/k-1}^T \end{gathered}$$
再对$W_{k/k-1}^T进行QR分解$
$W_{k/k-1}^T=\overline{Q}\overline{R}(R为下三角阵)$
$A={\overline{Q}}^T{\overline{D}}_{k-1}\overline{Q}$
$$\begin{gathered} P_{k/k-1}=U_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T=W_{k/k-1}{\overline{D}}_{k-1}{W}_{k/k-1}^T \\ =(\overline{Q}\overline{R}{)}^T{D}_{k/k-1}(\overline{Q}\overline{R}) \\ ={\overline{R}}^{T}A\overline{R} \end{gathered}$$
再对$A进行UD分解$
$A=\overline{U}\overline{D}{\overline{U}}^T$
$$P_{k/k-1}={\overline{R}}^{T}A\overline{R}={\overline{R}}^T\overline{U}\overline{D}{\overline{U}}^T\overline{R}=({\overline{R}}^T\overline{U})\overline{D}({\overline{R}}^T\overline{U}{)}^T$$
令$U_{k/k-1}={\overline{R}}^T\overline{U},D_{k/k-1}=\overline{D}$,也就完成了UD分解滤波
因为UD需要了两次矩阵分解和多次矩阵乘法运算，Bierman给出了直接由$W_{k/k-1}$和${\overline{D}}_{k-1}$进行UD分解，求解$U_{k/k-1}$和$D_{k/k-1}$的方法：

$$\{\begin{array}{ll}{D}_{k/k-1,jj}={\sum }_{s=1}^{n+l}{\overline{D}}_{k-1,ss}{W}_{j,s}^{(n-j)}{W}_{j,s}^{(n-j)}& \\ U_{k/k-1,ij}=\frac{{\sum }_{s=1}^{n+l}({\overline{D}}_{k-1,ss}{W}_{i,s}^{n-j}{W}_{j,s}^{n-j})}{D_{k/k-1,jj}}& \\ W_i^{n-j+1}=W_i^{n-j}-U_{k/k-1,ij}{W}_j^{n-j}& j=n,n-1,...,1;i=1,2,...,j-1\end{array}$$

平方根信息kalman滤波（Square Root Information SRIKF）

信息滤波公式为：
$$\{\begin{array}{l}{N}_{k-1}=M_{k-1}{\Gamma }_{k-1}(Q_{k-1}^{-1}+{\Gamma }_{k-1}^T{M}_{k-1}{\Gamma }_{k-1}{)}^{-1}{\Gamma }_{k-1}^T（正定可逆）\\ M_{k-1}={\Phi }_{k/k-1}^{-T}{I}_{k-1}{\Phi }_{k/k-1}^{-1}(非负正定的)\\ I_{k/k-1}=(I-N_{k-1})M_{k-1}\\ I_k=I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k\\ {\hat{S}}_{k/k-1}=(I-N_{k-1}){\Phi }_{k/k-1}^{-T}{\hat{S}}_{k-1}\\ {\hat{S}}_k={\hat{S}}_{k/k-1}+H_k^T{R}_k^{-1}{Z}_k\end{array}$$
记信息阵$I_k,I_{k/k-1}$的平方根分别记为：
$$I_k={\xi }_k{\xi }_k^T,I_{k/k-1}={\xi }_{k/k-1}{\xi }_{k/k-1}^T$$
将平方根带入滤波方程：
$$\begin{gathered} I_{k/k-1}=(I-N_{k-1})M_{k-1} \\ ={\Phi }_{k/k-1}^{-T}{I}_{k-1}{\Phi }_{k/k-1}^{-1}-{\Phi }_{k/k-1}^{-T}{I}_{k-1}{\Phi }_{k/k-1}^{-1}{\Gamma }_{k-1}({\Gamma }_{k-1}^T{\Phi }_{k/k-1}^{-T}{I}_{k-1}{\Phi }_{k/k-1}^{-1}{\Gamma }_{k-1}+Q_{k-1}^{-1}{)}^{-1}\times {\Gamma }_{k-1}^T{\Phi }_{k/k-1}^{-T}{I}_{k-1}{\Phi }_{k/k-1}^{-1} \end{gathered}$$类比Kalman的均方误差更新公式，可以的SRIKF的时间更新算法：
kalman平方根滤波时间更新方程：
$$\begin{gathered} P_k=P_{k/k-1}-P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}{H}_k{P}_{k/k-1} \\ {\Delta }_k={\Delta }_{k/k-1}[I-{\Delta }_{k/k-1}^T{H}_k^T(p_k{p}_k^T+R_k^{1/2}{P}_k^T{)}^{-1}{H}_k{\Delta }_{k/k-1}] \end{gathered}$$
SRIKF的时间更新算法：
令:${\Delta }_k\to {\xi }_{k/k-1};{\Delta }_{k/k-1}\to {\Phi }_{k/k-1}^{-T}{\xi }_{k-1};H_k\to {\Gamma }_{k-1}^T;R_k^{1/2}\to Q_{k-1}^{-1/2}$
得到:
$${\xi }_{k/k-1}={\Phi }_{k/k-1}^{-T}{\xi }_{k-1}[I-{\xi }_{k-1}^T{\Phi }_{k/k-1}^{-1}{\Gamma }_{k-1}(p_k{p}_k^T+Q_{k-1}^{-1/2}{p}_k^T{)}^{-1}{\Gamma }_{k-1}^T{\Phi }_{k/k-1}^{-T}{\xi }_{k-1}]$$
求解$p_k^T$:
$$\left[\begin{array}{c}{\xi }_{k-1}^T{\Phi }_{k/k-1}^{-1}{\Gamma }_{k-1}\\ (Q_{k-1}^{-1/2}{)}^T\end{array}\right]{\to }^{QR}\to p_k^T$$
$$\left[\begin{array}{c}{\xi }_{k/k-1}^T\\ (R_K^{-1/2}{)}^T{H}_k\end{array}\right]{\to }^{QR}\to p_k^T$$