[发明专利]火星动力下降段非线性三步滤波方法

摘要

一种火星动力下降段非线性三步滤波方法，其步骤如下：一、建立离散型非线性三步方法的动力学系统和量测系统方程；二、给定初始值；三、状态量滤波；四、动力学系统偏差滤波；五、测量系统中的未知测量系统误差滤波；六、更新相关系数、校正状态估计和动力学偏差估计；七、返回步骤三往下进行，直到等于火星动力下降段时间截止对应的时刻T时，即火星着陆器着陆为止，至此完成火星动力下降段非线性三步滤波方法。本发明统筹考虑了火星实际动力下降过程中，非线性、非高斯随机系统在动力学系统偏差和测量系统中的未知测量系统误差条件下的探测器位置速度估计问题，有效保证探测器在火星动力下降段的位置速度估计。

主权项

1.一种火星动力下降段非线性三步滤波方法，其特征在于：它包括以下步骤：步骤一、离散型非线性三步方法的动力学系统和量测系统形式为xk+1=f(xk,uk)+Fkxfk+Ekxdk+wkx---(1)]]>yk=h(xk)+Fkyfk+Ekydk+wky---(2)]]>其中x_k表示系统状态量，y_k是测量系统测量值,f_k是未知的动力学系统偏差,d_k是未知的量测系统误差；非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于可x_k微；矩阵具有恰当的维数；和分别是动力学系统噪声，它们是不相关的高斯白噪声，满足以下式子E[wkx]=0Cov[wkx,wjx]=E[wkxwjxT]=QkδkjE[wky]=0Cov[wky,wjy]=E[wkywjyT]=RkδkjCov[wkx,wjy]=E[wkxwjyT]=0---(3)]]>；步骤二、给定初始值：和为初始状态的估计值，为初始状态估计均方误差，为动力学偏差和量测系统误差的相关系数；步骤三、状态量滤波x-k+1(-)=f(x^k,uk)---(4)]]>P-k+1x(-)=ΦkP^kx(+)ΦkT+Qk---(5)]]>Ck+1=Sk+11P-k+1x(-)Sk+11T+Rk+1---(6)]]>K-k+1x=P-k+1x(-)Sk+11TCk+1-1---(7)]]>P-k+1x(+)=(I-K-k+1xSk+11)P-k+1x(-)---(8)]]>η-k+1x=zk+1-h(x-k+1(-)),]]>x-k+1(+)=x-k+1(-)+K-k+1xη-k+1x---(9)]]>其中Φk+1=∂f(x)∂x|x=x^k+1(+)]]>Sk+11=Hk+1=∂h(x)∂x|x=x^k+1(-).---(10)]]>式中：为状态的一步预测，为t_k时刻的状态量，u_k为t_k时刻的控制输入量；为t_k时刻的状态估计均方误差，Φ_k为t_k时刻到t_k+1时刻的一步转移矩阵；Q_k为系统的噪声的方差阵，为一步预测均方误差；为量测阵，C_k+1为量测新息误差阵，为状态增益；I为单位阵，为t_k+1时刻的状态估计均方误差；为量测新息，为状态估计；步骤四、动力学系统偏差滤波Uk+112=Fkx---(11)]]>Sk+12=Hk+1Uk+112+Fk+1y---(12)]]>P-k+1f(+)=(Sk+12TCk+1-1Sk+12)+---(13)]]>K-k+1f=P-k+1f(+)Sk+12TCk+1-1---(14)]]>f-k+1(+)=K-k+1fη-k+1x---(15)]]>为t_k时刻动力学系统偏差对动力学系统驱动阵，为转移矩阵，为t_k+1时刻动力学系统偏差对量测系统量测阵，为t_k+1时刻动力学系统偏差对量测系统校正量测阵；为t_k+1时刻动力学系统偏差估计均方误差，为广义逆矩阵；为动力学系统偏差滤波增益，为t_k+1时刻动力学系统偏差状态估计；步骤五、测量系统中的未知测量系统误差滤波Uk+123=Vk23---(16)]]>Uk+113=Ekx+FkxVk23---(17)]]>Sk+13=Hk+1Uk+113+Fk+1yUk+123+Ek+1y---(18)]]>P-k+1d(+)=(Sk+13TCk+1-1Sk+13)+---(19)]]>K-k+1d=P-k+1d(+)Sk+13TCk+1-1---(20)]]>d-k+1(+)=K-k+1dη-k+1x---(21)]]>为t_k时刻动力学偏差和量测系统误差的相关系数，为转移矩阵；为t_k时刻量测系统误差对动力学系统驱动阵，为变换矩阵；为t_k+1时刻量测系统误差对量测系统量测阵，为t_k+1时刻量测系统误差对量测系统校正量测阵；为t_k+1时刻量测系统误差估计均方误差，为广义逆矩阵；为量测系统误差滤波增益；为t_k+1时刻动力学系统偏差状态估计；步骤六、更新相关系数、校正状态估计和动力学偏差估计Vk+112=Uk+112-K-k+1xSk+12Vk+113=Uk+113-Vk+112K-k+1fSk+13-K-k+1xSk+13Vk+123=Vk23-K-k+1fSk+13---(22)]]>x^k+1(+)=x-k+1(+)+Vk+112f-k+1(+)+Vk+113d-k+1(+)P^k+1x(+)=P-k+1x(+)+Vk+112P-k+1f(+)Vk+112T+Vk+113P-k+1d(+)Vk+113T---(23)]]>f^k+1(+)=f-k+1(+)+Vk+123d-k+1(+)P^k+1f(+)=P-k+1f(+)+Vk+123P-k+1d(+)Vk+123T---(24)]]>均为相关系数，为t_k+1时刻校正后的状态量，为t_k+1时刻校正后的状态估计均方误差，为t_k+1时刻校正后的状态量，为t_k+1时刻校正后的状态估计均方误差；步骤七、令k=k+1，返回步骤三往下进行，直到k等于火星动力下降段时间截止对应的时刻T时，即火星着陆器着陆为止；至此完成火星动力下降段非线性三步滤波方法。