多项式角运动下的圆锥误差补偿

传统主流的姿态更新求解方法是,先使用 陀螺角增量的多子样采样计算等效旋转矢量,补偿转动不可交换误差, 再使用等效旋转矢量计算姿态更新四元数。但是前两节的多子样算法都是在 Bortz方程二阶近似的基础上进行推导的,从源头上看不可避免地存在原理性误差,使得在大机动环境下选用的子样数越多,精度往往越差。本节将直接利用 Bortz方程给出基于多项式迭代的等效旋转矢量精确数值解的算法,这样可以有效避免原理性近似误差。

角运动的多项式描述

在实际捷联惯导系统中,大多数陀螺采样直接获得的是角增量输出,而本节算法需要用到角速度作为输入,因此,在角运动为多项式形式假设条件下先给出由角增量信息构造角速度的方法。

常值角速度(零次曲线)

假设在时间段[0,T]内﹐载体运动角速度为常值形式,即

线性角速度(一次曲线)

假设在时间段[0,T]内﹐载体运动角速度为线性形式,即

抛物线角速度(二次曲线)

假设在时间段[0,T]内﹐载体运动角速度为抛物线形式,即

严老师 的PSINS源码中也有相关阐述：

function dphim = conepolyn(wm)
% Calculation of noncommutativity error using polynomial compensation 
% method. Ref. Qin Yongyuan 'Inertial Navigation' P314.
%
% Prototype: dphim = conepolyn(wm)
% Input: wm - gyro angular increments
% Output: dphim - noncommutativity error compensation vector
%
% See also  conetwospeed, conedrift, scullpolyn, cnscl.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/03/2014, 16/02/2017
    n = size(wm,1);
    if n==1
        dphim = [0,0,0];
    elseif n==2
        dphim = 2/3*cros(wm(1,:),wm(2,:));
    elseif n==3
        dphim = ...
            33/80*cros(wm(1,:),wm(3,:)) + ...
            57/80*cros(wm(2,:),wm(3,:)-wm(1,:));
    elseif n==4
        dphim = ...
            736/945*(cros(wm(1,:),wm(2,:))+cros(wm(3,:),wm(4,:))) + ...
            334/945*(cros(wm(1,:),wm(3,:))+cros(wm(2,:),wm(4,:))) + ...
            526/945*cros(wm(1,:),wm(4,:)) + ...
            654/945*cros(wm(2,:),wm(3,:));
    elseif n==5
        dphim = ...
            123425/145152*(cros(wm(1,:),wm(2,:))+cros(wm(4,:),wm(5,:))) + ...
             34875/145152*(cros(wm(1,:),wm(3,:))+cros(wm(3,:),wm(5,:))) + ...
             90075/145152*(cros(wm(1,:),wm(4,:))+cros(wm(2,:),wm(5,:))) + ...
             66625/145152* cros(wm(1,:),wm(5,:)) + ...
            103950/145152*(cros(wm(2,:),wm(3,:))+cros(wm(3,:),wm(4,:))) + ...
             55400/145152* cros(wm(2,:),wm(4,:));
    elseif n==6
        dphim = ...
            9.225974023727258e-01*(cros(wm(1,:),wm(2,:))+cros(wm(5,:),wm(6,:))) + ...
            8.639610528915165e-02*(cros(wm(1,:),wm(3,:))+cros(wm(4,:),wm(6,:))) + ...
            7.733225109265687e-01*(cros(wm(1,:),wm(4,:))+cros(wm(3,:),wm(6,:))) + ...
            3.930627701652648e-01*(cros(wm(1,:),wm(5,:))+cros(wm(2,:),wm(6,:))) + ...
            5.317640683291427e-01* cros(wm(1,:),wm(6,:)) + ...
            7.627597403941779e-01*(cros(wm(2,:),wm(3,:))+cros(wm(4,:),wm(5,:))) + ...
            3.400757575106209e-01*(cros(wm(2,:),wm(4,:))+cros(wm(3,:),wm(5,:))) + ...
            5.909848488909383e-01* cros(wm(2,:),wm(5,:)) + ...
            7.071861474024891e-01* cros(wm(3,:),wm(4,:));
    else
        dphim = [0,0,0];
%         error('no suitable compensation in conepolyn');
    end

等效旋转矢量的迭代求解

根据前述分析,迭代式(2.7.15)具有5阶精度﹐高于多子样算法。研究还表明，第i次迭代可达到i＋1阶等效旋转矢量求解精度，因此迭代算法的收敛速度是比较快的。当然，与多子样算法相比,此处算法的计算量是比较大的,进行一次完整的等效旋转矢量求解约需上千次乘法运算。