卡尔曼滤波与组合导航原理（七）平方根Kalman滤波：Potter平方根滤波、SVD分解滤波、UD分解滤波、平方根信息滤波SRIKF

常见矩阵分解简介：

• LU分解：$M=LU$ ，$L$ 是下三角矩阵，$U$ 是上三角矩阵。方便求矩阵的行列式和逆，解线性方程组。
• Cholesky分解：$M=L^{T}L$ ，$L$ 是上三角矩阵。是LU分解的进阶版。但是不同于LU分解的是，Cholesky分解只适用于正定的对称阵。在复数域，Cholesky分解要求矩阵是正定的共轭对称矩阵。要求这么多，就是因为Cholesky分解可以看成是给矩阵开平方根。
• QR分解：$M=QR$ ，其中 $Q$ 是正交矩阵，$R$ 是上三角矩阵。主要有三种方法：Gram-Schmidt正交化法（QR分解中的Q本身就可以看作是正交化构造出来的），Household变换法，Givens变换法。 可以用来求矩阵的特征值以及求解最小二乘法。
• 特征值分解：$M=VD{V}^T$ ，其中 $V$ 是正交阵，$D$ 是由 $M$ 的特征值构成的对角矩阵。只适用于方阵，目的是提取出矩阵最重要的特征。
• 奇异值分解（SVD分解）：$M=US{V}^T$ ，其中 $U$、$V$ 是正交矩阵，$S$ 是由 $M$ 的奇异值构成的对角矩阵。特征值分解只适用于方阵，对于普通矩阵，则可以采用奇异值分解，提取出奇异值。
• UD分解：$M=UD{U}^T$，$U$ 为上三角且对角线元素为 $1$，$D$ 为对角阵

一、平方根滤波基本形式

在计算机中，单精度浮点数(float)有效数字为7位，双精度 (double)为16位，为了提高前者环境下的均方差阵计算精度，须采用平方根滤波。Kalman滤波计算过程中，有三个方差阵，分别可以记为：
$$\begin{array}{l}{\mathbf{P}}_{\mathbf{k}-1}={\mathbf{U}}_{k-1}{\mathbf{U}}_{k-1}^{\mathrm{T}}\\ {\mathbf{P}}_{k/k-1}={\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}\\ {\mathbf{P}}_k={\mathbf{U}}_k{\mathbf{\Delta }}_k^{\mathrm{T}}\end{array}$$

方差都可以表示成平方的形式，标量的平方直接拆成中误差的平方就行，矩阵的平方可以拆成一个矩阵乘以它的转置，最常见是表示成下三角阵乘上三角阵形式（Cholesky分解）。

标准Kalman滤波的流程图可以表示成：

将图中的 $P、Q、R$ 都用平方根形式替换 ，得：

• 更新都用平方根，损失精度小。
• 每一次滤波只计算一次增益矩阵 $K$ ，用于左边的滤波计算回路，而对右边的增益计算回路无影响，可以损失一点精度。

二、Potter平方根滤波

1、方差阵的量测更新

考虑量测为标量的情况，由标准Kalman滤波方差阵递推公式：

如果不是标量量测，就用序贯滤波的方法，一个个做滤波

$$\begin{array}{l}{\mathbf{K}}_k={\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+R_k\right)}^{-1}\\ {\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k/k-1}\end{array}$$

将 $K$ 带入第二个公式：
$${\mathbf{P}}_k={\mathbf{P}}_{k/k-1}-{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+R_k\right)}^{-1}{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}$$
将方差阵Cholesky分解，表示成平方根形式：
$$\begin{array}{l}{\mathbf{\Delta }}_k{\mathbf{\Delta }}_k^{\mathrm{T}}={\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}-{\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}+R_k\right)}^{-1}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}\\ ={\mathbf{\Delta }}_{k/k-1}\left[\mathbf{I}-{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}+R_k\right)}^{-1}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right]{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}\end{array}$$
其中，${\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}+R_k$ 是标量，记为 ${\rho }_k^2$ ，可以在矩阵的前后随意移动，上式变为：
$${\mathbf{\Delta }}_{k/k-1}\left(\mathbf{I}-{\rho }_k^{-2}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right){\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}$$

