1.一种用于惯性-地磁组合的快速扩展卡尔曼算法,其特征在于:所述惯性-地磁组合包括三轴加速度计、三轴陀螺仪以及三轴地磁传感器,还包括以下步骤:步骤1:构建重力加速度矢量gr和地磁场矢量mr在当地地理坐标系下的表示,即:gr=[0,0,-1]T,mr=[cos(90°-θgm),0,-sin(90°-θgm)]T (1)其中,θgm为gr和mr的空间夹角,随后利用gr和mr构建两个相互正交的空间矢量,即:r1′=(gr+mr)/[2cos(θgm/2)],r2′=(gr-mr)/[2sin(θgm/2)] (2)然后,将上述r1′和r′2进行空间旋转,使得r1′和r′2的测量轴与陀螺仪的测量轴一致,即:其中,qrot=cos(θgm/4)+sin(θgm/4)·(b′1×b′2)/|b′1×b′2|,后续步骤会将r1′和r′2作为姿态解算时的参考矢量;
步骤2:在每个采样时刻k,由三轴加速度计和三轴地磁传感器的输出分别构建r1和r2在载体坐标系下的观测值,即gb和mb;随后利用gb和mb构建两个正交矢量,即:b′1=(gb+mb)/[2cos(θgm/2)],b′2=(gb-mb)/[2sin(θgm/2)] (4)然后,对b′1和b′2进行空间旋转,以使得旋转后的两个正交矢量的测量轴与陀螺仪一致,即:扩展卡尔曼算法的过程方程和观测方程通常分别具有如下形式,过程方程为:
观测方程为:
其中,fs为采样频率; 为由k时刻的陀螺仪三轴向输出ωx、ωy和ωz构成的三维反对称阵;Φk命名为状态转换矩阵;qk和qk+1分别为k时刻和k+1时刻由四元数表示的载体姿态;wk为高斯白噪声,其协方差其中I4×4为四维单位矩阵,σw为陀螺仪每个轴向上的输出噪声的标准差,进一步地,如果定义姿态的一步预测估计为qk+1/k=Φkqk=[q0 q1i q2j q3k],那么:旋转矩阵:
和 为互不相关的观测噪声,其协方差分别为, 和 其中σa和σm分别为b1和b2在每个轴向上的输出噪声的标准差;
扩展卡尔曼算法的姿态递推迭代方程为:式中,卡尔曼增益矩阵 其中Pk+1/k为利用k时刻的姿态及k+1时刻的陀螺仪输出对k+1时刻的姿态进行一步预测的预测协方差,为线性化后的观测矩阵, 为观测方程的观测误差协方差;
步骤3:利用r1和r2,将Fk+1化简为:步骤4:令姿态验后估计误差协方差Pk/k=α2I4×4,随后利用Pk/k将Pk+1/k简化为:步骤5:令 其中max()函数表示取括号内的两者的最大值,然后将矩阵 简化为:
其中,
将c/4Δk合理地略去,在略去c/4Δk之后,矩阵 的逆矩阵简化为:其中,
步骤6:将矩阵 简化为:
步骤7:利用步骤5和步骤6的结果将卡尔曼增益简化为:并最终将扩展卡尔曼算法的姿态递推迭代方程简化为:其中,
返回步骤2,直至用户终止运算。
2.如权利要求1所述的用于惯性-地磁组合的快速扩展卡尔曼算法,其特征在于:对Pk/k=α2I4×4具体解释为:由于初始时刻时载体是静止的,利用单采样点姿态解算算法,解算出载体的初始姿态,此时,扩展卡尔曼算法的初始姿态估计误差设置为:忽略高阶项,得到:
其中,
利用式(16)-式(18),Pk/k可以由P0/0迭代计算出:上式中,变量的说明已经在解释式(6)-式(19)时给出。