捷联惯导系统学习6.4(平方根滤波 )

(37) 2024-03-09 12:01:01

平方根滤波

针对经典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...δnnT
可以得到:
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+1nδikδjk+δijδjj(1in,ijn)
从而得到:
δ 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=(Piji+jnδikδjk)/δjjPjjk=j+1nδjk2
0
i<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:mn,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/k1:
Γ k / k − 1 : 已 知 的 系 统 结 构 参 数 , 分 别 为 n × l 阶 系 统 分 配 噪 声 \Gamma_{k/k-1}:已知的系统结构参数,分别为n×l阶系统分配噪声 Γk/k1: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维系统噪声向量,高斯白噪声,服从正太分布 Wk1:m
V k 与 W k − 1 互 不 相 关 V_k与W_{k-1}互不相关 VkWk1
{ 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/k1Xk1+Γk/k1Wk1Zk=HkXk+Vk
st.{
E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk0R0

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/k1=Φk/k1X^k1Pk/k1=Φk/k1Pk1Φk/k1T+Γk1Qk1Γk1TKk=Pk/k1HkT(HkPk/k1HkTRk)1X^k=(IKkHk)X^k/k1+KkZkPk=(IKkHk)Pk/k1
滤波增益带入状态估计均方误差阵
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=(IPk/k1HkT(HkPk/k1HkTRk)1Hk)Pk/k1
假设 P k − 1 , P k / k − 1 , P k P_{k-1},P_{k/k-1},P_k Pk1,Pk/k1,Pk的平方根分别为 Δ k − 1 , Δ k / k − 1 , Δ k \Delta_{k-1},\Delta_{k/k-1},\Delta_k Δk1,Δk/k1,Δ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 Pk1=Δk1Δk1T,Pk/k1=Δk/k1Δk/k1T,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/k1[IΔk/k1THkT(HkΔk/k1Δk/k1THkT+Rk)1HkΔk/k1]Δk/k1T=Δk/k1(Iρk2Δk/k1THkTHkΔk/k1)Δk/k1Tst:ρk2=HkΔk/k1Δk/k1THkT+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ρk2Δk/k1THkTHkΔk/k1的根
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ρk2Δk/k1THkTHkΔk/k1=(Irk1Δk/k1THkTHkΔk/k1)(I+rk1Δk/k1THkTHkΔk/k1)
利用待定系数法求得:
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ρk44ρk2(ρk2Rk)

Δ 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/k1(Iρk2Δk/k1THkTHkΔk/k1)Δk/k1T=(Δk/k1(Irk1Δk/k1THkTHkΔk/k1))(Δk/k1(Irk1Δk/k1THkTHkΔk/k1))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/k1(Irk1Δk/k1THkTHkΔk/k1)(Δ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 Pk1=Δk1Δk1T,Pk/k1=Δk/k1Δk/k1T,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/k1=Φk/k1Pk1Φk/k1T+Γk1Qk1Γk1T
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 分解法求得) Qk11/2(Qk11/2)T=Qk1(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/k1=Δk/k1Δk/k1T=Φk/k1Δk1Δk1TΦk/k1T+Γk1Qk11/2(Qk11/2)TΓk1T=[Φk/k1Δk1Γk1Qk11/2][Δk1TΦk/k1T(Qk11/2)TΓk1T]
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=[Δk1TΦk/k1T(Qk11/2)TΓk1T],对 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/k1Δk/k1T
Δ k / k − 1 T \Delta_{k/k-1}^T Δk/k1T 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/k1THkTρk2=HkΔk/k1Δk/k1THkT+Rk=akTak+RkΔk=Δk/k1(Irk1Δk/k1THkTHkΔk/k1)=Δk/k1(Irk1akakT)Kk=Δk/k1Δk/k1THkT(HkΔk/k1Δk/k1THkT+Rk)1=Δk/k1akρk2(ΔkΔkTHkTRk1)
捷联惯导系统学习6.4(平方根滤波 ) (https://mushiming.com/)  第1张

当量测为矢量时
上述情况针对量测为标量时的情况,当量测为矢量时:

  1. 使用序贯滤波将矢量转化为标量
  2. 直接进行向量量测平方根滤波
    此时:均方误差阵及其对应平方根滤波公式为:
    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/k1Pk/k1HkT(HkPk/k1HkT+Rk)1HkPk/k1Δk=Δk/k1[IΔk/k1THkT(pkpkT+Rk1/2PkT)1HkΔk/k1]st:pkpkT=HkPk/k1HkT+Rk=[HkΔk/k1Rk1/2][Δk/k1THkT(Rk1/2)T]>PkT(QRQR)
THE END

发表回复