捷联惯导系统学习6.4(平方根滤波 )
平方根滤波
针对经典Kalman滤波中，状态误差阵 P k P_k Pk是状态估计的平方，可能占有更多位数，所以使用状态误差阵 P k P_k Pk的平方根进行更新，减少数值位数和计算误差（在早期计算机中比较有校）。
Potter平方根滤波
将均方误差阵 P k P_k Pk分解为下三角阵，再求平方根
Cholesky分解
P : n 阶 正 定 对 称 矩 阵 P:n阶正定对称矩阵 P:n阶正定对称矩阵
总可以得到：
P = [ P 11 P 12 . . . P 1 n P 21 P 22 . . . P 2 n . . . . . . . . . . . . P n 1 P n 2 . . . P n n ] , Δ = [ δ 11 δ 12 . . . δ 1 n 0 δ 21 . . . δ 2 n . . . . . . . . . . . . 0 0 . . . δ n n ] 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],\Delta=\left[\begin{matrix} \delta_{11}&\delta_{12}&...&\delta_{1n}\\0&\delta_{21}&...&\delta_{2n}\\ ...&...&...&...\\0&0&...&\delta_{nn} \end{matrix}\right] P=⎣⎢⎢⎡P11P21...Pn1P12P22...Pn2............P1nP2n...Pnn⎦⎥⎥⎤,Δ=⎣⎢⎢⎡δ110...0δ12δ21...0............δ1nδ2n...δnn⎦⎥⎥⎤
P = Δ Δ T = [ δ 11 δ 12 . . . δ 1 n 0 δ 21 . . . δ 2 n . . . . . . . . . . . . 0 0 . . . δ n n ] [ δ 11 δ 12 . . . δ 1 n 0 δ 21 . . . δ 2 n . . . . . . . . . . . . 0 0 . . . δ n n ] T P=\Delta \Delta^T =\left[\begin{matrix} \delta_{11}&\delta_{12}&...&\delta_{1n}\\0&\delta_{21}&...&\delta_{2n}\\ ...&...&...&...\\0&0&...&\delta_{nn} \end{matrix}\right]\left[\begin{matrix} \delta_{11}&\delta_{12}&...&\delta_{1n}\\0&\delta_{21}&...&\delta_{2n}\\ ...&...&...&...\\0&0&...&\delta_{nn} \end{matrix}\right]^T P=ΔΔT=⎣⎢⎢⎡δ110...0δ12δ21...0............δ1nδ2n...δnn⎦⎥⎥⎤⎣⎢⎢⎡δ110...0δ12δ21...0............δ1nδ2n...δnn⎦⎥⎥⎤T
可以得到：
P i j = δ i j δ j j + δ i , j + 1 δ i , j + 1 + δ i , j + 2 δ i , j + 2 + . . . + δ i , j + n δ i , j + n = ∑ k = j + 1 n δ i k δ j k + δ i j δ j j ( 1 ≤ i ≤ n , i ≤ j ≤ n ) P_{ij}=\delta_{ij}\delta_{jj}+\delta_{i,j+1}\delta_{i,j+1}+\delta_{i,j+2}\delta_{i,j+2}+...+\delta_{i,j+n}\delta_{i,j+n}=\sum_{k=j+1}^n\delta_{ik}\delta_{jk}+\delta_{ij}\delta_{jj}(1\leq i\leq n,i\leq j \leq n) Pij=δijδjj+δi,j+1δi,j+1+δi,j+2δi,j+2+...+δi,j+nδi,j+n=k=j+1∑nδikδjk+δijδjj(1≤i≤n,i≤j≤n)
从而得到:
δ i j = { ( P i j − ∑ i + j n δ i k δ j k ) / δ j j i < j P j j − ∑ k = j + 1 n δ j k 2 i = j 0 i > j \delta_{ij}=\begin{cases} (P_{ij}-\sum_{i+j}^{n}\delta_{ik}\delta_{jk})/\delta_{jj} &i<j\\ \sqrt{P_{jj}-\sum_{k=j+1}^{n}\delta_{jk}^2}&i=j\\ 0&i>j\\ \end{cases} δij=⎩⎪⎪⎨⎪⎪⎧(Pij−∑i+jnδikδjk)/δjjPjj−∑k=j+1nδjk20i<ji=ji>j
QR分解
A m × n : 为 列 满 秩 矩 阵 （ m ≥ n ） , r a n k ( A m × n ) = n A_{m×n}:为列满秩矩阵（m\geq n）,rank(A_{m×n})=n Am×n:为列满秩矩阵（m≥n）,rank(Am×n)=n
那末必有：
A m × n = Q m × n R n × n , Q m × n T Q m × n = I n × n , R n × n 为 上 三 角 阵 或 下 三 角 阵 A_{m×n}=Q_{m×n}R_{n×n},Q_{m×n}^TQ_{m×n}=I_{n×n},R_{n×n}为上三角阵或下三角阵 Am×n=Qm×nRn×n,Qm×nTQm×n=In×n,Rn×n为上三角阵或下三角阵
均方误差的量测更新
系统状态空间
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
假设 P k − 1 , P k / k − 1 , P k P_{k-1},P_{k/k-1},P_k Pk−1,Pk/k−1,Pk的平方根分别为 Δ k − 1 , Δ k / k − 1 , Δ k \Delta_{k-1},\Delta_{k/k-1},\Delta_k Δk−1,Δk/k−1,Δk
P k − 1 = Δ k − 1 Δ k − 1 T , P k / k − 1 = Δ k / k − 1 Δ k / k − 1 T , P k = Δ k Δ k T P_{k-1}=\Delta_{k-1}\Delta_{k-1}^T,P_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^T,P_{k}=\Delta_{k}\Delta_{k}^T Pk−1=Δk−1Δk−1T,Pk/k−1=Δk/k−1Δk/k−1T,Pk=ΔkΔkT
带入 P k P_k Pk
Δ k Δ k T = P k = Δ k / k − 1 [ I − Δ k / k − 1 T H k T ( H k Δ k / k − 1 Δ k / k − 1 T H k T + R k ) − 1 H k Δ k / k − 1 ] Δ k / k − 1 T = Δ k / k − 1 ( I − ρ k − 2 Δ k / k − 1 T H k T H k Δ k / k − 1 ) Δ k / k − 1 T s t : ρ k 2 = H k Δ k / k − 1 Δ k / k − 1 T H k T + R k \Delta_{k}\Delta_{k}^T=P_k=\Delta_{k/k-1}[I-\Delta_{k/k-1}^TH_k^T(H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k)^{-1}H_k\Delta_{k/k-1}]\Delta_{k/k-1}^T \\ =\Delta_{k/k-1}(I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})\Delta_{k/k-1}^T \\ st:\\ \rho_k^2=H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k ΔkΔkT=Pk=Δk/k−1[I−Δk/k−1THkT(HkΔk/k−1Δk/k−1THkT+Rk)−1HkΔk/k−1]Δk/k−1T=Δk/k−1(I−ρk−2Δk/k−1THkTHkΔk/k−1)Δk/k−1Tst:ρk2=HkΔk/k−1Δk/k−1THkT+Rk
求： P k P_k Pk的根等价于 I − ρ k − 2 Δ k / k − 1 T H k T H k Δ k / k − 1 I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1} I−ρk−2Δk/k−1THkTHkΔk/k−1的根
I − ρ k − 2 Δ k / k − 1 T H k T H k Δ k / k − 1 = ( I − r k − 1 Δ k / k − 1 T H k T H k Δ k / k − 1 ) ( I + r k − 1 Δ k / k − 1 T H k T H k Δ k / k − 1 ) I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}=(I-r_k^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})(I+r_k^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}) I−ρk−2Δk/k−1THkTHkΔk/k−1=(I−rk−1Δk/k−1THkTHkΔk/k−1)(I+rk−1Δk/k−1THkTHkΔk/k−1)
利用待定系数法求得：
r k = ρ k ( ρ k ± R k ) = 2 ρ k 2 ± 4 ρ k 4 − 4 ρ k 2 ( ρ k 2 − R k ) 2 r_k=\rho_k(\rho_k \pm\sqrt{R_k})=\frac{2\rho_k^2\pm\sqrt{4\rho_k^4-4\rho_k^2(\rho_k^2-R_k)}}{2} rk=ρk(ρk±Rk)=22ρk2±4ρk4−4ρk2(ρk2−Rk)
Δ k Δ k T = Δ k / k − 1 ( I − ρ k − 2 Δ k / k − 1 T H k T H k Δ k / k − 1 ) Δ k / k − 1 T = ( Δ k / k − 1 ( I − r k − 1 Δ k / k − 1 T H k T H k Δ k / k − 1 ) ) ( Δ k / k − 1 ( I − r k − 1 Δ k / k − 1 T H k T H k Δ k / k − 1 ) ) T \Delta_{k}\Delta_{k}^T=\Delta_{k/k-1}(I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})\Delta_{k/k-1}^T\\ =(\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}))(\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}))^T ΔkΔkT=Δk/k−1(I−ρk−2Δk/k−1THkTHkΔk/k−1)Δk/k−1T=(Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1))(Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1))T
Δ k = Δ k / k − 1 ( I − r k − 1 Δ k / k − 1 T H k T H k Δ k / k − 1 ) ( Δ k 计 算 之 后 一 般 不 为 三 角 阵 ) \Delta _k=\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})(\Delta _k 计算之后一般不为三角阵) Δk=Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1)(Δk计算之后一般不为三角阵)
均方误差的时间更新
P k − 1 = Δ k − 1 Δ k − 1 T , P k / k − 1 = Δ k / k − 1 Δ k / k − 1 T , P k = Δ k Δ k T P_{k-1}=\Delta_{k-1}\Delta_{k-1}^T,P_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^T,P_{k}=\Delta_{k}\Delta_{k}^T Pk−1=Δk−1Δk−1T,Pk/k−1=Δk/k−1Δk/k−1T,Pk=ΔkΔkT
带入 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
Q k − 1 1 / 2 ( Q k − 1 1 / 2 ) T = Q k − 1 ( c h o l e s k y 分 解 法 求 得 ) Q_{k-1}^{1/2}(Q_{k-1}^{1/2})^T=Q_{k-1}(cholesky 分解法求得) Qk−11/2(Qk−11/2)T=Qk−1(cholesky分解法求得)
P k / k − 1 = Δ 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 ] P_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^T=\Phi_{k/k-1}\Delta_{k-1}\Delta_{k-1}^T\Phi^T_{k/k-1}+\Gamma_{k-1}Q^{1/2}_{k-1}(Q^{1/2}_{k-1})^T\Gamma_{k-1}^T \\ =\left[\begin{matrix} \Phi_{k/k-1}\Delta_{k-1}&\Gamma_{k-1}Q_{k-1}^{1/2}\\ \end{matrix}\right] \left[\begin{matrix} \Delta_{k-1}^T\Phi_{k/k-1}^T\\(Q_{k-1}^{1/2})^T\Gamma_{k-1}^T\\ \end{matrix}\right] Pk/k−1=Δk/k−1Δk/k−1T=Φk/k−1Δk−1Δk−1TΦk/k−1T+Γk−1Qk−11/2(Qk−11/2)TΓk−1T=[Φk/k−1Δk−1Γk−1Qk−11/2][Δk−1TΦk/k−1T(Qk−11/2)TΓk−1T]
令 A ( n + l ) × n = [ Δ k − 1 T Φ k / k − 1 T ( Q k − 1 1 / 2 ) T Γ k − 1 T ] A_{(n+l)×n}=\left[\begin{matrix} \Delta_{k-1}^T\Phi_{k/k-1}^T\\(Q_{k-1}^{1/2})^T\Gamma_{k-1}^T\\ \end{matrix}\right] A(n+l)×n=[Δk−1TΦk/k−1T(Qk−11/2)TΓk−1T],对 A ( n + l ) × n A_{(n+l)×n} A(n+l)×n进行UD分解
A ( n + l ) × n = Q ‾ ( n + l ) × n R ‾ n × n A_{(n+l)×n}=\overline Q_{(n+l)×n}\overline R_{n×n} A(n+l)×n=Q(n+l)×nRn×n
A ( n + l ) × n T A ( n + l ) × n = ( Q ‾ ( n + l ) × n R ‾ n × n ) T ( Q ‾ ( n + l ) × n R ‾ n × n ) = R n × n T R n × n = Δ k / k − 1 Δ k / k − 1 T A_{(n+l)×n}^TA_{(n+l)×n}=(\overline Q_{(n+l)×n}\overline R_{n×n})^T(\overline Q_{(n+l)×n}\overline R_{n×n})=R_{n×n}^TR_{n×n}=\Delta_{k/k-1}\Delta_{k/k-1}^T A(n+l)×nTA(n+l)×n=(Q(n+l)×nRn×n)T(Q(n+l)×nRn×n)=Rn×nTRn×n=Δk/k−1Δk/k−1T
Δ k / k − 1 T \Delta_{k/k-1}^T Δk/k−1T为 A ( n + l ) × n A_{(n+l)×n} A(n+l)×n进行UD分解之后的，上三角或下三角阵
平方根滤波流程
{ a k = Δ k / k − 1 T H k T ρ k 2 = H k Δ k / k − 1 Δ k / k − 1 T H k T + R k = a k T a k + R k Δ k = Δ k / k − 1 ( I − r k − 1 Δ k / k − 1 T H k T H k Δ k / k − 1 ) = Δ k / k − 1 ( I − r k − 1 a k a k T ) K k = Δ k / k − 1 Δ k / k − 1 T H k T ( H k Δ k / k − 1 Δ k / k − 1 T H k T + R k ) − 1 = Δ k / k − 1 a k ρ k − 2 ( 或 Δ k Δ k T H k T R k − 1 ) \begin{cases} a_k=\Delta_{k/k-1}^TH_k^T&\\ \rho_k^2=H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k=a_k^Ta_k+R_k\\ \Delta _k=\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})=\Delta_{k/k-1}(I-r^{-1}_ka_ka_k^T)\\ K_k=\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T(H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k)^{-1}=\Delta_{k/k-1}a_k\rho_k^{-2}(或\Delta_{k}\Delta_{k}^TH_k^TR_k^{-1})\\ \end{cases} ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧ak=Δk/k−1THkTρk2=HkΔk/k−1Δk/k−1THkT+Rk=akTak+RkΔk=Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1)=Δk/k−1(I−rk−1akakT)Kk=Δk/k−1Δk/k−1THkT(HkΔk/k−1Δk/k−1THkT+Rk)−1=Δk/k−1akρk−2(或ΔkΔkTHkTRk−1)
当量测为矢量时
上述情况针对量测为标量时的情况，当量测为矢量时：
- 使用序贯滤波将矢量转化为标量
- 直接进行向量量测平方根滤波
此时：均方误差阵及其对应平方根滤波公式为：
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 ] s t : p k p k T = H k P k / k − 1 H k T + R k = [ H k Δ k / k − 1 R k 1 / 2 ] [ Δ k / k − 1 T H k T ( R k 1 / 2 ) T ] − > P k T ( Q R 同 上 Q R 分 解 得 到 ) 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}]\\ st:\\ p_kp_k^T=H_kP_{k/k-1}H_k^T+R_k=\left[\begin{matrix} H_{k}\Delta_{k/k-1}&R_{k}^{1/2}\\ \end{matrix}\right] \left[\begin{matrix} \Delta_{k/k-1}^TH_{k}^T\\ (R_{k}^{1/2})^T\\ \end{matrix}\right]->P_k^T(QR同上QR分解得到) 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]st:pkpkT=HkPk/k−1HkT+Rk=[HkΔk/k−1Rk1/2][Δk/k−1THkT(Rk1/2)T]−>PkT(QR同上QR分解得到)
