PSINS工具箱函数介绍——insinit

insinit是初始化INS系统的函数

函数需要用到PSINS工具箱，关于工具箱的一些入门知识，参考这篇文章，是关于工具箱的讲解：

• PSINS初学指导：https://blog.csdn.net/callmeup/article/details/137087932

函数使用方法

正常的使用方法如下：

ins = insinit(avpadderr(trj.avp0,davp0), ts);

其中，avpadderr用于生成初始的avp误差。ts是采样间隔。
作为例程，“avpadderr(trj.avp0,davp0)”的内容为：
是一个9维的列向量，分别是姿态角（3维）、速度（三维）、位置（三维，纬度经度高度）的初始误差。
函数生成的ins是一个结构体，如下：

包含几乎所有的关于惯导系统的参数，其中：

1. ts是采样间隔
2. qnb是四元数
3. vn是三轴速度
4. pos是三轴位置
5. att是三轴姿态角
6. vn0/pos0/att0分别是上面三个值对应的初始值
   等等

源代码

代码不算很长，如下：

function ins = insinit(avp0, ts, var1, var2)
global glv
    avp0 = avp0(:);
    if length(avp0)==1, avp0=zeros(9,1); end  % ins = insinit(0, ts);
    if length(avp0)==4, avp0=[0;0;avp0(1); 0;0;0; avp0(2:4)]; end  % ins = insinit([yaw;pos], ts);
    if length(avp0)==6, avp0=[avp0(1:3); 0;0;0; avp0(4:6)]; end  % ins = insinit([att;pos], ts);
    if length(avp0)==7, avp0=[0;0;avp0]; end  % ins = insinit([yaw;vn;pos], ts);
    if nargin==2      % ins = insinit(avp0, ts);
        [qnb0, vn0, pos0] = setvals(a2qua(avp0(1:3)), avp0(4:6), avp0(7:9));
    elseif nargin==3  % ins = insinit(avp0, ts, avperr);
        avperr = var1;
        avp0 = avpadderr(avp0, avperr);
        [qnb0, vn0, pos0] = setvals(a2qua(avp0(1:3)), avp0(4:6), avp0(7:9));
	elseif nargin==4  % ins = insinit(qnb0, vn0, pos0, ts);
        [qnb0, vn0, pos0, ts] = setvals(avp0, ts, var1, var2);
    end        
	ins = [];
	ins.ts = ts; ins.nts = 2*ts;
    [ins.qnb, ins.vn, ins.pos] = setvals(qnb0, vn0, pos0); ins.vn0 = vn0; ins.pos0 = pos0;
	[ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);  ins.Cnb0 = ins.Cnb;
    ins.avp  = [ins.att; ins.vn; ins.pos];
    ins.eth = ethinit(ins.pos, ins.vn);
	% 'wib,web,fn,an,Mpv,MpvCnb,Mpvvn,CW' may be very useful outside SINS,
    % so we calucate and save them.
    ins.wib = ins.Cnb'*ins.eth.wnin;
    ins.fn = -ins.eth.gn;  ins.fb = ins.Cnb'*ins.fn;
	[ins.wnb, ins.web, ins.an] = setvals(zeros(3,1));
	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; 
	[ins.Kg, ins.Ka] = setvals(eye(3)); % calibration parameters
    [ins.eb, ins.db] = setvals(zeros(3,1));
    [ins.tauG, ins.tauA] = setvals(inf(3,1)); % gyro & acc correlation time
    ins.lever = zeros(3,1); ins = inslever(ins); % lever arm
	ins.tDelay = 0; % time delay
    ins.openloop = 0;
    glv.wm_1 = zeros(3,1)';  glv.vm_1 = zeros(3,1)';  % for 'single sample+previous sample' coning algorithm
    ins.an0 = zeros(3,1);  ins.anbar = ins.an0;

代码解析

其中，eth也是一个结构体，使用函数$ins.eth=ethinit(ins.pos,ins.vn);$生成，存放关于地球有关的函数。