[发明专利]一种基于地磁信息更新的载体航姿测量方法

权利要求书

1.一种基于地磁信息更新的载体航姿测量方法，其特征在于，包括以下步骤：

11)初始化：获取当前时刻t＝k的俯仰角θ_k、翻滚角和航向角ψ_k信息；

12)磁偏角的计算：利用当前时刻t＝k的磁传感器量测数据，解算量测磁航向角α_m.k；根据当前时刻的航向角ψ_k和量测磁航向角α_m,k，计算磁偏角D_k，其计算公式如下：

D_k＝ψ_k-α_m,k；

13)下一时刻的数据获取：获取下一时刻t＝k+1的加速度计、陀螺仪和磁传感器的量测数据；

14)下一时刻航姿信息的测量：利用离散化卡尔曼滤波方法解算下一时刻t＝k+1的俯仰角θ_k+1、翻滚角和航向角ψ_k+1以及量测磁航向角α_m,k+1；

15)磁偏角信息的更新：根据下一时刻t＝k+1的航向角ψ_k+1和量测磁航向角α_m,k+1，更新下一时刻t＝k+1时的磁偏角D_k+1；

16)测量方法的持续执行：返回下一时刻的数据获取步骤，获取时刻t＝k+2的数据信息，并进行时刻t＝k+2时磁偏角的更新。

2.根据权利要求1所述的一种基于地磁信息更新的载体航姿测量方法，其特征在于，所述下一时刻航姿信息的测量步骤中，离散化卡尔曼滤波方法为离散化扩展卡尔曼滤波方法；所述下一时刻航姿信息的测量包括以下步骤：

21)建立离散化的状态方程和量测方程，其表达式如下：

Z_k+1＝h(X_k+1,k+1)+v_k+1＝X_k+1+v_k+1， (3)

ψ_m,k+1＝α_m,k+1+D_k， (7)

其中：状态向量

表示当前时刻t＝k的载体坐标系相对于当地水平坐标系的航向角ψ_k、俯仰角θ_k和翻滚角k表示离散时间点；

函数f表征下一时刻k+1的状态向量x_k+1与当前时刻k的状态向量x_k之间的关系；表示角度的时间变化率；dt为t＝k与t＝k+1之间的时间间隔；和分别为k时刻载体坐标系中标定后陀螺仪在x，y和z轴上方向上量测数据；

函数h表征下一时刻k+1的状态向量x_k+1与量测量z_k+1两者的关系；表示下一时刻k+1的载体坐标系相对于当地水平坐标系的航向角ψ_k+1、俯仰角θ_k+1和翻滚角的量测量；和分别为下一时刻k+1载体坐标系中标定后加速度传感器在x、y和z轴上方向上量测数据；和分别为下一时刻k+1载体坐标系中标定后磁传感器在x、y和z轴上方向上量测数据；和分别为和转换到地理坐标系l三个轴x_l、y_l和z_l上得到量测数据；w_k和v_k+1分别为系统过程噪声和量测噪声，其统计特性如下：

其中，k、j表示离散时间点，Q为系统过程噪声协方差矩阵，δ_kj为Kronecker符号，R为量测噪声协方差矩阵；

22)设定忽略系统过程噪声w_k和量测噪声v_k+1，

由方程(1)-(4)获得状态向量和量测向量的先验估计和其表示如下：

其中，为状态向量在时刻k的后验估计值；

由此得到近似呈线性的状态方程和量测方程为：

其中，X_k+1和Z_k+1分别为状态向量和量测向量在下一时刻k+1的真实值；

B为函数f关于x偏导的Jacobian矩阵，

W为函数f关于w偏导的Jacobian矩阵，

H为函数h关于x偏导的Jacobian矩阵，

V为函数h关于v偏导的Jacobian矩阵，

23)构建一组完整的离散化扩展卡尔曼滤波方法迭代过程，其表示如下：

其中，为k+1时刻状态向量的先验协方差矩阵，P_k为k时刻状态向量的协方差矩阵；

k+1时刻的量测新息为：

结合方程(4)和(16)，可得到，

24)离散化扩展卡尔曼滤波方法测量更新方程如下：

令，

其中，(γ₁,γ₂,γ₃)为传感器特性相关的参数，取正数；

R_k+1＝f(η₁,η₂,η₃)R， (19)

其中，f(η₁,η₂,η₃)为(η₁,η₂,η₃)相关的函数；

其中，K_k为k时刻的滤波增益矩阵，P_k为k时刻的状态向量的协方差矩阵；

25)k+1时刻状态向量X_k+1近似如下：