[发明专利]一种基于失准角的卡尔曼滤波姿态估计方法

摘要

本发明实施例公开了一种基于失准角的卡尔曼滤波姿态估计方法。所述方法包括：构建载体坐标系，并确定载体坐标系到NED导航坐标系的转换矩阵；采用东向水平失准角、北向水平失准角和陀螺漂移作为系统状态，以水平加速度作为量测状态；根据失准角方程构建卡尔曼滤波系统的状态方程；基于卡尔曼滤波实现载体姿态的估计。另外，综合考虑载体运动状态，确定是否对载体姿态进行修正。本发明实施例的技术方案根据失准角方程构建4维状态量的卡尔曼滤波，在保证姿态估计精确度的同时，降低了系统运算复杂度，并且基于载体运动状态，判断是否进行姿态修正，能够进一步提高运算效率，提高系统的鲁棒性。

主权项

1.一种基于失准角的卡尔曼滤波姿态估计方法，其特征在于，所述方法包含以下步骤：步骤一：设置载体坐标系，所述载体坐标系三轴表示为记作xyz；步骤二：通过惯性传感器确定当前时刻的载体姿态，记作att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；步骤三：确定载体坐标系向导航坐标系的转换矩阵，记作简化转换矩阵为以下形式步骤四：采用东向水平失准角北向水平失准角和陀螺漂移ω_bx,ω_by作为系统状态，记作采用水平加速度fx,fy作为量测状态，记作Z＝[fy fx]T；步骤五：构建基于失准角的卡尔曼滤波；步骤六：根据所述基于失准角的卡尔曼滤波进行载体姿态的估计。