[发明专利]火星大气进入段的非线性三步滤波方法

摘要

一种火星大气进入段的非线性三步滤波方法，它包括以下步骤：一、建立工程实际方程；二、给定初始值；三、对状态量xk进行滤波；四、对动力学系统偏差fk进行滤波；五、对测量系统中的未知测量系统误差dk进行滤波；六、更新相关系数、校正状态估计和动力学偏差估计；七、令k=k+1，返回步骤三往下进行，直到k等于火星大气进入时间截止对应的时刻T时，即降落伞打开为止；至此完成火星大气进入段的非线性三步滤波方法。本发明统筹考虑了火星实际大气进入过程中，非线性、非高斯随机系统在动力学系统偏差和测量系统中的未知测量系统误差条件下的航天器位置速度估计问题，可以有效保证航天器在火星大气进入段的位置速度估计。

主权项

1.一种火星大气进入段的非线性三步滤波方法，其特征在于：它包括以下步骤：步骤一、建立工程实际方程：离散时间下的动力学系统和量测系统xk+1=f(xk,uk)+Fkxfk+Ekxdk+wkx---(1)]]>zk=h(xk)+Fkzfk+Ekzdk+vk---(2)]]>其中x_k表示系统状态量，z_k是测量系统测量值,f_k是未知的动力学系统偏差,d_k是未知的量测系统误差；非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于可x_k微；矩阵具有恰当的维数；和v_k分别是动力学系统噪声，它们是不相关的高斯白噪声满足以下式子：E[wkx]=0Cov[wkx,wjx]=E[wkxwjxT]=QkδkjE[vk]=0Cov[vk,vj]=E[vkvjT]=RkδkjCov[wkx,vj]=E[wkxvjT]=0---(3)]]>步骤二、给定初始值：和为初始状态的估计值，为初始状态估计均方误差，为动力学偏差和量测系统误差的相关系数；步骤三、对状态量x_k进行滤波x-k(-)=f(x^k-1,uk-1)---(4)]]>P-kx(-)=Φk-1P^k-1x(+)Φk-1T+Qk-1---(5)]]>K-kx=P-kx(-)Sk1TCk-1---(6)]]>P-kx(+)=(I-K-kxSk1)P-kx(-)---(7)]]>η-kx=zk-h(x-k(-)),]]>x-k(+)=x-k(-)+K-kxη-kx---(8)]]>Ck=Sk1P-kx(-)Sk1T+Rk---(9)]]>其中Φk=∂f(x)∂x|x=x^k(+)]]>Sk1=Hk=∂h(x)∂x|x=x-k(-).---(10)]]>式中：为t_k-1时刻的状态量，u_k-1为t_k-1时刻的控制输入量；为状态的一步预测；为t_k-1时刻的状态估计均方误差，Φ_k-1为t_k-1时刻到t_k时刻的一步转移矩阵；Q_k-1为系统的噪声的方差阵，为一步预测均方误差；为量测阵，为状态增益，C_k为量测新息误差阵；I为单位阵，为t_k时刻的状态估计均方误差；为量测新息，为状态估计；步骤四、对动力学系统偏差f_k进行滤波Uk12=Fk-1x---(11)]]>Sk2=HkUk12+Fky---(12)]]>P-kf(+)=(Sk2TCk-1Sk2)+---(13)]]>K-kf=P-kf(+)Sk2TCk-1---(14)]]>f-k(+)=K-kfη-kx---(15)]]>为t_k-1时刻动力学系统偏差对动力学系统驱动阵，为t_k时刻动力学系统偏差对量测系统量测阵，为t_k时刻动力学系统偏差对量测系统校正量测阵，为t_k时刻动力学系统偏差估计均方误差，为广义逆矩阵，为动力学系统偏差滤波增益；为t_k时刻动力学系统偏差状态估计；步骤五、对测量系统中的未知测量系统误差d_k进行滤波Uk23=Vk-123---(16)]]>Uk13=Ek-1x+Fk-1xVk-123---(17)]]>Sk3=HkUk13+FkyUk23+Eky---(18)]]>P-ka(+)=(Sk3TCk-1Sk3)+---(19)]]>K-kd=P-kd(+)Sk3TCk-1---(20)]]>d-k(+)=K-kdη-kx---(21)]]>为t_k-1时刻量测系统误差对动力学系统驱动阵，为t_k时刻量测系统误差对量测系统量测阵，为t_k时刻量测系统误差对量测系统校正量测阵，为t_k时刻量测系统误差估计均方误差，为广义逆矩阵，为量测系统误差滤波增益；为t_k时刻动力学系统偏差状态估计；为t_k-1时刻动力学偏差和量测系统误差的相关系数；步骤六、更新相关系数、校正状态估计和动力学偏差估计：情况如下Vk12=Uk12-K-kxSk2,]]>Vk13=Uk13-Vk12K-kfSk3-K-kxSk3]]>Vk23=Vk-123-K-kfSk3---(22)]]>x^k(+)=x-k(+)+Vk12f-k(+)+Vk13d-k(+),]]>P^kx(+)=P-kx(+)+Vk12P-kf(+)Vk12T+Vk13P-kd(+)Vk13T---(23)]]>f^k(+)=f-k(+)+Vk23d-k(+),]]>P^kf(+)=P-kf(+)+Vk23P-kd(+)Vk23T---(24)]]>为t_k时刻校正后的状态量，为t_k时刻校正后的状态估计均方误差，为t_k时刻校正后的状态量，为t_k时刻校正后的状态估计均方误差；步骤七、令k=k+1，返回步骤三往下进行，直到k等于火星大气进入时间截止对应的时刻T时，即降落伞打开为止；至此完成火星大气进入段的非线性三步滤波方法；当离散条件下的扩维动力学系统和量测系统表达形式为xk+1=Φkxk+Bkuk+Fkxfk+Ekxdk+wkx---(25)]]>zk=Hkxk+Fkzfk+Ekzdk+vk---(26)]]>其扩维Kalman滤波表达形式为x^ka(-)=Φk-1ax^k-1a(+)+Bk-1auk-1---(27)]]>Pka(-)=Φk-1aPk-1a(+)Φk-1aT+Qk-1a---(28)]]>Kka=Pka(-)HkaT(HkaPka(-)HkaT+Rk)-1---(29)]]>x^ka(+)=x^ka(-)+Kkaz~k=x^k/k-1a+Kka(zk-Hkax^ka(-))---(30)]]>Pka(+)=(I-KkaHka)Pka(-)---(31)]]>其中xka(·)=xk(·)fk(·)dk(·),]]>Pka(·)=ΔPkx(·)Pkxf(·)Pkxd(·)(Pkxf(·))TPkf(·)Pkfd(·)(Pkxd(·))T(Pkfd(·))TPkd(·),]]>Kka=KkxKkfKkd,]]>Φka=ΦkFkxEkxAkfAkd,]]>Hka=HkFkzEkz]]>和Qka=Qkx000Qkf000Qkd]]>将一步预测均方误差和估计均方误差进行非线性三步U-V变换，其表达形式为Pk/k-1a=UkP-k/k-1aUkT]]>Pk/ka=VkP-kaVkT]]>其中矩阵U_k和V_k定义为如下形式：Uk=IUk12Uk130IUk2300I]]>Vk=IVk12Vk130IVk2300I]]>。