捷联惯导数值更新算法-姿态更新+速度更新+位置更新

姿态更新

捷联惯导数值更新算法通常可划分为姿态、速度和位置更新三部分﹐姿态更新算法是核心,其求解精度对整个捷联惯导的精度起着决定性的作用。目前主流的姿态更新求解方法是，先使用陀螺角增量的多子样采样计算等效旋转矢量,补偿转动不可交换误差,再使用等效旋转矢量计算姿态更新四元数。

地球自转角速度和牵连角速度

在考虑n系相对于i系的旋转时，一定要考虑这两部分:地球自转引起的n系旋转，以及惯导系统在地球表面附近移动因地球表面弯曲而引起的n系旋转

更新姿态

姿态更新可采用四元数连乘来实现
也可由严老师书中提到的方向余弦矩阵计算得到

速度更新

比力方程

比力方程是在地球表面附近进行惯性导航解算的基本方程

上式后两项可统称为有害加速度。比力方程式表明,只有在加速度计输出中扣除有害加速度后,才能获得运载体在导航系下的几何运动加速度,对加速度积分一次可得速度，再积分一次可得位置。因此,比力方程是惯导解算的基本方程。

PSINS源码

严老师在PSINS中也给出了相应的代码，计算了有害加速度

function eth = ethupdate(eth, pos, vn)
% Update the Earth related parameters, much faster than 'earth'.
%
% Prototype: eth = ethupdate(eth, pos, vn)
% Inputs: eth - input earth structure array
%         pos - geographic position [lat;lon;hgt]
%         vn - velocity
% Outputs: eth - parameter structure array
%
% See also  ethinit, earth.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 23/05/2014
    if nargin==2,  vn = [0; 0; 0];  end
    eth.pos = pos;  eth.vn = vn;
    eth.sl = sin(pos(1));  eth.cl = cos(pos(1));  eth.tl = eth.sl/eth.cl; 
    eth.sl2 = eth.sl*eth.sl;  sl4 = eth.sl2*eth.sl2;
    sq = 1-eth.e2*eth.sl2;  RN = eth.Re/sqrt(sq); 
    eth.RNh = RN+pos(3);  eth.clRNh = eth.cl*eth.RNh;
    eth.RMh = RN*(1-eth.e2)/sq+pos(3);
%     eth.wnie = [0; eth.wie*eth.cl; eth.wie*eth.sl];
    eth.wnie(2) = eth.wie*eth.cl; eth.wnie(3) = eth.wie*eth.sl;
%     eth.wnen = [-vn(2)/eth.RMh; vn(1)/eth.RNh; vn(1)/eth.RNh*eth.tl];
    eth.wnen(1) = -vn(2)/eth.RMh; eth.wnen(2) = vn(1)/eth.RNh; eth.wnen(3) = eth.wnen(2)*eth.tl;
%     eth.wnin = eth.wnie + eth.wnen;
    eth.wnin(1) = eth.wnie(1) + eth.wnen(1); eth.wnin(2) = eth.wnie(2) + eth.wnen(2); eth.wnin(3) = eth.wnie(3) + eth.wnen(3); 
%     eth.wnien = eth.wnie + eth.wnin;
    eth.wnien(1) = eth.wnie(1) + eth.wnin(1); eth.wnien(2) = eth.wnie(2) + eth.wnin(2); eth.wnien(3) = eth.wnie(3) + eth.wnin(3);
%     eth.gn = [0;0;-eth.g];
    eth.g = eth.g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80
    eth.gn(3) = -eth.g;
%     eth.gcc = eth.gn - cros(eth.wnien,vn); % Gravitational/Coriolis/Centripetal acceleration
%     eth.gcc =  [ eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);  % faster than previous line
%                  eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
%                  eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3) ];
    eth.gcc(1) = eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);
    eth.gcc(2) = eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
    eth.gcc(3) = eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3);

双子样假设

本篇博文以双子样算法为例，推导速度更新方程。
双子样假设:[tk-2,tk]时段内角速度观测量和比力观测量均随时间线性变化。如图2所示，在两个采样区间内，将实际的角速度和比力(黑色曲线)近似为随时间线性变化(曲线简化为直线，红色虚线，黑色曲线下的积分面积为惯性传感器实际输出的角增量或速度增量)。上式表明，线性假设的系数 a、b、c、d不但决定了角速度和比力的大小，同时还决定了其方向，也即线性假设也考虑了角速度向量和比力向量的方向变化。

如下是双子样假设下的角速度和比力随时间变化函数

速度更新算法

速度更新中，有两项积分非常重要，一个是比力积分项，另一个就是重力/哥式积分项

那么可推导出双子样速度更新公式：

对于中间时刻(前一时刻和当前时刻的中间时刻)的速度求解，可采用线性外推方法：

对位置也可进行如上外推。

划桨误差补偿算法

下式描述的运动与现实生活划船中的划桨运动状况非常相似：一方面桨绕船身的横轴做往复角运动,另一方面船身连带船桨沿纵轴做间歇性加速线运动。可见,划船过程中船桨同时存在周期性的角运动和线运动,因此形象地称下式所描述的运动为划桨运动(sculling motion)。

推导可得N子样划桨误差补偿算法：

PSINS源码

严老师同样在源码中给出了这个算法：

 if coneoptimal==0
                scullm = [0, 0, 0];
            else
                scullm = 1/12*(cros(glv.wm_st,vm)+cros(glv.vm_st,wm));  glv.wm_st = wm; glv.vm_st = vm;
            end
        else
            vmm = sum(vm,1);
            if coneoptimal==0
                sm = glv.cs(n-1,1:n-1)*vm(1:n-1,:);
                scullm = (cros(cm,vm(n,:))+cros(sm,wm(n,:)));
            else  % else: using polynomial fitting sculling compensation method
                scullm = scullpolyn(wm, vm);
            end
        end

其中

 sm = glv.cs(n-1,1:n-1)*vm(1:n-1,:);
 scullm = (cros(cm,vm(n,:))+cros(sm,wm(n,:)));

这一块代码，为N子样划桨误差补偿算法的实现

位置更新

计算公式

可采用严老师书中的公式计算

也可参考牛老师的讲义中的公式

但需要注意的是，不同的公式，对应的不同的b系得定义，严老师定义东北天为正，牛老师定位北东地为正

PSINS源码

严老师在源码中也给出了相应得矩阵形式

ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];
ins.MpvCnb = ins.Mpv*ins.Cnb;  ins.Mpvvn = ins.Mpv*ins.vn;