[发明专利]一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法

权利要求书

1.一种基于鲁棒无迹四元数卡尔曼滤波的极区粗对准方法，其特征在于，包括：

获取惯性传感器数据和GPS速度数据；

根据横向坐标系的比力方程和所述GPS速度数据构建横向坐标系下的参考矢量和观测矢量；

根据所述惯性传感器的数据、所述GPS速度数据以及所述横向坐标系下的参考矢量和观测矢量构建无迹卡尔曼滤波模型；

基于实际速度测量值与理想值得关系构建抑制GPS野值的模值匹配权值函数；

基于所述无迹四元数卡尔曼滤波模型与所述抑制GPS野值的模值匹配权值函数构建横向系鲁棒无迹四元数卡尔曼估计器模型；

利用所述横向系鲁棒无迹四元数卡尔曼估计器模型进行极区初始姿态估计，完成粗对准过程。

2.如权利要求1所述的方法，其特征在于，所述采集惯性传感器数据和GPS速度数据包括：

采集所述惯性传感器中的陀螺输出值和加速度计输出值，采集GPS输出速度值；

将所述陀螺输出值、所述加速度计输出值和所述GPS输出速度值由传统坐标系转化为横向坐标系。

3.如权利要求1所述的方法，其特征在于，所述根据横向坐标系的比力方程和所述GPS速度数据构建参考矢量和观测矢量包括：

根据采集的所述惯性传感器数据、所述GPS速度数据以及横向坐标系的比力方程构建横向坐标系下参考矢量和观测矢量；

所述参考矢量为：

所述观测矢量为：

其中，α_M为横向坐标系下参考矢量，β_M为横向坐标系下观测矢量，f^b为采集的传感器加速度计数据，b(0)为初始对准起始时刻载体坐标系，并且相对惯性系不变，b(t_k)为t_k时刻的载体坐标系，为b系姿态矩阵，n_t(0)为初始对准起始时刻横向坐标系，并且相对惯性系不变，n_t(t_k)为t_k时刻的横向导坐标系，为n_t系姿态矩阵，n_t为横向坐标系，为n_t系下得重力加速度矢量，为n_t系下e_t系相对于i系的角速度，e_t为横向地球坐标系，i为惯性坐标系，ω_ie为地球自转角速度，L_t为横向纬度，λ_t为横向经度，为横向坐标系下北向速度，为横向坐标系下东向速度，为横向坐标系下速度，Δt_g为GPS采样时间间隔，Δt_s为SINS的采样时间间隔，且有G为GPS采样间隔和SINS采样间隔的关系系数，为正实数集合，t_M为GPS的采样时间，M为更新时间间隔的次数，t_M＝t＝MΔt_g，Δt_g＝t_k+1-t_k，k＝0,1，2，3，...,M-1。

4.如权利要求3所述的方法，其特征在于，所述无迹卡尔曼滤波模型包括预测模型和量测模型；

所述无迹卡尔曼滤波的预测模型为：

其中，为陀螺仪的输出，ε^b(t)为陀螺零偏，η为量测噪声，是均值为零的高斯白噪声，为微分后的陀螺零偏，为横向坐标系姿态方向余弦矩阵，为横向坐标系相对于惯性系的角速度，为横向坐标系姿态方向余弦矩阵的矩阵转置。

将所述无迹卡尔曼滤波的预测模型转换为四元数表示的预测模型，其表达式为：

其中，为横向坐标系k-1时刻的姿态四元数，ε_k-1为k-1时刻的陀螺零偏，为陀螺在b系中的测量角速度，为横向坐标系相对于惯性系的角速度，b为载体坐标系，h为用四元数表示的离散时间姿态运动学方程。

所述无迹卡尔曼滤波的量测模型为：

其中，为的方向余弦矩阵，为t_M时刻的横向坐标系姿态四元数，t_M为GPS的采样时间，M为更新时间间隔的次数，η为量测噪声，T为矩阵转置。