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

摘要

本发明实施例公开了一种基于磁偏角、磁倾角卡尔曼滤波导航方法。所述方法包括：构建载体坐标系，并确定载体坐标系到NED导航坐标系的转换矩阵；采用四元数，三个位置，三个速度，三轴加速度计零偏，三轴陀螺零偏，一个磁偏角，一个磁倾角，共18个状态量作为系统状态；构建基于磁偏角和磁倾角的卡尔曼滤波系统的状态方程；基于卡尔曼滤波计算载体的速度和位置。另外，在卡尔曼滤波进行UD序贯更新，对矩阵D对角线元素进行尺度变换，计算完成之后，再变换回之前的尺度，能够避免出现计算误差。本发明实施例的技术方案充分考虑磁偏角和磁倾角的影响，具有较高的鲁棒性和扩展性。

主权项

1.一种基于磁偏角、磁倾角的卡尔曼滤波导航方法，其特征在于，所述方法包含以下步骤：步骤一：设置载体坐标系，所述载体坐标系三轴表示为记作xyz；步骤二：通过惯性传感器确定当前时刻的载体姿态，记作att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；步骤三：确定载体坐标系向导航坐标系的转换矩阵，记作简化转换矩阵为以下形式步骤四：采用四元数q＝[q0 q1 q2 q3]，三个位置p＝[pn pe pd]，三个速度v＝[vn ve vd]，三轴加速度计零偏ba＝[bax bay baz]，三轴陀螺零偏bω＝[bωx bωy bωz]，一个磁偏角α，一个磁倾角γ这18个状态量作为系统状态，记作X＝[q0 q1 q2 q3 pn pe pd vn ve vd bax bay baz bωx bωy bωz α γ]T；步骤五：构建基于磁偏角、磁倾角的卡尔曼滤波；步骤六：根据所述基于磁偏角、磁倾角的卡尔曼滤波进行载体速度、位置的解算。