[发明专利]一种基于磁偏角、磁倾角的卡尔曼滤波导航方法

权利要求书

1.一种基于磁偏角、磁倾角的卡尔曼滤波导航方法，其特征在于，所述方法包含以下步骤：

步骤一：设置载体坐标系，所述载体坐标系三轴表示为记作xyz；

步骤二：通过惯性传感器确定当前时刻的载体姿态，记作att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；

步骤三：确定载体坐标系向导航坐标系的转换矩阵，记作

简化转换矩阵为以下形式

步骤四：采用四元数q＝[q₀ q₁ q₂ q₃]，三个位置p＝[p_n p_e p_d]，三个速度v＝[v_n v_ev_d]，三轴加速度计零偏b_a＝[b_ax b_ay b_az]，三轴陀螺零偏b_ω＝[b_ωx b_ωy b_ωz]，一个磁偏角α，一个磁倾角γ这18个状态量作为系统状态，记作X＝[q₀ q₁ q₂ q₃ p_n p_e p_d v_n v_e v_d b_ax b_ayb_az b_ωx b_ωy b_ωz α γ]^T；

步骤五：构建基于磁偏角、磁倾角的卡尔曼滤波系统，所述卡尔曼滤波系统的状态方程为：

其中，为系统状态的导数，f(X)为系统状态的函数，w为系统噪声，Z为系统的量测状态，h(X)为系统的量测状态函数，v为量测噪声；

记公式(3)具体化为：

其中，为机体向四元数的转换矩阵；

设量测数据中位置Z_p＝[Z_pn Z_pe Z_pd]^T，速度为Z_v＝[Z_vn Z_ve Z_vd]^T，机体的三轴磁场强度为Z_m＝[Z_mx Z_my Z_mz]^T，B_e为地球磁场矢量和，其为一已知常量；

则系统的量测向量可以表示为

Z＝[Z_pn Z_pe Z_pd Z_vn Z_ve Z_vd Z_mx Z_my Z_mz]^T (6)

对f(X)和h(X)求解雅克比矩阵得到矩阵F和H，可得系统线性化后的状态方程；

步骤六：根据所述基于磁偏角、磁倾角的卡尔曼滤波进行载体速度、位置的解算。

2.根据权利要求1所述的方法，其特征在于，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定；所述导航坐标系为NED导航坐标系。