其中，绿色部分 $\left(\mathbf{I}-{\rho }_k^{-2}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right)$ 的平方根分解：

取 ${\gamma }_k^{-1}$ 为待 定系数，写成分解的形式：
$$\begin{array}{rl}(\mathbf{I}-& {\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}){\left(\mathbf{I}-{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right)}^{\mathrm{T}}\\ & =\mathbf{I}-2{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}+{\gamma }_k^{-2}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\\ & =\mathbf{I}-2{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}+{\gamma }_k^{-2}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}\left({\rho }_k^2-R_k\right){\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\\ & =\mathbf{I}-\left[2{\gamma }_k^{-1}-\left({\rho }_k^2-R_k\right){\gamma }_k^{-2}\right]{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\end{array}$$
最后的式子和原式很相似，只差了棕色的部分，令棕色部分相等，可解待定系数 ${\gamma }_k^{-1}$ ：
$${\rho }_k^{-2}=2{\gamma }_k^{-1}-\left({\rho }_k^2-R_k\right){\gamma }_k^{-2}$$
由：
$$\begin{array}{l}{\rho }_k^{-2}=2{\gamma }_k^{-1}-\left({\rho }_k^2-R_k\right){\gamma }_k^{-2}\\ {\gamma }_k^2-2{\rho }_k^2{\gamma }_k+{\rho }_k^2\left({\rho }_k^2-R_k\right)=0{\rho }_k^2={\mathbf{H}}_k{\mathbf{\Delta }}_{k\mid k-1}{\mathbf{\Delta }}_{k\mid k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}+R_k\end{array}$$
由于是二次，所以 ${\gamma }_k^{-1}$ 有两个解：
$${\gamma }_k=\frac{2{\rho }_k^2\pm \sqrt{4{\rho }_k^4-4{\rho }_k^2\left({\rho }_k^2-R_k\right)}}{2}={\rho }_k\left({\rho }_k\pm \sqrt{R_k}\right)$$

将改部分进行分解：
$$\begin{array}{rl}{\mathbf{\Delta }}_k{\mathbf{\Delta }}_k^{\mathrm{T}}& ={\mathbf{\Delta }}_{k/k-1}\left(\mathbf{I}-{\rho }_k^{-2}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right){\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}\\ & ={\mathbf{\Delta }}_{k/k-1}\left(\mathbf{I}-{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right){\left(\mathbf{I}-{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right)}^{\mathrm{T}}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}\\ & =\left[{\mathbf{\Delta }}_{k/k-1}\left(\mathbf{I}-{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right)\right]{\left[{\mathbf{\Delta }}_{k/k-1}\left(\mathbf{I}-{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right)\right]}^{\mathrm{T}}\end{array}$$
得到方差阵的分解：
$${\mathbf{\Delta }}_k={\mathbf{U}}_{k/k-1}\left(\mathbf{I}-{\gamma }_k^{-1}{\mathbf{\Delta }}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{H}}_k{\mathbf{\Delta }}_{k/k-1}\right)$$
实现方差阵的量测更新： ${\mathbf{\Delta }}_{k/k-1},{\mathbf{R}}_k^{1/2}⟶{\mathbf{\Delta }}_k$

2、方差阵的时间更新

由标准Kalman滤波的时间更新：
$${\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}$$
写成分解形式：
Δ k / k − 1 Δ k / k − 1 T = Φ k / k − 1 Δ k − 1 Δ k − 1 T Φ k / k − 1 T + Γ k − 1 Q k − 1 1 2 ( Q k − 1 1 2 ) T Γ k − 1 T = [ Φ k / k − 1 Δ k − 1 Γ k − 1 Q k − 1 1 2 ] [ Δ k − 1 T Φ k / k − 1 T ( Q k − 1 1 2 ) T Γ k − 1 T ] \begin{array}{c}\boldsymbol{\Delta}_{k / k-1} \boldsymbol{\Delta}_{k / k-1}^{\mathrm{T}}=\boldsymbol{\Phi}_{k / k-1} \boldsymbol{\Delta}_{k-1} \boldsymbol{\Delta}_{k-1}^{\mathrm{T}} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1}^{\frac{1}{2}}\left(\boldsymbol{Q}_{k-1}^{\frac{1}{2}}\right)^{\mathrm{T}} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \\ =\left[\begin{array}{ll}\boldsymbol{\Phi}_{k / k-1} \boldsymbol{\Delta}_{k-1} & \boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1}^{\frac{1}{2}}\end{array}\right]\left[\begin{array}{c}\boldsymbol{\Delta}_{k-1}^{\mathrm{T}} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}} \\ \left(\boldsymbol{Q}_{k-1}^{\frac{1}{2}}\right)^{\mathrm{T}} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\end{array}\right]\end{array} Δk/k−1​Δk/k−1T​=Φk/k−1​Δk−1​Δk−1T​Φk/k−1T​+Γk−1​Qk−121​​(Qk−121​​)TΓk−1T​=[Φk/k−1​Δk−1​​Γk−1​Qk−121​​​] ​Δk−1T​Φk/k−1T​(Qk−121​​)TΓk−1T​​ ​​

