捷联惯导系统学习6.8(最优平滑算法 )

预测、估计、平滑

对于一组量测序列: Z ‾ M = { Z ‾ 1 , Z ‾ 2 , . . . , Z ‾ k , . . . , Z ‾ M } \overline Z_M=\{\overline Z_1,\overline Z_2,...,\overline Z_k,...,\overline Z_M\} ZM={Z1,Z2,...,Zk,...,ZM}
对于 Z k Z_k Zk来说
{ 预测：寻找 X j ( j > k ) 的点 , 用 X ^ j / k 表示最有估计：寻找 X j ( j = k ) , 用 X ^ j / k = X ^ k 表示平滑 ( 又称内插 ) : 寻找 X j ( j < k ) , 用 X ^ j / k 表示 \begin{cases} 预测：寻找X_j(j>k)的点,用\hat X_{j/k}表示\\ 最有估计：寻找X_j(j=k),用\hat X_{j/k}=\hat X_k表示\\ 平滑(又称内插):寻找X_j(j<k),用\hat X_{j/k}表示\\ \end{cases} ⎩⎪⎨⎪⎧预测：寻找Xj(j>k)的点,用X^j/k表示最有估计：寻找Xj(j=k),用X^j/k=X^k表示平滑(又称内插):寻找Xj(j<k),用X^j/k表示捷联惯导系统学习6.8(最优平滑算法 )

固定点平滑、固定平滑、固定区间平滑

固定点平滑（fixed-point smoothing）：被估状态X_j是某个固定的j时刻，输出为使用不同量测，对j时刻不同的被估结果 X ^ j / j + 1 , X ^ j / j + 1 , . . . , X ^ j / j + 1 , \hat X_{j/j+1},\hat X_{j/j+1},...,\hat X_{j/j+1}, X^j/j+1,X^j/j+1,...,X^j/j+1,
固定滞后平滑(fixed-lag smoothing):被估状态 X j X_j Xj与 Z j + N Z_{j+N} Zj+N之间存在固定的时间间隔，假设滞后值为N，
则固定滞后平滑输出为： X ^ 1 / 1 + N , X ^ 2 / 2 + N , . . . , X ^ M − N / M , \hat X_{1/1+N},\hat X_{2/2+N},...,\hat X_{M-N/M}, X^1/1+N,X^2/2+N,...,X^M−N/M,
固定区间平滑（fixed-interval smoothing）:被估状态 X j X_j Xj在量测区间内取所有值，
固定区间输出为： X ^ 1 / M , X ^ 2 / M , . . . , X ^ j / M , . . . , X ^ M / M \hat X_{1/M},\hat X_{2/M},...,\hat X_{j/M},...,\hat X_{M/M} X^1/M,X^2/M,...,X^j/M,...,X^M/M

固定区间平滑（双向平滑滤波方法）

实现原理
在前向kalman滤波基础上，再进行反向滤波
系统状态空间
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
推导
基本概念：

在平滑算法中，针对某一时刻 X j X_j Xj进行最优平滑，表示利用所有量测序列 Z ‾ M = { Z ‾ 1 , Z ‾ 2 , . . . , Z ‾ k , . . . , Z ‾ M } \overline Z_M=\{\overline Z_1,\overline Z_2,...,\overline Z_k,...,\overline Z_M\} ZM={Z1,Z2,...,Zk,...,ZM}对 X j X_j Xj做估计，结果记为 X ^ j / M \hat X_{j/M} X^j/M
以 j j j时刻为分界，将 Z ‾ M \overline Z_M ZM分为两部分: Z ‾ 1 , j = { Z 1 , Z 2 , . . . , Z j } ; Z ‾ j + 1 , M = { Z j + 1 , Z j + 2 , . . . , Z M } \overline Z_{1,j}=\{Z_1 ,Z_2,...,Z_j\};\overline Z_{j+1,M}=\{Z_{j+1},Z_{j+2},...,Z_{M}\} Z1,j={Z1,Z2,...,Zj};Zj+1,M={Zj+1,Zj+2,...,ZM}

使用kalman滤波对 Z ‾ 1 , j \overline Z_{1,j} Z1,j做正向滤波，得到 X j X_j Xj的估计：
{ X ^ f , j = Φ k / k − 1 X ^ f , k − 1 P f , k / k − 1 = Φ k / k − 1 P f , k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T K f , k = P f , k / k − 1 H k T ( H k P f , k / k − 1 H k T + R k ) − 1 X ^ f , k = X ^ f , k / k − 1 + K f , k ( Z k − H k X ^ f , k / k − 1 ) P f , k = ( I − K f , k H k ) P f , k / k − 1 k = 1 , 2 , . . . , j f 表示前向滤波 \begin{cases} \hat X_{f,j}=\Phi_{k/k-1}\hat X_{f,k-1} \\ P_{f,k/k-1}=\Phi_{k/k-1}P_{f,k-1}\Phi_{k/k-1}^T+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T\\ K_{f,k}=P_{f,k/k-1}H_k^T(H_kP_{f,k/k-1}H_k^T+R_k)^{-1}\\ \hat X_{f,k}=\hat X_{f,k/k-1}+K_{f,k}(Z_k-H_k\hat X_{f,k/k-1})\\ P_{f,k}=(I-K_{f,k}H_k)P_{f,k/k-1}\\ k=1,2,...,j&f表示前向滤波 \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧X^f,j=Φk/k−1X^f,k−1Pf,k/k−1=Φk/k−1Pf,k−1Φk/k−1T+Γk−1Qk−1Γk−1TKf,k=Pf,k/k−1HkT(HkPf,k/k−1HkT+Rk)−1X^f,k=X^f,k/k−1+Kf,k(Zk−HkX^f,k/k−1)Pf,k=(I−Kf,kHk)Pf,k/k−1k=1,2,...,jf表示前向滤波
kalman滤波反向滤波

将系统状态空间改写为：
{ X k = Φ k + 1 / k − 1 X k + 1 − Φ k + 1 / k − 1 Γ k W k Z k = H k X k + V k 令 : Φ k / k − 1 ∗ = Φ k + 1 / k − 1 , Γ k ∗ = − Φ k + 1 / k − 1 Γ k 得到： { X k = Φ k / k + 1 ∗ X k + 1 + Γ k / ∗ W k Z k = H k X k + V k \begin{cases} X_k=\Phi_{k+1/k}^{-1}X_{k+1}-\Phi_{k+1/k}^{-1}\Gamma_{k}W_{k}\\ Z_k=H_kX_k+V_k\\ \end{cases} \\ 令:\Phi_{k/k-1}^*=\Phi_{k+1/k}^{-1},\Gamma_k^*=-\Phi_{k+1/k}^{-1}\Gamma_k \\ 得到：\\ \begin{cases} X_k=\Phi_{k/k+1}^*X_{k+1}+\Gamma_{k/}^*W_{k}\\ Z_k=H_kX_k+V_k\\ \end{cases} {Xk=Φk+1/k−1Xk+1−Φk+1/k−1ΓkWkZk=HkXk+Vk令:Φk/k−1∗=Φk+1/k−1,Γk∗=−Φk+1/k−1Γk得到：{Xk=Φk/k+1∗Xk+1+Γk/∗WkZk=HkXk+Vk
为了避免求逆运算：
如果一步状态转移矩阵来自于某一连续时间系统，根据kalman离散化可以得到
Φ k + 1 / k ≈ 1 + F ( t k ) T s \Phi_{k+1/k}\approx 1+F(t_k)T_s Φk+1/k≈1+F(tk)Ts
当离散化时间足够短时，可以做近似处理令：
Φ k / k − 1 ∗ = Φ k / k − 1 − 1 ≈ [ I + F ( t k ) T s ] − 1 ≈ I − F ( t k + 1 ) T s \Phi_{k/k-1}^*=\Phi_{k/k-1}^{-1}\approx[I+F(t_k)T_s]^{-1}\approx I-F(t_{k+1})T_s Φk/k−1∗=Φk/k−1−1≈[I+F(tk)Ts]−1≈I−F(tk+1)Ts
再利用kalman滤波方程，进行反向滤波

{ X ^ b , k / k + 1 = Φ k / k + 1 ∗ X ^ b , k + 1 P b , k / k + 1 = Φ k / k + 1 ∗ P b , k + 1 ( Φ k / k + 1 ∗ ) T + Γ k ∗ Q k ( Γ k ∗ ) T K b , k = P b , k / k + 1 H k T ( H k P b , k / k + 1 H k T + R k ) − 1 X ^ b , k = X ^ b , k / k + 1 + K b , k ( Z k − H k X ^ b , k / k + 1 ) P b , k = ( I − K b , k H k ) P b , k / k + 1 k = M − 1 , M − 2 , . . . , j b 表示反向滤波 \begin{cases} \hat X_{b,k/k+1}=\Phi_{k/k+1}^*\hat X_{b,k+1}\\ P_{b,k/k+1}=\Phi^*_{k/k+1}P_{b,k+1}(\Phi_{k/k+1}^*)^T+\Gamma_k^*Q_k(\Gamma_k^*)^T\\ K_{b,k}=P_{b,k/k+1}H_k^T(H_kP_{b,k/k+1}H_k^T+R_k)^{-1}\\ \hat X_{b,k}=\hat X_{b,k/k+1} +K_{b,k}(Z_k-H_k\hat X_{b,k/k+1})\\ P_{b,k}=(I-K_{b,k}H_k)P_{b,k/k+1}\\ k=M-1,M-2,...,j&b表示反向滤波\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧X^b,k/k+1=Φk/k+1∗X^b,k+1Pb,k/k+1=Φk/k+1∗Pb,k+1(Φk/k+1∗)T+Γk∗Qk(Γk∗)TKb,k=Pb,k/k+1HkT(HkPb,k/k+1HkT+Rk)−1X^b,k=X^b,k/k+1+Kb,k(Zk−HkX^b,k/k+1)Pb,k=(I−Kb,kHk)Pb,k/k+1k=M−1,M−2,...,jb表示反向滤波
在k=j，可以获得状态 X j X_j Xj的反向最有一部预测 X ^ b , j / j + 1 \hat X_{b,j/j+1} X^b,j/j+1,及其均方误差阵 P b , j / j + 1 P_{b,j/j+1} Pb,j/j+1

综合正向和反向滤波可以得到：
{ X ^ f , j = X j + V f , j X ^ b , j / j + 1 = X j + V b , j / j + 1 s t : V f , j ∼ N ( 0 , P f , j ) ; V b , j / j + 1 ∼ N ( 0 , P b , j / j + 1 ) ; C o v ( V f , j , V b , j / j + 1 ) = 0 \begin{cases} \hat X_{f,j}=X_j+V_{f,j}\\ \hat X_{b,j/j+1}=X_j+V_{b,j/j+1}\\ \end{cases} \\ st:\\ V_{f,j} \sim N(0,P_{f,j});V_{b,j/j+1}\sim N(0,P_{b,j/j+1});Cov(V_{f,j},V_{b,j/j+1})=0 {X^f,j=Xj+Vf,jX^b,j/j+1=Xj+Vb,j/j+1st:Vf,j∼N(0,Pf,j);Vb,j/j+1∼N(0,Pb,j/j+1);Cov(Vf,j,Vb,j/j+1)=0
根据信息融合滤波公式（N=2,即两种新息来源时）：
{ P s , j = ( P i , j − 1 + P b , j / j + 1 − 1 ) − 1 X ^ s , j = P s , j ( P b , j / j + 1 − 1 X ^ f , j + P f , j − 1 X ^ b , j / j + 1 ) s 表示平滑结果 \begin{cases} P_{s,j}=(P_{i,j}^{-1}+P^{-1}_{b,j/j+1})^{-1}\\ \hat X_{s,j}=P_{s,j}(P_{b,j/j+1}^{-1}\hat X_{f,j}+P^{-1}_{f,j}\hat X_{b,j/j+1})\\ s表示平滑结果 \end{cases} ⎩⎪⎨⎪⎧Ps,j=(Pi,j−1+Pb,j/j+1−1)−1X^s,j=Ps,j(Pb,j/j+1−1X^f,j+Pf,j−1X^b,j/j+1)s表示平滑结果
如果要实现整个区间的平滑，那么应该先从整个区间从头到尾进行，前向滤波，再从尾到到头进行后向滤波，每得到一个后向滤波值，就进行新息滤波融合。
实际使用为了降低数据存储量和求逆运算：
状态估计值和均方误差阵只保留对角线元素，分别以滤波值,并且令：
P b , j / j + 1 = P b , j ; X ^ b , j / j + 1 = X ^ b , j P_{b,j/j+1}=P_{b,j};\hat X_{b,j/j+1}=\hat X_{b,j} Pb,j/j+1=Pb,j;X^b,j/j+1=X^b,j
在进行加权处理：
P s , j ( i ) = ( 1 P f , j ( i ) + 1 P b , j ( i ) ) − 1 X ^ s , j ( i ) = P s , j ( i ) ( X ^ ( i ) P b , j ( i ) + X ^ b , j ( i ) P f , j ( i ) ) P_{s,j}^{(i)}=(\frac{1}{P^{(i)}_{f,j}}+\frac{1}{P^{(i)}_{b,j}})^{-1} \\ \hat X^{(i)}_{s,j}=P_{s,j}^{(i)}(\frac{\hat X^{(i)}}{P^{(i)}_{b,j}}+\frac{\hat X_{b,j}^{(i)}}{P^{(i)}_{f,j}}) Ps,j(i)=(Pf,j(i)1+Pb,j(i)1)−1X^s,j(i)=Ps,j(i)(Pb,j(i)X^(i)+Pf,j(i)X^b,j(i))

RTS固定区间平滑

双向平滑滤波方法的反向滤波，涉及求逆运算，计算较大
为了解决这一问题，提出了RTS平滑算法，RTS算法在状态魏书不高时能够在一定程度上降低计算量。
算法流程

进行前向滤波，与双向平滑滤波相同。
再执行后向滤波，即RTS平滑滤波算法,没得到一个结果，进行吃一次信息融合滤波：
{ K s , k = P f , k Φ k + 1 / k T P f . k + 1 / k − 1 X ^ s , k = X ^ f , k + K s , k ( X ^ s , k + 1 − X ^ f , k + 1 / k ) P s , k = P f , k + K s , k ( P s , k + 1 − P f , k + 1 / k ) K s , k T k = M − 1 , M − 2 , . . . , 1 \begin{cases} K_{s,k}=P_{f,k}\Phi^T_{k+1/k}P^{-1}_{f.k+1/k}\\ \hat X_{s,k}=\hat X_{f,k}+K_{s,k}(\hat X_{s,k+1}-\hat X_{f,k+1/k})\\ P_{s,k}=P_{f,k}+K_{s,k}(P_{s,k+1}-P_{f,k+1/k})K_{s,k}^T \\ k=M-1,M-2,...,1 \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧Ks,k=Pf,kΦk+1/kTPf.k+1/k−1X^s,k=X^f,k+Ks,k(X^s,k+1−X^f,k+1/k)Ps,k=Pf,k+Ks,k(Ps,k+1−Pf,k+1/k)Ks,kTk=M−1,M−2,...,1
最后令 X ^ s , M = X ^ f , M ; P s , M = P f , M \hat X_{s,M}=\hat X_{f,M};P_{s,M}=P_{f,M} X^s,M=X^f,M;Ps,M=Pf,M

捷联惯导系统学习6.8(最优平滑算法 )

预测、估计、平滑

固定点平滑、固定平滑、固定区间平滑

固定区间平滑（双向平滑滤波方法）

RTS固定区间平滑

相关推荐