一种两级分离融合的姿态和航向估计方法

摘要

本发明公开了一种两级分离融合的姿态和航向估计方法，包括：建立系统的卡尔曼滤波状态方程和测量方程：其中x_k和y_k分别表示系统的状态向量和观测向量，w_k和v_k分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；f为前后时刻状态向量之间的传递函数；解算上述卡尔曼滤波方程得到状态向量根据计算得到俯仰角θ和横滚角φ。本发明方法将姿态角和航向角的解算进行两级分离计算，构建的线性滤波方程可以避免扩展卡尔曼滤波和无迹卡尔曼滤波等非线性滤波方法的使用，将极大降低系统计算复杂度。同时，第一级卡尔曼滤波负责利用陀螺仪和加速度计进行姿态角的解算，磁传感器不参与第一级滤波，排除了磁传感器所受干扰对于姿态角估计的影响。

主权项

一种两级分离融合的姿态和航向估计方法，其特征在于，采用两级分离的卡尔曼滤波进行陀螺仪和加速度计的数据融合，包括：建立航向和姿态参考系统的卡尔曼滤波状态方程和测量方程：其中x_k和y_k分别表示航姿参考系统的状态向量和观测向量，w_k和v_k分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；A表示状态向量之间的传递矩阵；解算上述卡尔曼滤波方程得到状态向量根据上述状态向量计算得到俯仰角θ和横滚角φ，其中：$\theta =-arctan\left(\frac{g_x^b}{\sqrt{{\left(g_y^b\right)}^2+{\left(g_z^b\right)}^2}}\right),\phi =arctan\left(\frac{g_y^b}{g_z^b}\right).$