[发明专利]火星大气进入段消除测量数据中系统误差的两步滤波方法

权利要求书

1.一种火星大气进入段消除测量数据中系统误差的两步滤波方法，其特征在于：其步骤如下：

步骤一、建立基于火星大气进入段飞行器的工程实际方程：离散时间下的动力学系统和量测系统

x_k+1=f(x_k)+B_ku_k+w_k   (1)

z_k=h(x_k)+H_kd_k+v_k   (2)

其中x_k表示系统状态量，z_k是测量系统测量值，u_k是状态方程的确定性控制项,d_k是未知的量测系统误差；非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于x_k可微；矩阵B_k和H_k具有恰当的维数；w_k和v_k分别是动力学系统噪声向量和量测噪声向量，其方差阵分别为Q_k和R_k，它们是不相关的高斯白噪声满足以下式子：

E[wk]=0Cov[wk,wj]=E[wkwjT]=QkδkjE[vk]=0Cov[vk,vj]=E[vkvjT]=RkδkjCov[wk,vj]=E[wkvjT]=0]]>   (3)

步骤二、给定初始值：和

其中，为初始状态的估计值，为初始状态的方差阵；

步骤三、两步滤波方法：

（1）、未知的量测系统误差估计

R~k=CkPk/k-1xCkT+Rk]]>   (4)

Mk=(HkTR~k-1Hk)-1HkTR~k-1]]>   (5)

d^k/k=Mk(zk-Ckx^k/k-1)]]>   (6)

Pk/kd=(HkTR~k-1Hk)-1]]>   (7)

其中，

Ck=∂h(x)∂x|x=x^k/k-1]]>   (8)

式中，为t_k时刻的新息均方误差估计，M_k为t_k时刻的未知的量测系统误差的滤波增益，为t_k时刻的未知的量测系统误差的估计，为状态的一步预测，z_k为t_k时刻的量测数据，为t_k时刻状态估计的均方误差，为t_k时刻未知量测系统误差估计的均方误差，C_k为量测阵；

（2）、更新

（a）、量测更新

Kk=Pk/k-1xCkTR~k-1]]>   (9)

x^k/k=x^k/k-1+Kk(zk-Ckx^k/k-1-Hkd^k/k)]]>   (10)

Pk/kx=Pk/k-1x-Kk(R~-HkPk/kdHkT)KkT]]>   (11)

pk/kdx=(Pk/kxd)T=-(KkHkPk/kd)T]]>   (12)

式中，为t_k-1时刻到t_k时刻的状态估计的一步预测均方误差，K_k为t_k时刻的状态滤波增益，为t_k时刻的状态估计，为状态估计与未知量测系统误差估计的协方差，为的转置矩阵；

（b）、时间更新

x^k+1/k=Akx^k/k+Bkuk+Gkd^k/k]]>   (13)

Pk+1/kx=AkGKPk/kxPk/kxdPk/kdxPk/kdAkTGkT+Qk]]>   (14)

其中

Ak=∂f(x)∂x|x=x^k,Gk=0.]]>   (15)

式中，A_k为t_k-1时刻到t_k时刻的一步转移矩阵，G_k为常数矩阵，在状态方程中无未知输入的情况下，即在此情况下，G_k取为0矩阵；

步骤四、令k=k+1，返回步骤三往下进行，直到k等于火星大气进入时间截止对应的时刻T时，至此完成火星大气进入段消除测量数据中系统误差的两步滤波方法。

2.根据权利要求1所述的一种火星大气进入段消除测量数据中系统误差的两步滤波方法，其特征在于：在在步骤一中所述的“建立基于火星大气进入段飞行器的工程实际方程”，其步骤如下：

飞行器沿飞行轨迹进入火星大气，其对应的简化动力学系统为如下方程：

r·=vsinγ]]>

v·=-(D+gMsinγ)]]>

γ·=(vr-gMv)cosγ+1vLcosσ]]>

θ·=vcosγsinψrcosλ]]>   (16)

λ·=vrcosγcosψ]]>

ψ·=vrsinψcosγtanλ+Lsinσvcosγ]]>

其中r飞行器到火星中心的距离，v是飞行器的速度,θ是经度，λ是纬度,γ是飞行路径角,ψ是航向角,控制量σ是滚转角，火星引力GM_Mars=4.28221×10¹³m³/s²；

L，D分别是气动升力和助力定义为式：其中C_L和C_D为升力系数和阻力系数；火星大气密度ρ近似满足指数表达形式，其表达形式为而ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向半径，大小为3437.2km；

由于目前状态下无法形成有效的导航网，量测系统存在未知的量测系统误差，其对应的量测系统表示为：

a~B=aB+aa]]>   (17)

其中加速度计量测值,a^B加速度真实值,a_a未知的加速计系统误差包含偏差随机游走；

R~=R+aR]]>   (18)

R=(r-ri)T(r-ri)]]>   (19)

其中是无线电测量值,R是飞行器与通信卫星间的真实距离，r是飞行器在火星中心惯性坐标系下的位置，r_i是第i个通信卫星在火星中心惯性坐标系下的位置，a_R是未知的量测系统误差；

1)火星大气进入段对应的离散动力学系统改写为如下形式：

x_k+1=f(x_k)+B_ku_k+w_k   (20)

其中x_k为火星大气进入段动力学系统中的状态量包含式（16）中的左边的各分量，飞行器到火星中心的距离r，飞行器的速度v，经度θ，是纬度λ，飞行路径角γ，航向角ψ，B_k为对应的动力学系统控制量对动力学系统的驱动阵，u_k是控制量对应于滚转角σ；

2)对应的离散量测方程为

z_k=h(x_k)+a_x+v_k   (21)

其中

z=aB~R~]]>   (22)

h(x)=aBR]]>   (23)

式中，为加速度计的量测值，其具体表达式见式（17）；为无线电量测值，其具体表达式见式（18）；h(x)包含加速度真实值和无线电测距真实值，其对应的真实表达式见式（17）和式（19）。