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结构的初值给定、地球参数的初始化等