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 parameter
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 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(:)
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 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];
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: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
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/2015
len = length(A);
D = ones(len,1).*D;
for k=1:len
A(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/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
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、
评论(0)
您还未登录,请登录后发表或查看评论