不能直接将 ${\mathbf{\Phi }}_{k/k-1}{\mathbf{\Delta }}_{k-1}{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}^{\frac{1}{2}}$ 作为 ${\mathbf{\Delta }}_{k/k-1}$ ，因为 $A{A}^T=B{B}^T$ 时，不能说 $A=B$

QR分解：列满秩矩阵 $A_{m\ast n}$ 总可以做QR分解：${\mathbf{A}}_{m\times n}={\hat{\mathbf{Q}}}_{m\times n}{\hat{\mathbf{R}}}_{n\times n}$ 且有 ${\hat{\mathbf{Q}}}_{m\times n}^{\mathrm{T}}{\hat{\mathbf{Q}}}_{m\times n}=\mathbf{I}$ ，${\hat{\mathbf{R}}}_{n\times n}$ 是上三角可逆。

对上式QR分解：
$${\left({\hat{\mathbf{Q}}}_{2n\times n}{\hat{\mathbf{R}}}_{n\times n}\right)}^{\mathrm{T}}\left({\hat{\mathbf{Q}}}_{2n\times n}{\hat{\mathbf{R}}}_{n\times n}\right)={\mathbf{R}}_{n\times n}^{\mathrm{T}}{\hat{\mathbf{R}}}_{n\times n}$$

QR分解的改进：施密特正交化法，伪代码如下：

核心就是每个新的矢量都减去它在已经正交化的矢量方向的投影，进而每次新增一个新的正交矢量。新的矢量只和之前的矢量有关，而与后面的矢量无关。

先把 [ Δ k − 1 T Φ k / k − 1 T ( Q k − 1 1 2 ) T Γ k − 1 T ] \left[\begin{array}{c}\boldsymbol{\Delta}_{k-1}^{\mathrm{T}} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}} \\ \left(\boldsymbol{Q}_{k-1}^{\frac{1}{2}}\right)^{\mathrm{T}} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\end{array}\right] ​Δk−1T​Φk/k−1T​(Qk−121​​)TΓk−1T​​ ​ 求出来，再用Gram-Schmidt法即可得到方差阵的时间更新 ${\mathbf{\Delta }}_{k/k-1}$

3、Potter平方根滤波流程

4、向量量测情况下的方差阵量测更新

改量测更新就行，时间更新没必要改

前述标量量测情形

同理，向量量测情形：

全流程：

