UD(unit upper traingular& diagonal factorization)分解滤波
UD(unit upper traingular& diagonal factorization)分解
P : n 阶 正 定 的 对 称 矩 阵 P:n阶正定的对称矩阵 P:n阶正定的对称矩阵
对矩阵P进行上三角-对角分解:
U : 上 三 角 阵 U:上三角阵 U:上三角阵
D : 对 角 阵 D:对角阵 D:对角阵
P = U D U T P = [ P 11 P 12 . . . P 1 n P 21 P 22 . . . P 2 n . . . . . . . . . . . . P n 1 P n 2 . . . P n n ] , U = [ U 11 U 12 . . . U 1 n U 21 U 22 . . . U 2 n . . . . . . . . . . . . U n 1 U n 2 . . . U n n ] ( U i i = 1 , ( i = 1 , 2 , 3 , . . , n ) ) , D = [ D 11 D 12 . . . D 1 n D 21 D 22 . . . D 2 n . . . . . . . . . . . . D n 1 D n 2 . . . D n n ] P=UDU^T \\ P=\left[\begin{matrix} P_{11}&P_{12}&...&P_{1n}\\ P_{21}&P_{22}&...&P_{2n}\\ ...&...&...&...\\ P_{n1}&P_{n2}&...&P_{nn}\\ \end{matrix}\right],U=\left[\begin{matrix} U_{11}&U_{12}&...&U_{1n}\\ U_{21}&U_{22}&...&U_{2n}\\ ...&...&...&...\\ U_{n1}&U_{n2}&...&U_{nn}\\ \end{matrix}\right](U_{ii}=1,(i=1,2,3,..,n)),D=\left[\begin{matrix} D_{11}&D_{12}&...&D_{1n}\\ D_{21}&D_{22}&...&D_{2n}\\ ...&...&...&...\\ D_{n1}&D_{n2}&...&D_{nn}\\ \end{matrix}\right] P=UDUTP=⎣⎢⎢⎡P11P21...Pn1P12P22...Pn2............P1nP2n...Pnn⎦⎥⎥⎤,U=⎣⎢⎢⎡U11U21...Un1U12U22...Un2............U1nU2n...Unn⎦⎥⎥⎤(Uii=1,(i=1,2,3,..,n)),D=⎣⎢⎢⎡D11D21...Dn1D12D22...Dn2............D1nD2n...Dnn⎦⎥⎥⎤
可以得到：
P i j = D j j U i j U j j + D j + 1 , j + 1 U i , j + 1 U j , j + 1 + . . . + D n n U i n U j n = ∑ k = i + j n D k k U i k U j k + D j j U i j U j j ( 1 ≤ i ≤ n , i ≤ j ≤ n ) 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}^nD_{kk}U_{ik}U_{jk}+D_{jj}U_{ij}U_{jj}(1\leq i \leq n,i\leq j \leq n) Pij=DjjUijUjj+Dj+1,j+1Ui,j+1Uj,j+1+...+DnnUinUjn=k=i+j∑nDkkUikUjk+DjjUijUjj(1≤i≤n,i≤j≤n)
从而得到上三角阵和对角阵各元素计算规则如下：
U i j = { ( P i j − ∑ k = j + 1 n D k k U i k U j k ) / D j j i < j 1 i = j 0 i > j U_{ij}=\begin{cases} (P_{ij}-\sum_{k=j+1}^{n}D_{kk}U_{ik}U_{jk})/D_{jj}&i<j\\ 1&i=j\\ 0&i>j\\ \end{cases} Uij=⎩⎪⎨⎪⎧(Pij−∑k=j+1nDkkUikUjk)/Djj10i<ji=ji>j
UD与乔莱斯三角分解存在如下关系
Δ = U D \Delta=U\sqrt{D} Δ=UD
P = U D U T = U ( D D T ) U T = ( U D ) ( U D ) T = Δ Δ T P=UDU^T=U(\sqrt{D}\sqrt{D}^T)U^T=(U\sqrt{D})(U\sqrt{D})^T=\Delta \Delta^T P=UDUT=U(DDT)UT=(UD)(UD)T=ΔΔT
UD分解滤波
系统状态空间
X k : n 维 状 态 向 量 X_k:n维状态向量 Xk:n维状态向量
Z k : m 维 测 量 向 量 Z_k:m维测量向量 Zk:m维测量向量
Φ k / k − 1 : 已 知 的 系 统 结 构 参 数 \Phi_{k/k-1}:已知的系统结构参数 Φk/k−1:已知的系统结构参数
Γ k / k − 1 : 已 知 的 系 统 结 构 参 数 ， 分 别 为 n × l 阶 系 统 分 配 噪 声 \Gamma_{k/k-1}:已知的系统结构参数，分别为n×l阶系统分配噪声 Γk/k−1:已知的系统结构参数，分别为n×l阶系统分配噪声
H k : 已 知 的 系 统 结 构 参 数 ， 分 别 为 m × n 阶 测 量 矩 阵 H_k:已知的系统结构参数，分别为m×n阶测量矩阵 Hk:已知的系统结构参数，分别为m×n阶测量矩阵
V k : m 维 测 量 噪 声 ， 高 斯 白 噪 声 ， 服 从 正 太 分 布 V_k:m维测量噪声，高斯白噪声，服从正太分布 Vk:m维测量噪声，高斯白噪声，服从正太分布
W k − 1 : m 维 系 统 噪 声 向 量 ， 高 斯 白 噪 声 ， 服 从 正 太 分 布 W_{k-1}:m维系统噪声向量，高斯白噪声，服从正太分布 Wk−1:m维系统噪声向量，高斯白噪声，服从正太分布
V k 与 W k − 1 互 不 相 关 V_k与W_{k-1}互不相关 Vk与Wk−1互不相关
{ X k = Φ k / k − 1 X k − 1 + Γ k / k − 1 W k − 1 Z k = H k X k + V k s t . { E [ W k ] = 0 , E [ W k W j T ] = Q k δ k j Q k ≥ 0 E [ V k ] = 0 , E [ V k V j T ] = R k δ k j , E [ W k V j T ] = 0 R ≥ 0 \begin{cases} X_k=\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1}\\ Z_k=H_kX_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W_k]=0,E[W_kW_j^T]=Q_k\delta_{kj} &Q_k \geq 0\\ E[V_k]=0,E[V_kV_j^T]=R_k\delta_{kj},E[W_kV_j^T]=0&R\geq 0\\ \end{cases} {Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0
kalman滤波
{ X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 状 态 一 步 预 测 P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T 状 态 一 步 预 测 均 方 差 阵 K k = P k / k − 1 H k T ( H k P k / k − 1 H k T − R k ) − 1 滤 波 增 益 X ^ k = ( I − K k H k ) X ^ k / k − 1 + K k Z k 状 态 估 计 P k = ( I − K k H k ) P k / k − 1 状 态 估 计 均 方 误 差 阵 \begin{cases} \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^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T&状态一步预测均方差阵\\ K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T-R_k)^{-1}&滤波增益\\ \hat X_k=(I-K_kH_k)\hat X_{k/k-1}+K_kZ_k&状态估计\\ P_k=(I-K_kH_k)P_{k/k-1}&状态估计均方误差阵\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1=Φk/k−1X^k−1Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1TKk=Pk/k−1HkT(HkPk/k−1HkT−Rk)−1X^k=(I−KkHk)X^k/k−1+KkZkPk=(I−KkHk)Pk/k−1状态一步预测状态一步预测均方差阵滤波增益状态估计状态估计均方误差阵
滤波增益带入状态估计均方误差阵
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 P_k=(I-P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T-R_k)^{-1}H_k)P_{k/k-1} Pk=(I−Pk/k−1HkT(HkPk/k−1HkT−Rk)−1Hk)Pk/k−1
UD分解滤波
对非负定的均方误差阵进行UD分解如下：
U : 上 三 角 阵 , D : 斜 对 角 阵 U:上三角阵,D:斜对角阵 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 P_k=U_kD_kU_k^T,P_{k/k-1}=U_{k/k-1}D_{k/k-1}U_{k/k-1}^T Pk=UkDkUkT,Pk/k−1=Uk/k−1Dk/k−1Uk/k−1T
UD分解分解后的方程带入 P k P_k Pk
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 − α − 1 g g T ) U k / k − 1 T s t : { α = 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 P_k=U_kD_kU_k^T=(I-U_{k/k-1}D_{k/k-1}U_{k/k-1}^TH_k^T(H_kU_{k/k-1}D_{k/k-1}U_{k/k-1}^TH_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}^TH_k^T(H_kU_{k/k-1}D_{k/k-1}U_{k/k-1}^TH_k^T+R_k)^{-1}H_kU_{k/k-1}D_{k/k-1}]U_{k/k-1}^T \\ =U_{k/k-1}(D_{k/k-1}-\alpha^{-1}gg^T)U_{k/k-1}^T \\ st: \\ \begin{cases} \alpha=H_kU_{k/k-1}D_{k/k-1}U^T_{k/k-1}H_k^T+R_k=f^Tg+R_k\\ f=(H_kU_{k/k-1})^T\\ g=D_{k/k-1}U^T_{k/k-1}H_k^T=D_{k/k-1}f \end{cases} Pk=UkDkUkT=(I−Uk/k−1Dk/k−1Uk/k−1THkT(HkUk/k−1Dk/k−1Uk/k−1THkT−Rk)−1Uk/k−1Dk/k−1Uk/k−1T=Uk/k−1[Dk/k−1−Dk/k−1Uk/k−1THkT(HkUk/k−1Dk/k−1Uk/k−1THkT+Rk)−1HkUk/k−1Dk/k−1]Uk/k−1T=Uk/k−1(Dk/k−1−α−1ggT)Uk/k−1Tst:⎩⎪⎨⎪⎧α=HkUk/k−1Dk/k−1Uk/k−1THkT+Rk=fTg+Rkf=(HkUk/k−1)Tg=Dk/k−1Uk/k−1THkT=Dk/k−1f
当量测矩阵为标量的情况
可以通过比较等式两端不同，得到 U k , D k U_k,D_k Uk,Dk的直接计算公式：
f j ; f 的 第 j 个 分 量 , n 维 列 向 量 f_j;f的第j个分量,n维列向量 fj;f的第j个分量,n维列向量
g j : g 的 第 j 个 分 量 , n 维 列 向 量 g_j:g的第j个分量,n维列向量 gj:g的第j个分量,n维列向量
a a a为标量, a n = α a_n=\alpha an=α
{ D k , j j = a j − 1 / a j ∗ D k / k − 1 , j j U k , i j = U k / k − 1 , i j + λ j ( g i + ∑ s = i + 1 j − 1 U k / k − 1 , i s ∗ g s ) λ j = − 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 \begin{cases} D_{k,jj}=a_{j-1}/a_j*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} *g_s)\\ \lambda_j=-\frac{f_j}{a_{j-1}}\\ a_{j-1}=a_j-f_jg_j(j=n,n-1,...,1;i=1,2,...,j-1)&当R_k>0时，总有a>0\\ \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧Dk,jj=aj−1/aj∗Dk/k−1,jjUk,ij=Uk/k−1,ij+λj(gi+∑s=i+1j−1Uk/k−1,is∗gs)λj=−aj−1fjaj−1=aj−fjgj(j=n,n−1,...,1;i=1,2,...,j−1)当Rk>0时，总有a>0
均方误差阵的时间更新
P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T P_{k/k-1}=\Phi_{k/k-1}P_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1T
进行UD分解滤波
W k / k − 1 = [ Φ k / k − 1 U k − 1 Γ k − 1 ] W_{k/k-1}=\left[\begin{matrix} \Phi_{k/k-1}U_{k-1}& \Gamma_{k-1}\\ \end{matrix}\right] Wk/k−1=[Φk/k−1Uk−1Γk−1]
D ‾ = [ D k − 1 0 0 Q k − 1 ] \overline D=\left[\begin{matrix} D_{k-1}& 0\\0&Q_{k-1} \end{matrix}\right] D=[Dk−100Qk−1]
P k / k − 1 = U k / k − 1 D k / k − 1 U k / k − 1 T = Φ k / k − 1 U k − 1 D 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 ] [ D k − 1 0 0 Q k − 1 ] [ U k − 1 T Φ k / k − 1 T Γ k − 1 T ] = W k / k − 1 D ‾ k − 1 W k / k − 1 T 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^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T \\ =\left[\begin{matrix} \Phi_{k/k-1}U_{k-1}& \Gamma_{k-1}\\ \end{matrix}\right]\left[\begin{matrix} D_{k-1}& 0\\0&Q_{k-1} \end{matrix}\right]\left[\begin{matrix} U_{k-1}^T\Phi_{k/k-1}^T\\ \Gamma_{k-1}^T\\ \end{matrix}\right] \\ =W_{k/k-1}\overline D_{k-1}W_{k/k-1}^T Pk/k−1=Uk/k−1Dk/k−1Uk/k−1T=Φk/k−1Uk−1Dk−1Uk−1TΦk/k−1T+Γk−1Qk−1Γk−1T=[Φk/k−1Uk−1Γk−1][Dk−100Qk−1][Uk−1TΦk/k−1TΓk−1T]=Wk/k−1Dk−1Wk/k−1T
再对 W k / k − 1 T 进 行 Q R 分 解 W_{k/k-1}^T进行QR分解 Wk/k−1T进行QR分解
W k / k − 1 T = Q ‾ R ‾ ( R 为 下 三 角 阵 ) W_{k/k-1}^T=\overline Q \overline R( R为下三角阵 ) Wk/k−1T=QR(R为下三角阵)
A = Q ‾ T D ‾ k − 1 Q ‾ A=\overline Q^T \overline D_{k-1} \overline Q A=QTDk−1Q
P k / k − 1 = U k / k − 1 D k / k − 1 U k / k − 1 T = W k / k − 1 D ‾ k − 1 W k / k − 1 T = ( Q ‾ R ‾ ) T D k / k − 1 ( Q ‾ R ‾ ) = R ‾ T A R ‾ 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)^TD_{k/k-1}(\overline Q\overline R) \\ =\overline R^T A \overline R Pk/k−1=Uk/k−1Dk/k−1Uk/k−1T=Wk/k−1Dk−1Wk/k−1T=(QR)TDk/k−1(QR)=RTAR
再对 A 进 行 U D 分 解 A进行UD分解 A进行UD分解
A = U ‾ D ‾ U ‾ T A=\overline U \overline D \overline U^T A=UDUT
P k / k − 1 = R ‾ T A R ‾ = R ‾ T U ‾ D ‾ U ‾ T R ‾ = ( R ‾ T U ‾ ) D ‾ ( R ‾ T 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 Pk/k−1=RTAR=RTUDUTR=(RTU)D(RTU)T
令 U k / k − 1 = R ‾ T U ‾ , D k / k − 1 = D ‾ U_{k/k-1}=\overline R^T \overline U,D_{k/k-1}=\overline D Uk/k−1=RTU,Dk/k−1=D,也就完成了UD分解滤波
因为UD需要了两次矩阵分解和多次矩阵乘法运算，Bierman给出了直接由 W k / k − 1 W_{k/k-1} Wk/k−1和 D ‾ k − 1 \overline D_{k-1} Dk−1进行UD分解，求解 U k / k − 1 U_{k/k-1} Uk/k−1和 D k / k − 1 D_{k/k-1} Dk/k−1的方法：
{ D k / k − 1 , j j = ∑ s = 1 n + l D ‾ k − 1 , s s W j , s ( n − j ) W j , s ( n − j ) U k / k − 1 , i j = ∑ s = 1 n + l ( D ‾ k − 1 , s s W i , s n − j W j , s n − j ) D k / k − 1 , j j W i n − j + 1 = W i n − j − U k / k − 1 , i j W j n − j j = n , n − 1 , . . . , 1 ; i = 1 , 2 , . . . , j − 1 \begin{cases} 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{cases} ⎩⎪⎪⎨⎪⎪⎧Dk/k−1,jj=∑s=1n+lDk−1,ssWj,s(n−j)Wj,s(n−j)Uk/k−1,ij=Dk/k−1,jj∑s=1n+l(Dk−1,ssWi,sn−jWj,sn−j)Win−j+1=Win−j−Uk/k−1,ijWjn−jj=n,n−1,...,1;i=1,2,...,j−1
平方根信息kalman滤波（Square Root Information SRIKF）
信息滤波公式为：
{ N k − 1 = M k − 1 Γ k − 1 ( Q k − 1 − 1 + Γ k − 1 T M k − 1 Γ k − 1 ) − 1 Γ k − 1 T （ 正 定 可 逆 ） M k − 1 = Φ k / k − 1 − T I k − 1 Φ 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 S ^ k / k − 1 = ( I − N k − 1 ) Φ k / k − 1 − T S ^ k − 1 S ^ k = S ^ k / k − 1 + H k T R k − 1 Z k \begin{cases} N_{k-1}=M_{k-1}\Gamma_{k-1}(Q_{k-1}^{-1}+\Gamma_{k-1}^TM_{k-1}\Gamma_{k-1})^{-1}\Gamma_{k-1}^T（正定可逆）\\ M_{k-1}=\Phi^{-T}_{k/k-1}I_{k-1}\Phi^{-1}_{k/k-1}(非负正定的)\\ I_{k/k-1}=(I-N_{k-1})M_{k-1}\\ I_k=I_{k/k-1}+H_k^TR_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^TR_k^{-1}Z_k \\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧Nk−1=Mk−1Γk−1(Qk−1−1+Γk−1TMk−1Γk−1)−1Γk−1T（正定可逆）Mk−1=Φk/k−1−TIk−1Φk/k−1−1(非负正定的)Ik/k−1=(I−Nk−1)Mk−1Ik=Ik/k−1+HkTRk−1HkS^k/k−1=(I−Nk−1)Φk/k−1−TS^k−1S^k=S^k/k−1+HkTRk−1Zk
记信息阵 I k , I k / k − 1 I_k,I_{k/k-1} Ik,Ik/k−1的平方根分别记为：
I k = ξ k ξ k T , I k / k − 1 = ξ k / k − 1 ξ k / k − 1 T I_k=\xi_k\xi_k^T,I_{k/k-1}=\xi_{k/k-1}\xi_{k/k-1}^T Ik=ξkξkT,Ik/k−1=ξk/k−1ξk/k−1T
将平方根带入滤波方程：
I k / k − 1 = ( I − N k − 1 ) M k − 1 = Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 − Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 Γ k − 1 ( Γ k − 1 T Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 Γ k − 1 + Q k − 1 − 1 ) − 1 × Γ k − 1 T Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 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}×\Gamma_{k-1}^T\Phi_{k/k-1}^{-T}I_{k-1}\Phi_{k/k-1}^{-1} Ik/k−1=(I−Nk−1)Mk−1=Φk/k−1−TIk−1Φk/k−1−1−Φk/k−1−TIk−1Φk/k−1−1Γk−1(Γk−1TΦk/k−1−TIk−1Φk/k−1−1Γk−1+Qk−1−1)−1×Γk−1TΦk/k−1−TIk−1Φk/k−1−1类比Kalman的均方误差更新公式，可以的SRIKF的时间更新算法：
kalman平方根滤波时间更新方程：
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 Δ k = Δ k / k − 1 [ I − Δ k / k − 1 T H k T ( p k p k T + R k 1 / 2 P k T ) − 1 H k Δ k / k − 1 ] P_k=P_{k/k-1}-P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T+R_k)^{-1}H_kP_{k/k-1} \\ \Delta_k=\Delta_{k/k-1}[I-\Delta_{k/k-1}^TH_k^T(p_kp_k^T+R_k^{1/2}P_k^T)^{-1}H_k\Delta_{k/k-1}] Pk=Pk/k−1−Pk/k−1HkT(HkPk/k−1HkT+Rk)−1HkPk/k−1Δk=Δk/k−1[I−Δk/k−1THkT(pkpkT+Rk1/2PkT)−1HkΔk/k−1]
SRIKF的时间更新算法：
令: Δ k → ξ k / k − 1 ; Δ k / k − 1 → Φ k / k − 1 − T ξ k − 1 ; H k → Γ k − 1 T ; R k 1 / 2 → Q k − 1 − 1 / 2 \Delta_k \rightarrow \xi_{k/k-1};\Delta_{k/k-1}\rightarrow \Phi_{k/k-1}^{-T}\xi_{k-1};H_k \rightarrow \Gamma_{k-1}^T;R_k^{1/2}\rightarrow Q_{k-1}^{-1/2} Δk→ξk/k−1;Δk/k−1→Φk/k−1−Tξk−1;Hk→Γk−1T;Rk1/2→Qk−1−1/2
得到:
ξ k / k − 1 = Φ k / k − 1 − T ξ k − 1 [ I − ξ k − 1 T Φ k / k − 1 − 1 Γ k − 1 ( p k p k T + Q k − 1 − 1 / 2 p k T ) − 1 Γ k − 1 T Φ k / k − 1 − T ξ k − 1 ] \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_kp_k^T+Q_{k-1}^{-1/2}p_k^T)^{-1}\Gamma^{T}_{k-1}\Phi_{k/k-1}^{-T}\xi_{k-1}] ξk/k−1=Φk/k−1−Tξk−1[I−ξk−1TΦk/k−1−1Γk−1(pkpkT+Qk−1−1/2pkT)−1Γk−1TΦk/k−1−Tξk−1]
求解 p k T p_k^T pkT:
[ ξ k − 1 T Φ k / k − 1 − 1 Γ k − 1 ( Q k − 1 − 1 / 2 ) T ] → Q R → p k T \left[\begin{matrix} \xi_{k-1}^T\Phi_{k/k-1}^{-1}\Gamma_{k-1}\\(Q_{k-1}^{-1/2})^T \end{matrix}\right]\rightarrow ^{QR}\rightarrow p_k^T [ξk−1TΦk/k−1−1Γk−1(Qk−1−1/2)T]→QR→pkT
[ ξ k / k − 1 T ( R K − 1 / 2 ) T H k ] → Q R → p k T \left[\begin{matrix} \xi_{k/k-1}^T\\ (R_K^{-1/2})^TH_k \end{matrix}\right]\rightarrow ^{QR}\rightarrow p_k^T [ξk/k−1T(RK−1/2)THk]→QR→pkT
