% 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% 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 glv
if nargin<2, vn = [0; 0; 0]; end
if nargin<1, pos = [0; 0; 0]; end
eth.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(:);
% SINS Updating Alogrithm including attitude, velocity and position updating.
（1）首先对地球、载体相关参数进行外推以双子样为例，nts=2*ts，外推ts;
主要是利用：
（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 compensation
phim = 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/2
ins.eth = ethupdate(ins.eth, pos01, vn01);
ins.wib = phim/nts; ins.fb = dvbm/nts; % 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/2时刻，所以除以2
%% (1)velocity updating
ins.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-30
ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;
ins.pos = ins.pos + ins.Mpvvn*nts;
ins.vn = vn1;
ins.an0 = ins.an;
%% (3)attitude updating
ins.Cnb0 = ins.Cnb;
% ins.qnb = qupdt(ins.qnb, ins.wnb*nts); % lower accuracy than next line
ins.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];
首先定义一个结构体：
global psinsdef
psinsdef = [];
结构体中主要的成员变量为：tag、kfinit、kffk、 kfhk、 kfplot; 根据输入的数字，对转移矩阵、量测矩阵、画图进行相应的计算，其对应的子函数分别为：
% 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% 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
所有的变量都有相同的行数，根据指定的列数，进行分配！
调用如下：
[avp, xkk, zkk] = prealloc(fix(len/nn), 10, 10, 7);
for k=1:nargout
if nargin==2 % if all of the outputs share the same column
varargout{k} = zeros(row, varargin{1});
else
varargout{k} = zeros(row, varargin{k});
end
end常用于设置陀螺仪和加速度计的比例因子参数；
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/2015
len = length(A);
D = ones(len,1).*D;
for k=1:len
A(k,k) = D(k);
end输入等于输出；用于设置变量值；
[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/2014
for k=1:nargout
if nargin==1 % all the outputs are set to the same input value
if iscell(varargin{1}) % avoid nesting varargin, be careful !
if iscell(varargin{1}{1})
varargout{k} = varargin{1}{1}{k};
else
varargout{k} = varargin{1}{k};
end
else
varargout{k} = varargin{1};
end
else
varargout{k} = varargin{k};
end
end
例如：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、