• 时间更新：${\Delta }_{k-1}\stackrel{\mathrm{QR}}{⟶}{\Delta }_{k/k-1}$ ，要做一次QR分解
• 量测更新：${\mathbf{\Delta }}_{k/k-1}\stackrel{\mathrm{QR}}{⟶}{\gamma }_k\stackrel{\text{ 求逆 }}{⟶}{\mathbf{\Delta }}_k$ ，也要做一次QR分解（其实前面标量的开方也是QR分解）

三、奇异值（SVD）分解滤波

奇异值分解可以参考博客

$M=US{V}^T$，其中 $U$、$V$ 是正交矩阵，$S$ 是由 $M$ 的奇异值构成的对角矩阵。我们用奇异值分解的是方差阵，它对称正定。对称所以 $U$ 和 $V$ 相等，正定所以奇异值都大于 $0$

朴素分解方式：

上式每次更新要做两次QR分解，两次三角阵求逆；考虑用SVD分解，这样就只用做对角阵求逆。

1、时间更新方差方程的SVD分解

将方程的 $P_{k/k-1}$ 分解为 ${\mathbf{U}}_{k/k-1}{\mathbf{\Lambda }}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}$ ，$P_{k-1}$ 分解为 ${\mathbf{U}}_{k-1}{\mathbf{\Lambda }}_{k-1}{\mathbf{U}}_{k-1}^{\mathrm{T}}$ ，选择合适的 $\mathbf{\Gamma }$ 噪声系数分配矩阵，可以认为 $Q$ 是对角阵。
l U k / k − 1 Λ k / k − 1 U k / k − 1 T = Φ k / k − 1 U k − 1 Λ k − 1 U k − 1 T Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T = [ Φ k / k − 1 U k − 1 Λ k − 1 1 2 Γ k − 1 Q k − 1 1 2 ] [ ( Λ k − 1 1 2 ) T U k − 1 T Φ k / k − 1 T ( Q k − 1 1 2 ) T Γ k − 1 T ] \begin{aligned}{l} \boldsymbol{U}_{k / k-1} {\color{brown}\boldsymbol{\Lambda}_{k / k-1}} \boldsymbol{U}_{k / k-1}^{\mathrm{T}} & =\boldsymbol{\Phi}_{k / k-1} \boldsymbol{U}_{k-1} \boldsymbol{\Lambda}_{k-1} \boldsymbol{U}_{k-1}^{\mathrm{T}} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \\ & =\left[\begin{array}{lll}\boldsymbol{\Phi}_{k / k-1} \boldsymbol{U}_{k-1} \boldsymbol{\Lambda}_{k-1}^{\frac{1}{2}} & \boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1}^{\frac{1}{2}}\end{array}\right]\left[\begin{array}{c}\left(\boldsymbol{\Lambda}_{k-1}^{\frac{1}{2}}\right)^{\mathrm{T}} \boldsymbol{U}_{k-1}^{\mathrm{T}} \boldsymbol{\Phi}_{k / k-1}^{\mathrm{T}} \\ \left(\boldsymbol{Q}_{k-1}^{\frac{1}{2}}\right)^{\mathrm{T}} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}}\end{array}\right] \end{aligned} lUk/k−1​Λk/k−1​Uk/k−1T​​=Φk/k−1​Uk−1​Λk−1​Uk−1T​Φk/k−1T​+Γk−1​Qk−1​Γk−1T​=[Φk/k−1​Uk−1​Λk−121​​​Γk−1​Qk−121​​​] ​(Λk−121​​)TUk−1T​Φk/k−1T​(Qk−121​​)TΓk−1T​​ ​​
再对矩阵做奇异值分解，得：
$$\begin{array}{l}=\left({\mathbf{S}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{V}}_{k/k-1}^{\mathrm{T}}\right){\left({\mathbf{S}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{V}}_{k/k-1}^{\mathrm{T}}\right)}^{\mathrm{T}}\\ ={\mathbf{S}}_{k/k-1}{{\mathbf{D}}_{k/k-1}{\mathbf{D}}_{k/k-1}}^{\mathrm{T}}{\mathbf{S}}_{k/k-1}^{\mathrm{T}}\end{array}$$
此式与原式形式一致，令：
$${\mathbf{U}}_{k/k-1}={\mathbf{S}}_{k/k-1},{\mathbf{\Lambda }}_{k/k-1}^{\frac{1}{2}}={\mathbf{D}}_{k/k-1}$$

