PSINS insupdate理解与代码注释

function ins = insupdate(ins, imu)
% SINS Updating Alogrithm including attitude, velocity and position
% 捷联惯导更新算法，核心！！！
% updating.
%
% Prototype: ins = insupdate(ins, imu)
% Inputs: ins - SINS structure array created by function 'insinit'
%         imu - gyro & acc incremental sample(s)
% Output: ins - SINS structure array after updating
%
% See also  insinit, cnscl, earth, inspure, trjsimu, imuadderr, avpadderr, q2att,
%           inslever, alignvn, aligni0, etm, kffk, kfupdate, insplot.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/03/2008, 12/01/2013, 18/03/2014, 09/09/2014
    nn = size(imu,1);
    nts = nn*ins.ts;  nts2 = nts/2;  ins.nts = nts;
    [phim, dvbm] = cnscl(imu,0);    % coning & sculling compensation 
    % phim计算补偿圆锥误差后的 用多子样角增量 表示的 等效旋转矢量，用于更新前后两时刻b系间的姿态矩阵
    % dvbm加上旋转误差和划桨误差修正后的比力速度增量式的一部分，见式4.1.50 理解为比力速度增量
    phim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts;  % calibration 标定
    %为什么没有引入imu的Kg、Ka（虽然这个例子中imu也没有设，但是程序不具有通用性），
    %也没有引入imu常值零偏，前面imu数据里是添加了常值零偏和随机游走的
    %是不是可以理解为：如果标定出了系统误差和常值零偏，可以在此步骤进行校准？书中没有此步骤的理论依据？
    %% earth & angular rate updating 
    vn01 = ins.vn+ins.an*nts2; %an则是n系下的运动加速度：加上科里奥利加速度、向心加速度等伪力
    pos01 = ins.pos+ins.Mpv*vn01*nts2;  % extrapolation at t1/2 向前插值
    %Mpv是位置更新算法中位置微分与速度之间的系数矩阵，见式4.1.58
    %Mpv、MpvCnb、Mpvvn很重要，分别是位置关于速度的关系矩阵、其在{n}坐标系下的表示、由该矩阵算出来的各个方向速度增量，在位置更新中会用到。
    if ins.openloop==0, ins.eth = ethupdate(ins.eth, pos01, vn01);
    elseif ins.openloop==1, ins.eth = ethupdate(ins.eth, ins.pos0, ins.vn0); end %ethupdate更新地球物理参数，包括有害加速度gcc
    %不太明白ins.openloop是什么含义
    ins.wib = phim/nts;
    ins.fb = dvbm/nts; %b系下的比力等于比力速度增量除以采样时间
    % same as trjsimu 计算补偿后的角速度和速度
    ins.web = ins.wib - ins.Cnb'*ins.eth.wnie;
%     ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin;
    ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin;  % 2014-11-30 不太理解
    %% (1)velocity updating
    ins.fn = qmulv(ins.qnb, ins.fb);
    %ins.qnb是b系到n系的转换四元数，此命令将b系下的比力输出转换为n系下的比力输出
    %因为速度更新方程源于n系下的比力微分方程
%     ins.an = qmulv(rv2q(-ins.eth.wnin*nts2),ins.fn) + ins.eth.gcc;
    ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc; %对应式4.1.26
    % 右边第一项对应式4.1.50，差了一个采样时间，后面又乘上了，中间的坐标系转换矩阵在上面ins.fn = qmulv(ins.qnb, ins.fb);实现
    %将fn(fnib)在n坐标系中往下一个时刻传播再加上有害加速度gcc，gcc在上面ethupdate中更新
    %先除以时间是因为 eth中存储的是gcc有害加速度，为了和有害加速度做加和 先求出这部分对应的加速度形式
    ins.anbar = 0.9*ins.anbar + 0.1*ins.an; %anbar为时间间隔内的平均值，不太理解这一步
    vn1 = ins.vn + ins.an*nts;%从这里分析an应该是 n系下 比力速度增量和有害速度增量的和 除以时间间隔，即比力和有害加速度的和
    %% (2)position updating
%     ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];
    ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh;
    %Mpv是位置更新算法中位置微分与速度之间的系数矩阵，见式4.1.58
%     ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3);  % 2014-11-30
    ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;%Mpvvn即式4.1.60右边第二项除以T
    ins.pos = ins.pos + ins.Mpvvn*nts;   %见式4.1.60
    ins.vn = vn1;%因为位置更新同时需要前后两个速度值（56行），所以使用完后，才在这一步将新的速度存入ins结构体中完成更新
    ins.an0 = ins.an;
    %% (3)attitude updating
    ins.Cnb0 = ins.Cnb;
%     ins.qnb = qupdt(ins.qnb, ins.wnb*nts);  % lower accuracy than the next line
    ins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);
    %姿态更新式的四元数表示方式，见式4.1.8，
    %可以理解为qupdt2(上一时刻Cnb的等效旋转矢量, b系姿态更新矩阵的等效旋转矢量, n系姿态更新阵的等效旋转矢量)
    %ins.eth.wnin*nts可见式4.1.11
    [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);%输出各种形式的姿态表示并存入ins结构体
    ins.avp = [ins.att; ins.vn; ins.pos];%将更新完的姿态 速度 位置存入ins结构体，更新完成