PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-初始化设置

test_SINS_GPS_153源码

主函数

% SINS/GPS intergrated navigation simulation using kalman filter.
% Please run 'test_SINS_trj.m' to generate 'trj10ms.mat' beforehand!!!
% See also  test_SINS_trj, test_SINS, test_SINS_GPS_186, test_SINS_GPS_193.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/06/2011
glvs
psinstypedef(153);
trj = trjfile('trj10ms.mat');
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
imuerr = imuerrset(0.03, 100, 0.001, 5);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]);
ins = insinit(avpadderr(trj.avp0,davp0), ts);
% KF filter
rk = poserrset([1;1;3]);
kf = kfinit(ins, davp0, imuerr, rk);
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1;
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '15-state SINS/GPS Simulation.'); 
ki = 1;
for k=1:nn:len-nn+1
    k1 = k+nn-1;  
    wvm = imu(k:k1,1:6);  t = imu(k1,end);
    ins = insupdate(ins, wvm);
    kf.Phikk_1 = kffk(ins);
    kf = kfupdate(kf);
    if mod(t,1)==0
        posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);  % GPS pos simulation with some white noise
        kf = kfupdate(kf, ins.pos-posGPS, 'M');
        [kf, ins] = kffeedback(kf, ins, 1, 'avp');
        avp(ki,:) = [ins.avp', t];
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;
    end
    timebar;
end
avp(ki:end,:) = [];  xkpk(ki:end,:) = []; 
% show results
insplot(avp);
avperr = avpcmpplot(trj.avp, avp);
kfplot(xkpk, avperr, imuerr);

psinstypedef

function psinstypedef(typestr, kffk, kfhk, kfplot)
% PSINS type define (typedef). To shorten or simplify the main 
% program, especially for kalman filter intergated progarmming, 
% users may add their specific kftypestr here, and then add corresponding 
% code in these user-defined functions: kfinit, kffk, kfhk and kfplot.
%
% Prototype: psinstypedef(typestr)
% Inputs: typestr - PSINS type string (or kfinit), which will be used as 
%             identifier in other PSINS related routines.
%         kffk, kfhk, kfplot - related indicators
%
% See also  kfinit, kffk, kfhk, kfplot.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 05/10/2013, 02/02/2015
global psinsdef
    psinsdef = [];
    %% tags
    psinsdef.kfinittag = 1;
    psinsdef.kffktag = 2;
    psinsdef.kfhktag = 3;
    psinsdef.kfplottag = 4;
    %% kfinit
    psinsdef.kfinit93 = 93;
    psinsdef.kfinit96 = 96;
    psinsdef.kfinit153 = 153;
    psinsdef.kfinit156 = 156;
    psinsdef.kfinit183 = 183;
    psinsdef.kfinit186 = 186;
    psinsdef.kfinit193 = 193;
    psinsdef.kfinit196 = 196;
    psinsdef.kfinit331 = 331;
    psinsdef.kfinit343 = 343;
    psinsdef.kfinit346 = 346;
    psinsdef.kfinit373 = 373;
    psinsdef.kfinit376 = 376;
    %% kffk
    psinsdef.kffk9 = 9;
    psinsdef.kffk15 = 15;
    psinsdef.kffk18 = 18;
    psinsdef.kffk19 = 19;
    psinsdef.kffk33 = 33;
    psinsdef.kffk34 = 34;
    psinsdef.kffk37 = 37;
    %% kfhk
    psinsdef.kfhk93 = 93;
    psinsdef.kfhk96 = 96;
    psinsdef.kfhk153 = 153;
    psinsdef.kfhk156 = 156;
    psinsdef.kfhk183 = 183;
    psinsdef.kfhk186 = 186;
    psinsdef.kfhk193 = 193;
    psinsdef.kfhk196 = 196;
    psinsdef.kfhk331 = 331;
    psinsdef.kfhk343 = 343;
    psinsdef.kfhk346 = 346;
    psinsdef.kfhk373 = 373;
    psinsdef.kfhk376 = 376;
    %% kfplot
    psinsdef.kfplot9 = 9;
    psinsdef.kfplot15 = 15;
    psinsdef.kfplot18 = 18;
    psinsdef.kfplot19 = 19;
    psinsdef.kfplot33 = 33;
    psinsdef.kfplot34 = 34;
    psinsdef.kfplot37 = 37;
    %%
    psinsdef.kfinit = 0;
    if ischar(typestr)
        psinsdef.typestr = typestr;
        psinsdef.kffk = 0;
        psinsdef.kfhk = 0;
        psinsdef.kfplot = 0;
    else
        psinsdef.kfinit = typestr;
        psinsdef.kffk = fix(psinsdef.kfinit/10);
        psinsdef.kfhk = psinsdef.kfinit;
        psinsdef.kfplot = psinsdef.kffk;
    end
    if exist('kffk', 'var'), psinsdef.kffk = kffk; end
    if exist('kfhk', 'var'), psinsdef.kfhk = kfhk; end
    if exist('kfplot', 'var'), psinsdef.kfplot = kfplot; end

imuerrset

function imuerr = imuerrset(eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij, KA2, rxyz, dtGA)
% SIMU errors setting, including gyro & acc bias, noise and installation errors, etc.
%
% Prototype: imuerr = imuerrset(eb, db, web, wdb, sqrtR0G, TauG, sqrtR0A, TauA, dKGii, dKAii, dKGij, dKAij, KA2, rxyz, dtGA)
% Inputs: including information as follows
%     eb - gyro constant bias (deg/h)
%     db - acc constant bias (ug)
%     web - angular random walk (deg/sqrt(h))
%     wdb - velocity random walk (ug/sqrt(Hz))
%     sqrtR0G,TauG - gyro correlated bias, sqrtR0G in deg/h and TauG in s
%     sqrtR0A,TauA - acc correlated bias, sqrtR0A in ug and TauA in s
%     dKGii - gyro scale factor error (ppm)
%     dKAii - acc scale factor error (ppm)
%     dKGij - gyro installation error (arcsec)
%     dKAij - acc installation error (arcsec)
%     KA2 - acc quadratic coefficient (ug/g^2)
%       where, 
%               |dKGii(1) dKGij(4) dKGij(5)|         |dKAii(1) 0        0       |
%         dKg = |dKGij(1) dKGii(2) dKGij(6)| , dKa = |dKAij(1) dKAii(2) 0       |
%               |dKGij(2) dKGij(3) dKGii(3)|         |dKAij(2) dKAij(3) dKAii(3)|
%     rxyz - acc inner-lever-arm, =[rx;ry;rz] (cm)
%     dtGA - time-asynchrony between gyro & acc, dtGA=Tacc_later-Tgyro_early>0 (ms)
% Output: imuerr - SIMU error structure array
%
% Example:
%     For inertial grade SIMU, typical errors are:
%       eb=0.01dph, db=50ug, web=0.001dpsh, wdb=10ugpsHz
%       scale factor error=10ppm, askew installation error=10arcsec
%       sqrtR0G=0.001dph, taug=1000s, sqrtR0A=10ug, taug=1000s
%    then call this funcion by
%       imuerr = imuerrset(0.01,100,0.001,10, 0.001,1000,10,1000, 10,10,10,10, 10, 10, 10);
%
% See also  imuadderr, gabias, avperrset, insinit, kfinit.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 11/09/2013, 06/03/2014, 15/02/2015, 22/08/2015, 17/08/2016, 24/07/2020
global glv
    if nargin==1  % for specific defined case 
        switch eb
            case 1,  imuerr = imuerrset(0.01, 100, 0.001, 10);
            case 2,  imuerr = imuerrset(0.01,100,0.001,10, 0.001,300,10,300, 10,10,10,10, 10, 2, 1);
        end
        return;
    end
    o31 = zeros(3,1); o33 = zeros(3);
    imuerr = struct('eb',o31, 'db',o31, 'web',o31, 'wdb',o31,...
        'sqg',o31, 'taug',inf(3,1), 'sqa',o31, 'taua',inf(3,1), 'dKg',o33, 'dKa',o33, 'dKga',zeros(15,1),'KA2',o31); 
    %% constant bias & random walk
    if ~exist('web', 'var'), web=0; end
    if ~exist('wdb', 'var'), wdb=0; end
    imuerr.eb(1:3) = eb*glv.dph;   imuerr.web(1:3) = web*glv.dpsh;
    imuerr.db(1:3) = db*glv.ug;    imuerr.wdb(1:3) = wdb*glv.ugpsHz;
    %% correlated bias
    if exist('sqrtR0G', 'var')
        if TauG(1)==inf, imuerr.sqg(1:3) = sqrtR0G*glv.dphpsh;   % algular rate random walk !!!
        elseif TauG(1)>0, imuerr.sqg(1:3) = sqrtR0G*glv.dph.*sqrt(2./TauG); imuerr.taug(1:3) = TauG; % Markov process
        end
    end
    if exist('sqrtR0A', 'var')
        if TauA(1)==inf, imuerr.sqa(1:3) = sqrtR0A*glv.ugpsh;   % specific force random walk !!!
        elseif TauA(1)>0, imuerr.sqa(1:3) = sqrtR0A*glv.ug.*sqrt(2./TauA); imuerr.taua(1:3) = TauA; % Markov process
        end
    end
    %% scale factor error
    if exist('dKGii', 'var')
        imuerr.dKg = setdiag(imuerr.dKg, dKGii*glv.ppm);
    end
    if exist('dKAii', 'var')
        imuerr.dKa = setdiag(imuerr.dKa, dKAii*glv.ppm);
    end
    %% installation angle error
    if exist('dKGij', 'var')
        dKGij = ones(6,1).*dKGij*glv.sec;
        imuerr.dKg(2,1) = dKGij(1); imuerr.dKg(3,1) = dKGij(2); imuerr.dKg(3,2) = dKGij(3); 
        imuerr.dKg(1,2) = dKGij(4); imuerr.dKg(1,3) = dKGij(5); imuerr.dKg(2,3) = dKGij(6);
    end
    if exist('dKAij', 'var')
        dKAij = ones(3,1).*dKAij*glv.sec;
        imuerr.dKa(2,1) = dKAij(1); imuerr.dKa(3,1) = dKAij(2); imuerr.dKa(3,2) = dKAij(3); 
    end
    imuerr.dKga = [imuerr.dKg(:,1); imuerr.dKg(:,2);   imuerr.dKg(:,3);
                   imuerr.dKa(:,1); imuerr.dKa(2:3,2); imuerr.dKa(3,3)];
    %% acc 2nd scale factor error
    if exist('KA2', 'var')
        imuerr.KA2(1:3) = KA2*glv.ugpg2; 
    end
    %% acc inner-lever-arm error
    if exist('rxyz', 'var')
        if length(rxyz)==1, rxyz(1:6)=rxyz; end
        if length(rxyz)==6, rxyz(7:9)=[0;0;0]; end 
        imuerr.rx = rxyz(1:3)/100; imuerr.ry = rxyz(4:6)/100; imuerr.rz = rxyz(7:9)/100;
    end
    %% time-asynchrony between gyro & acc
    if exist('dtGA', 'var')
        imuerr.dtGA = dtGA/1000; 
    end
               

imuadderr

function imu = imuadderr(imu, imuerr)
% SIMU outputs adding errors simulation, denoted as:
%    imu = K*imu + drift error.
%
% Prototype: imu = imuadderr(imu, imuerr, ts)
% Inputs: imu - raw SIMU data
%         imuerr - SIMU error struture array
% Output: imu - output SIMU data added errors
%
% See also  imuerrset, imuclbt, imudeldrift, imuasyn, imugsensi, avperrset, trjsimu, insupdate.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 11/09/2013, 06/03/2014, 21/07/2015, 17/08/2016, 24/02/2017, 24/07/2020
    ts = imu(2,end)-imu(1,end);  % the last column implies sampling interval
    if isfield(imuerr, 'rx')  % inner-lever-arm effect for accelerometers
        wb = imu(:,1:3)/ts;
        dotwf = imudot(imu, 5.0);
        fL = [ (-dotwf(:,2).^2-dotwf(:,3).^2+0)*imuerr.rx(1) + (dotwf(:,1).*dotwf(:,2)-wb(:,3))*imuerr.rx(2) + (dotwf(:,1).*dotwf(:,3)+wb(:,2))*imuerr.rx(3), ... 
               (dotwf(:,1).*dotwf(:,2)+wb(:,3))*imuerr.ry(1) + (-dotwf(:,1).^2-dotwf(:,3).^2+0)*imuerr.ry(2) + (dotwf(:,2).*dotwf(:,3)-wb(:,1))*imuerr.ry(3), ... 
               (dotwf(:,1).*dotwf(:,3)-wb(:,2))*imuerr.rz(1) + (dotwf(:,2).*dotwf(:,3)+wb(:,1))*imuerr.rz(2) + (-dotwf(:,1).^2-dotwf(:,2).^2+0)*imuerr.rz(3)  ]; 
        imu(:,4:6) = imu(:,4:6) + fL*ts;
    end
    if isfield(imuerr, 'dtGA') % time-asynchrony between gyro & acc
        acc = [ [imu(1,4:6),imu(1,7)-ts]; imu(:,4:7); [repmat(imu(end,4:6),10,1),imu(end,7)+(1:10)'*ts] ];
        acc(:,1:3) = cumsum(acc(:,1:3),1);
        t = [imu(1,end)-ts; imu(:,end)] + imuerr.dtGA;
        for k=1:3
            acc1 = interp1(acc(:,end), acc(:,k), t, 'linear');
            imu(:,3+k) = diff(acc1,1);
        end
    end
    [m, n] = size(imu); sts = sqrt(ts);
    drift = [ ts*imuerr.eb(1) + sts*imuerr.web(1)*randn(m,1), ...
              ts*imuerr.eb(2) + sts*imuerr.web(2)*randn(m,1), ...
              ts*imuerr.eb(3) + sts*imuerr.web(3)*randn(m,1), ...
              ts*imuerr.db(1) + sts*imuerr.wdb(1)*randn(m,1), ...
              ts*imuerr.db(2) + sts*imuerr.wdb(2)*randn(m,1), ...
              ts*imuerr.db(3) + sts*imuerr.wdb(3)*randn(m,1) ];
    if min(abs(imuerr.sqg))>0
        mvg = markov1(imuerr.sqg.*sqrt(imuerr.taug/2), imuerr.taug, ts, m);   % q = 2*sigma.^2.*beta
        drift(:,1:3) = drift(:,1:3) + mvg*ts;
    end
    if min(abs(imuerr.sqa))>0
        mva = markov1(imuerr.sqa.*sqrt(imuerr.taua/2), imuerr.taua, ts, m);
        drift(:,4:6) = drift(:,4:6) + mva*ts;
    end
    if min(abs(imuerr.KA2))>0
        imu(:,4:6) = [ imu(:,4)+imuerr.KA2(1)/ts*imu(:,4).^2, ...
                       imu(:,5)+imuerr.KA2(2)/ts*imu(:,5).^2, ...
                       imu(:,6)+imuerr.KA2(3)/ts*imu(:,6).^2 ];
    end
    if isfield(imuerr, 'dKg')
        Kg = eye(3)+imuerr.dKg; Ka = eye(3)+imuerr.dKa;
        imu(:,1:6) = [imu(:,1:3)*Kg', imu(:,4:6)*Ka'];
    end
    imu(:,1:6) = imu(:,1:6) + drift;

avperrset

function avperr = avperrset(phi, dvn, dpos)
% avp errors setting.
%
% Prototype: avperr = avperrset(phi, dvn, dpos)
% Inputs: phi - platform misalignment angles. all in arcmin
%         dvn - velocity errors in m/s
%         dpos - position errors dpos=[dlat;dlon;dhgt], all in m
% Output: avperr = [phi; dvn; dpos]
% 
% See also  poserrset, vperrset, avpadderr, imuerrset, avpset, insupdate, avperrstd.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/03/2014
global glv
    avperr = [rep3(phi)*glv.min; vperrset(dvn,dpos)];

avpadderr

function avp = avpadderr(avp0, davp, israd, phimu)
% avp add some errors, it can be denoted as avp=avp0+davp, where
% avp0=[att0;vn0;pos0] and davp=[phi;dvn;dpos].
%
% Prototype: avp = avpadderr(avp0, davp, israd)
% Inputs: avp0 - avp0=[att0;vn0;pos0], avp initial values
%         davp - davp=[phi;dvn;dpos], avp errors
%         israd - phi/lat/lon unit is in rad
%         phimu - att err is phi or mu
% Output: avp - avp=[att; vn; pos], avp with errors
% 
% See also  avperrset, imuadderr, qaddphi, avpcmp, avpinterp, insupdate.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 07/03/2014, 16/10/2020
global glv
    if nargin<4, phimu=1; end
    if nargin<3, israd=1; end
    if length(davp)<6, davp = [davp(1:3);zeros(6,1)]; % att err
    elseif length(davp)<9, davp = [davp(1:3);zeros(3,1);davp(4:6)]; end % att&pos err
    avp0 = avp0(:); davp = davp(:);
    if israd==0
        [phi, dvn, dpos] = setvals(davp(1:3).*[glv.sec;glv.sec;glv.min], davp(4:6), davp(7:9)./[glv.Re;glv.Re*cos(avp0(7));1]);
    else
        [phi, dvn, dpos] = setvals(davp(1:3), davp(4:6), davp(7:9));
    end
    if phimu==1
        avp(1:3,1) = q2att(qaddphi(a2qua(avp0(1:3)), phi));
    else
        avp(1:3,1) = aaddmu(avp0(1:3)', phi);
    end
    avp(4:9,1) = avp0(4:9)+[dvn; dpos];

insinit

function ins = insinit(avp0, ts, var1, var2)
% 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
%
% See also  insupdate, avpset, kfinit.

% 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
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 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;

流程

1.glvs首先初始化全局变量
2.psinstypedef(153)初始化KF的全局变量，比如状态转移矩阵、量测系数矩阵维数等的定义
3.trj = trjfile('trj10ms.mat')加载轨迹文件，其中有仿真的IMU原始数据以及AVP信息
4.[nn, ts, nts] = nnts(2, trj.ts)设置子样数和采样间隔
5.imuerr = imuerrset(0.03, 100, 0.001, 5)设置imu误差，包括陀螺和加速度计常值零偏、角度随机游走、速度随机游走
6.imu = imuadderr(trj.imu, imuerr)为IMU真实测量值增加误差
7.davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3])设置初始avp误差
8.ins = insinit(avpadderr(trj.avp0,davp0), ts)初始化INS结构体，其中包含avp结构的初值给定、地球参数的初始化等