2、量测更新方差方程的SVD分解

同样，将方程的 $P_{k/k-1}$ 分解为 ${\mathbf{U}}_{k/k-1}{\mathbf{\Lambda }}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}$ ，$P_k$ 分解为 ${\mathbf{U}}_k{\mathbf{\Lambda }}_k{\mathbf{U}}_k^{\mathrm{T}}$ ，得：
U k Λ k − 1 U k T = U k / k − 1 Λ k / k − 1 − 1 U k / k − 1 T + H k T R k − 1 H k = [ U k / k − 1 Λ k / k − 1 − 1 2 H k T R k − 1 2 ] [ ( Λ k / k − 1 − 1 2 ) T U k / k − 1 T ( R k − 1 2 ) T H k ] \boldsymbol{U}_{k} \boldsymbol{\Lambda}_{k}^{-1} \boldsymbol{U}_{k}^{\mathrm{T}} =\boldsymbol{U}_{k / k-1} \boldsymbol{\Lambda}_{k / k-1}^{-1} \boldsymbol{U}_{k / k-1}^{\mathrm{T}}+\boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{H}_{k} \\ =\left[\begin{array}{ll}\boldsymbol{U}_{k / k-1} \boldsymbol{\Lambda}_{k / k-1}^{-\frac{1}{2}} & \boldsymbol{H}_{k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-\frac{1}{2}}\end{array}\right]\left[\begin{array}{c}\left(\boldsymbol{\Lambda}_{k / k-1}^{-\frac{1}{2}}\right)^{\mathrm{T}} \boldsymbol{U}_{k / k-1}^{\mathrm{T}} \\ \left(\boldsymbol{R}_{k}^{-\frac{1}{2}}\right)^{\mathrm{T}} \boldsymbol{H}_{k}\end{array}\right] Uk​Λk−1​UkT​=Uk/k−1​Λk/k−1−1​Uk/k−1T​+HkT​Rk−1​Hk​=[Uk/k−1​Λk/k−1−21​​​HkT​Rk−21​​​] ​(Λk/k−1−21​​)TUk/k−1T​(Rk−21​​)THk​​ ​
对矩阵做SVD分解，得：
$$\begin{array}{l}=\left({\mathbf{S}}_k{\mathbf{D}}_k{\mathbf{V}}_k^{\mathrm{T}}\right){\left({\mathbf{S}}_k{\mathbf{D}}_k{\mathbf{V}}_k^{\mathrm{T}}\right)}^{\mathrm{T}}={\mathbf{S}}_k{\mathbf{D}}_k{\mathbf{D}}_k^{\mathrm{T}}{\mathbf{S}}_k^{\mathrm{T}}\end{array}$$
也令：
$${\mathbf{U}}_k={\mathbf{S}}_k,{\mathbf{\Lambda }}_k^{-\frac{1}{2}}={\mathbf{D}}_k$$

3、SVD分解滤波流程

