1、insinit.m 惯导参数结构体初始化(SINS structure array initialization).

% Prototype: ins = insinit(avp0, ts, var1, var2)
% Initialization usages(maybe one of the following methods):
%       ins = insinit(avp0, ts);
%       ins = insinit(avp0, ts, avperr);
%       ins = insinit(qnb0, vn0, pos0, ts);
% Inputs: avp0 - initial avp0 = [att0; vn0; pos0]
%         ts - SIMU sampling interval
%         avperr - avp error setting
% Output: ins - SINS structure array

调用子函数：ethinit.m ；

调用子函数：inslever.m  

% Prototype: ins = inslever(ins, lever)
% Inputs: ins - SINS structure array created by function 'insinit'
%         lever - lever arms, each column stands for a monitoring point
% Output: ins - SINS structure array with lever arm parameters

2、ethinit.m 地球相关参数结构体初始化(The Earth related parameters (structure array) initialization.)

% Prototype: eth = ethinit(pos, vn)
% Inputs: pos - geographic position [lat;lon;hgt]
%         vn - velocity
% Outputs: eth - parameter structure array
%

调用子函数：ethupdate.m

% 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

函数中主要变量：

global glvif nargin<2,  vn = [0; 0; 0];  endif nargin<1,  pos = [0; 0; 0];  endeth.Re = glv.Re; eth.e2 = glv.e2; eth.wie = glv.wie; eth.g0 = glv.g0;eth = ethupdate(eth, pos, vn);eth.wnie = eth.wnie(:);   eth.wnen = eth.wnen(:);eth.wnin = eth.wnin(:);   eth.wnien = eth.wnien(:);eth.gn = eth.gn(:);       eth.gcc = eth.gcc(:);

 

3、insupdate.m

% SINS Updating Alogrithm including attitude, velocity and position updating.

（1）首先对地球、载体相关参数进行外推以双子样为例，nts=2*ts，外推ts;

主要是利用：

• 前一时刻速度、加速度、前一时刻位置；外推得到nts时刻的速度、位置
• 利用外推得到的速度、位置去更新地球相关参数，主要为：子午圈、卯酉圈半径；东北天下wnie分量（与纬度有关）、wnen分量（与速度、位置有关）、重力加速度（与纬度、高度有关）、等信息

（2）速度更新：以双子样为例，nts=2*ts

程序中，首先根据子样数进行圆锥补偿，得到dvbm，即速度增量；

然后根据速度更新公式：

其中速度更新公式中的一项如下：dvbm/Δt就等于括号中的内容，也就是下面的 ins.fb

也就是ins.fn=qmulv(ins.qnb,ins.fb);然后通过旋转矢量进行变换；

（3）位置更新：以双子样为例，nts=2*ts

利用上面外推nts时刻得到的地球参数和速度更新得到的速度；利用梯形积分得到位置；

（4）姿态更新：以双子样为例，nts=2*ts

利用四元数进行姿态更新；

其中用到了三角函数和单位四元数关系：

捷联惯导更新算法：

% 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

    nn = size(imu,1);nts = nn*ins.ts;  nts2 = nts/2;  ins.nts = nts;[phim, dvbm] = cnscl(imu,0);    % coning & sculling compensation
%     [phim, dvbm] = cnscl0(imu);    % coning & sculling compensationphim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts;  % calibration%% earth & angular rate updating vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2;  % extrapolation at t1/2ins.eth = ethupdate(ins.eth, pos01, vn01);ins.wib = phim/nts; ins.fb = dvbm/nts;  % same as trjsimuins.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/2时刻，所以除以2%% (1)velocity updatingins.fn = qmulv(ins.qnb, ins.fb);
%     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;vn1 = ins.vn + ins.an*nts;%% (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;
%     ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3);  % 2014-11-30ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;ins.pos = ins.pos + ins.Mpvvn*nts;  ins.vn = vn1;ins.an0 = ins.an;%% (3)attitude updatingins.Cnb0 = ins.Cnb;
%     ins.qnb = qupdt(ins.qnb, ins.wnb*nts);  % lower accuracy than next lineins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);[ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);ins.avp = [ins.att; ins.vn; ins.pos];

4、psinstypedef .m，卡尔曼数据融合时，选择状态和量测的数目

首先定义一个结构体：

global psinsdef
    psinsdef = [];

结构体中主要的成员变量为：tag、kfinit、kffk、 kfhk、 kfplot; 根据输入的数字，对转移矩阵、量测矩阵、画图进行相应的计算，其对应的子函数分别为：

• kffk.m 计算离散、连续转移矩阵 [Fk, Ft] = kffk(ins, varargin)

% Establish Kalman filter system transition matrix.
%
% Prototype: [Fk, Ft] = kffk(ins, fkno, varargin)
% Inputs: ins - SINS structure array, if not struct then nts=ins;
%         fkno - type NO. to get Ft, but fkno=0 for specific demand
%         varargin - if any other parameters
% Outputs: Fk - discrete-time transition matrix
%          Ft - continuous-time transition matirx

•  kfhk.m 计算量测矩阵

% Establish Kalman filter measurement matrix.
%
% Prototype: Hk = kfhk(ins, varargin)
% Inputs: ins - SINS structure array from function 'insinit'
%         varargin - if any other parameters
% Output: Hk - measurement matrix

 

5、分配内存函数 prealloc.m

所有的变量都有相同的行数，根据指定的列数，进行分配！

调用如下：

[avp, xkk, zkk] = prealloc(fix(len/nn), 10, 10, 7);

    for k=1:nargoutif nargin==2  % if all of the outputs share the same columnvarargout{k} = zeros(row, varargin{1});elsevarargout{k} = zeros(row, varargin{k});endend

6、利用矢量替换矩阵对角线元素 setdiag.m

常用于设置陀螺仪和加速度计的比例因子参数；

function A = setdiag(A, D)
% Replace the diagonals of a matrix with a vector/scale.
%
% Prototype: A = setdiag(A, D)
% Inputs: A - matrix
%         D - new diagonals
% Output: A - diagonals with 'D'% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/08/2015len = length(A);D = ones(len,1).*D;for k=1:lenA(k,k) = D(k);end

7、输出等于输入子函数 setvals.m

输入等于输出；用于设置变量值；

[ins.tauG, ins.tauA] = setvals(inf(3,1)); % gyro & acc correlation time

函数实现：

function varargout = setvals(varargin)
% Set several output variables to corresponding input values.
%
% Prototype: varargout = setvals(varargin)
%
% See also  prealloc, varpack.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 07/03/2014for k=1:nargoutif nargin==1  % all the outputs are set to the same input valueif iscell(varargin{1})  % avoid nesting varargin, be careful !if iscell(varargin{1}{1})varargout{k} = varargin{1}{1}{k};elsevarargout{k} = varargin{1}{k};endelsevarargout{k} = varargin{1};endelsevarargout{k} = varargin{k};endend

8、rv2m.m 旋转矢量到姿态方向矩阵

 

 例如：Cnn=rv2m(-eth.wnie*nts/2);  其代码实现如下：

% Convert rotation vector to transformation matrix.
%
% Prototype: m = rv2m(rv)
% Input: rv - rotation vector
% Output: m - corresponding DCM, such that
%     m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2
%     where rvx is the askew matrix or rv.

9、