[发明专利]一种用于废气激光雷达的姿态估计方法

摘要

本发明提供一种用于废气激光雷达的姿态估计方法，包括步骤：初始化参数设定，包括设定初始四元数初始误差协方差矩阵状态随机游走噪声协方差判定阈值系数α、检测窗宽度W和增益调整系数β；本发明充分考虑了系统的能耗问题与外部加速度的干扰问题，能够在降低外部加速度对系统的干扰的同时，降低系统整体能耗；本发明能够对物体的运动状态进行实时的监测，并且能够有效对抗加速度计存在异常的情况，具有较强的稳定性与鲁棒性。

主权项

1.一种用于废气激光雷达的姿态估计方法，其特征在于，所述方法包括步骤：步骤一、初始化参数设定，包括设定初始四元数初始误差协方差矩阵状态随机游走噪声协方差判定阈值系数α、检测窗宽度W和增益调整系数β；步骤二、读取t时刻的陀螺仪数据y_g，t、磁强计数据y_m，t和加速度计数据y_a，t，并将所述加速度数据y_a，t存储到栈Y_a，t中，其中Y_a，t＝{y_a，t‑W+1，y_a，t‑W+2，…，y_a，t}；步骤三、计算检测函数和检测函数的理论方差其中，为加速度计本体白噪声的方差，||y_a，i||表示加速度数据y_a，t的二范数，g为重力加速度；步骤四、根据所述步骤三得到的f_t与判断物体对象的运动状态，具体判定依据为：判断是否有如果是，则判定物体对象处于静止状态，否则判定物体对象处于运动状态；步骤五、根据所述步骤四判断的运动状态，计算先验估计与先验估计协方差当物体处于静止状态时，令当物体处于运动状态时，令其中，Φ_t＝exp(Ω_t·T)T为采样时间间隔，R_g为陀螺仪误差方差矩阵，I₃表示三阶单位阵；步骤六、计算滤波增益K_t：其中，H_t＝[H_1，t；H_2，t]′R_a表示加速度计噪声方差矩阵，R_m表示磁力计噪声方差矩阵；G＝[0，0，g]′；m＝[m_x，m_y，m_z]＝[||m||·cosθ，0，||m||·sinθ]′，m表示在世界坐标系下的地磁场向量，||m||表示m的二范数，θ表示磁场倾角；步骤七、计算后验估计和后验估计协方差