$$\begin{array}{l}\left({\mathbf{U}}_{k-1},{\mathbf{\Lambda }}_{k-1}^{\frac{1}{2}},{\mathbf{Q}}_{k-1}^{\frac{1}{2}}\right)\to \left[{\mathbf{\Phi }}_{k/k-1}{\mathbf{U}}_{k-1}{\mathbf{\Lambda }}_{k-1}^{\frac{1}{2}}{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}^{\frac{1}{2}}\right]\stackrel{\mathrm{SVD}}{⟶}\left({\mathbf{U}}_{k/k-1},{\mathbf{\Lambda }}_{k/k-1}^{\frac{1}{2}}\right)\\ \to \left({\mathbf{U}}_{k/k-1},{\mathbf{\Lambda }}_{k/k-1}^{-\frac{1}{2}},{\mathbf{R}}_k^{-\frac{1}{2}}\right)\to \left[{\mathbf{U}}_{k/k-1}{\mathbf{\Lambda }}_{k/k-1}^{-\frac{1}{2}}{\mathbf{H}}_k^{\mathrm{T}}{\mathbf{R}}_k^{-\frac{1}{2}}\right]\stackrel{\mathrm{SVD}}{⟶}\left({\mathbf{U}}_k,{\mathbf{\Lambda }}_k^{-\frac{1}{2}}\right)\to \cdots \end{array}$$

每次更新含2次SVD分解、2次对角阵求逆，运算量与“朴素”方法相比没有明显优势（SVD分解计算量远大于QR），没啥用。

四、UD分解滤波

UD分解：$M=UD{U}^T$，$U$ 为上三角且对角线元素为 $1$，$D$ 为对角阵。存储的时候可以让 $U$ 占据上三角（对角线都是1），对角线上都为 $D$ 。

1、量测更新方差方程UD分解

必须是标量量测，向量的还没有人推导过，只能改成标量再分解

$${\mathbf{P}}_k={\mathbf{P}}_{k/k-1}-{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}+R_k\right)}^{-1}{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}$$

将方程的 $P_{k/k-1}$ 分解为 ${\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}$ ，$P_k$ 分解为 ${\mathbf{U}}_k{\mathbf{D}}_k{\mathbf{U}}_k^{\mathrm{T}}$，$R$ 为标量可以前后移动，得：
$$\begin{array}{l}{\mathbf{U}}_k{\mathbf{D}}_k{\mathbf{U}}_k^{\mathrm{T}}={\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}-\\ {\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}+R_k\right)}^{-1}{\mathbf{H}}_k{\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}\\ ={\mathbf{U}}_{k/k-1}\left[{\mathbf{D}}_{k/k-1}-{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}{\left({\mathbf{H}}_k{\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}+R_k\right)}^{-1}{\mathbf{H}}_k{\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}\right]{\mathbf{U}}_{k/k-1}^{\mathrm{T}}\\ ={\mathbf{U}}_{k/k-1}\left({\mathbf{D}}_{k/k-1}-{\alpha }^{-1}\mathbf{g}{\mathbf{g}}^{\mathrm{T}}\right){\mathbf{U}}_{k/k-1}^{\mathrm{T}}\\ \text{ 记 }\{\begin{array}{l}\alpha ={\mathbf{H}}_k{\mathbf{U}}_{k/k-1}\cdot {\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}+R_k={\mathbf{f}}^{\mathrm{T}}\mathbf{g}+R_k\\ \mathbf{f}={\left({\mathbf{H}}_k{\mathbf{U}}_{k/k-1}\right)}^{\mathrm{T}}\\ \mathbf{g}={\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}{\mathbf{H}}_k^{\mathrm{T}}={\mathbf{D}}_{k/k-1}\mathbf{f}\end{array}\end{array}$$

从 $D_{nn}$ 开始先计算最后一列 ，再计算倒数第二列，直到 $D_{11}$ 。

2、时间更新方差方程UD分解

$${\mathbf{P}}_{k/k-1}={\mathbf{\Phi }}_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}$$

