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

权利要求书

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]]>。

2.根据权利要求1所述的一种火星大气进入段的非线性三步滤波方法，其特征在于：在步骤一中所述的建立工程实际方程，其步骤如下：

a、分析动力学不确定性之间的相互关系，并进行相应的数值计算分析；

b、得到这些不确定性因素对动力学模型的影响主要是通过速度的一阶微分方程中的加速度的影响进入动力学模型从而引起误差的传播；

c、将动力学系统x_k+1=f(x_k,u_k)改写为考虑动力学系统偏差和量测系统误差的动力学系统将量测系统z_k=h(x_k)改写为考虑动力学系统偏差和量测系统误差的量测系统zk=h(xk)+Fkzfk+Ekzdk+vk.]]>