捷联惯导系统学习6.2(序贯滤波 )
序貫濾波(sequential Kalman filtering)
一種將高維數(shù)據(jù)量測更新降低為多個低維數(shù)量測更新的方法，有效降低矩陣的求逆計算量(通過把矩陣對角化，將對角拆開分開計算)
特別的對于如下方法求解求解狀態(tài)增益誤差陣即：
Pk?1=Pk/k?1?1+HkTRk?1Hk=Pk/k?1?1+[(Hk(1))T...(Hk(N))T][Rk(1)...0...Rk(k)...0...RkN][Hk(1)...Hk(N)]=Pk?1+(Hk(1))TRk(1)Hk(1)+...+(Hk(N))TRk(N)Hk(N)P_k^{-1}=P_{k/k-1}^{-1}+H_k^TR_k^{-1}H_k\\ =P^{-1}_{k/k-1}+\left[\begin{matrix} (H_k^{(1)})^T&...&(H_k^{(N)})^T\\ \end{matrix}\right]\left[\begin{matrix} R_k^{(1)} &...&0\\ ...&R_k^{(k)}&...\\ 0&...&R_k^{N} \end{matrix}\right]\left[\begin{matrix} H_k^{(1)}\\ ...\\ H_k^{(N)} \end{matrix}\right] \\ =P_k^{-1}+(H_k^{(1)})^TR_k^{(1)}H_k^{(1)}+...+(H_k^{(N)})^TR_k^{(N)}H_k^{(N)}Pk?1?=Pk/k?1?1?+HkT?Rk?1?Hk?=Pk/k?1?1?+[(Hk(1)?)T?...?(Hk(N)?)T?]????Rk(1)?...0?...Rk(k)?...?0...RkN??????????Hk(1)?...Hk(N)??????=Pk?1?+(Hk(1)?)TRk(1)?Hk(1)?+...+(Hk(N)?)TRk(N)?Hk(N)?
使用Cholesky分解得到三角矩陣
因為 RkR_kRk?是正定矩陣:
Rk=LkLkTR_k=L_kL^{T}_kRk?=Lk?LkT?
構(gòu)造新的測量方程：
Lk?1Zk=Lk?1HkXk+Lk?1VkL_k^{-1}Z_k=L_k^{-1}H_kX_k+L_k^{-1}V_kLk?1?Zk?=Lk?1?Hk?Xk?+Lk?1?Vk?
等價于：
Zk?=Hk?Xk+Vk?Z^*_k=H^*_kX_k+V_k^*Zk??=Hk??Xk?+Vk??
Zk?=Lk?1Zk,Hk?=Lk?1Hk,Vk?=Lk?1VkZ^*_k=L_k^{-1}Z_k,H^*_k=L_k^{-1}H_k,V_k^*=L_k^{-1}V_kZk??=Lk?1?Zk?,Hk??=Lk?1?Hk?,Vk??=Lk?1?Vk?
一般kalman濾波過程
序貫濾波過程
利用N次最小二乘估計，進行N次遞推最小二乘法，
替換內(nèi)容：
Xk/k?1→Xk(0),Pk/k?1→Pk(0)X_{k/k-1}\rightarrow X_k^{(0)},P_{k/k-1}\rightarrow P_{k}^{(0)}Xk/k?1?→Xk(0)?,Pk/k?1?→Pk(0)?
{Kk(1)=Pk(0)(Hk(0))T[Hk(N)Pk(0)(Hk(1))T+Rk(1)]?1X^k(1)=(I?KkHk(1))X^k(0)+KkZkPk(1)=(I?Kk(1)Hk(1))Pk(0)...{Kk(N)=Pk(N?1)(Hk(N?1))T[Hk(N)Pk(N?1)(Hk(N))T+Rk(N)]?1X^k(N)=(I?KkHk(N))X^k(N?1)+KkZkPk(N)=(I?Kk(N)Hk(N))Pk(N?1)\begin{cases} K_k^{(1)}=P_k^{(0)}(H_k^{(0)})^T[H_k^{(N)}P_k^{(0)}(H_k^{(1)})^T+R_k^{(1)}]^{-1}\\ \hat X_k^{(1)}=(I-K^{}_kH^{(1)}_k)\hat X_{k}^{(0)}+K_kZ_k\\ P_k^{(1)}=(I-K^{(1)}_kH^{(1)}_k)P_k^{(0)}\\ \end{cases} \\ ...\\ \begin{cases} K_k^{(N)}=P_k^{(N-1)}(H_k^{(N-1)})^T[H_k^{(N)}P_k^{(N-1)}(H_k^{(N)})^T+R_k^{(N)}]^{-1}\\ \hat X_k^{(N)}=(I-K^{}_kH^{(N)}_k)\hat X_{k}^{(N-1)}+K_kZ_k\\ P_k^{(N)}=(I-K^{(N)}_kH^{(N)}_k)P_k^{(N-1)}\\ \end{cases} ??????Kk(1)?=Pk(0)?(Hk(0)?)T[Hk(N)?Pk(0)?(Hk(1)?)T+Rk(1)?]?1X^k(1)?=(I?Kk?Hk(1)?)X^k(0)?+Kk?Zk?Pk(1)?=(I?Kk(1)?Hk(1)?)Pk(0)??...??????Kk(N)?=Pk(N?1)?(Hk(N?1)?)T[Hk(N)?Pk(N?1)?(Hk(N)?)T+Rk(N)?]?1X^k(N)?=(I?Kk?Hk(N)?)X^k(N?1)?+Kk?Zk?Pk(N)?=(I?Kk(N)?Hk(N)?)Pk(N?1)??
X^k(N)→X^k,Pk(N)→Pk\hat X_k^{(N)}\rightarrow \hat X_k,P_{k}^{(N)}\rightarrow P_{k}X^k(N)?→X^k?,Pk(N)?→Pk?
總結(jié)
以上是生活随笔為你收集整理的捷联惯导系统学习6.2(序贯滤波 )的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 让机器学习手把手指导你的下一步实验-基于
- 下一篇： 渗透学校某系统