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

权利要求书

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时，即火星着陆器着陆为止；至此完成火星动力下降段非线性三步滤波方法。