将方程的 $P_{k/k-1}$ 分解为 ${\mathbf{U}}_{k/k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}$ ，$P_{k-1}$ 分解为 ${\mathbf{U}}_{k-1}{\mathbf{D}}_{k-1}{\mathbf{U}}_{k-1}^{\mathrm{T}}$ ，最后两边的矩阵记为 ${\mathbf{W}}_{k\mid k-1}$
$$\begin{array}{rl}{\mathbf{U}}_{k\mid k-1}{\mathbf{D}}_{k/k-1}{\mathbf{U}}_{k/k-1}^{\mathrm{T}}& ={\mathbf{\Phi }}_{klk-1}{\mathbf{U}}_{k-1}{\mathbf{D}}_{k-1}{\mathbf{U}}_{k-1}^{\mathrm{T}}{\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\\ & =\left[\begin{array}{ll}{\mathbf{\Phi }}_{klk-1}{\mathbf{U}}_{k-1}& {\mathbf{\Gamma }}_{k-1}\end{array}\right]\left[\begin{array}{cc}{\mathbf{D}}_{k-1}& 0\\ 0& {\mathbf{Q}}_{k-1}\end{array}\right]\left[\begin{array}{c}{\mathbf{U}}_{k-1}^{\mathrm{T}}{\mathbf{\Phi }}_{klk-1}^{\mathrm{T}}\\ {\mathbf{\Gamma }}_{k-1}^{\mathrm{T}}\end{array}\right]\triangleq {\mathbf{W}}_{k\mid k-1}{\tilde{\mathbf{D}}}_{k-1}{\mathbf{W}}_{k\mid k-1}^{\mathrm{T}}\end{array}$$

1.朴素算法

对 ${\mathbf{W}}_{k\mid k-1}$ 再做一次QR分解：
$${\mathbf{W}}_{klk-1}{\tilde{\mathbf{D}}}_{k-1}{\mathbf{W}}_{k/k-1}^{\mathrm{T}}={\hat{\mathbf{R}}}^{\mathrm{T}}{\hat{\mathbf{Q}}}^{\mathrm{T}}{\tilde{\mathbf{D}}}_{k-1}\mathbf{Q}\hat{\mathbf{R}}={\hat{\mathbf{R}}}^{\mathrm{T}}\mathbf{A}\hat{\mathbf{R}}$$
绿色部分正定对称，记为 $A$ ，对其进行UD分解：
$$={\hat{\mathbf{R}}}^{\mathrm{T}}\hat{\mathbf{U}}\hat{\mathbf{D}}{\hat{\mathbf{U}}}^{\mathrm{T}}\overline{\mathbf{R}}=\left({\hat{\mathbf{R}}}^{\mathrm{T}}\hat{\mathbf{U}}\right)\hat{\mathbf{D}}{\left({\hat{\mathbf{R}}}^{\mathrm{T}}\hat{\mathbf{U}}\right)}^{\mathrm{T}}$$
$R$ 是QR分解出的三角阵，$U$ 是UD分解出的三角阵，乘出来的 $R^{T}U$ 还是三角阵，还有保证对角线是 $1$，把这部分赋给 ${\mathbf{U}}_{k\mid k-1}$ ，不等于 $1$ 的部分归到 $D$ 里面。此方法要一次QR分解和一个UD分解，计算量大。

2.快速算法

用左边和右边元素一一对应得：
$$D_{k\mid k-1,jj}=\sum _{s=1}^{n+1}{\tilde{D}}_{k-1,ss}{W}_{j,s}^{(n-j)}{W}_{j,s}^{(n-j)}{U}_{klk-1,ij}=\frac{{\sum }_{s=1}^{n+1}{\tilde{D}}_{k-1,ss}{W}_{i,s}^{(n-j)}{W}_{j,s}^{(n-j)}}{D_{k/k-1,jj}}$$

滤波流程： $\left(U_{k-1,j,},D_{k-1,j}\right)\to \left(U_{k\mid k-1,jj},D_{klk-1,j}\right)\to \left(U_{k,j},D_{k,j,j}\right)$

运算量比较小，计算比较紧凑，已经推导好了一个一个元素怎么计算，比较实用。

五、平方根信息滤波SRIKF

将信息矩阵 $I$ 分解，与Potter平方根滤波很相似，计算量几乎一模一样，可以类比来看。