test_SINS_GPS_153源码
poserrset
function poserr = poserrset(dpos0, dlon, dhgt)
% position errors dpos=[dlat;dlon;dhgt] setting.
%
% Prototype: poserr = poserrset(dpos0)
% Input: dpos0=[dlat; dlon; dhgt], NOTE: dlat, dlon and dhgt are all in m.
% Output: poserr=[dpos0(1)/Re; dpos0(2)/Re; dpos0(3)], so poserr(1)=dlat,
% poserr(2)=dlon are in rad and poserr(3)=dhgt is in m.
%
% See also avperrset, vperrset, posset.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/03/2014
global glv
if nargin==3, dpos0=[dpos0;dlon; dhgt]; end % dpos = poserrset(dlat, dlon, dhgt)
if nargin==2, dpos0=[dpos0;dpos0; dlon]; end % dpos = poserrset(dlat_dlon, dhgt)
dpos0 = rep3(dpos0);
poserr = [dpos0(1:2)/glv.Re; dpos0(3)];
kfinit
function kf = kfinit(ins, varargin)
% Kalman filter initializes for structure array 'kf', this precedure
% usually includs the setting of structure fields: Qt, Rk, Pxk, Hk.
%
% Prototype: kf = kfinit(ins, varargin)
% Inputs: ins - SINS structure array, if not struct then nts=ins;
% varargin - if any other parameters
% Output: kf - Kalman filter structure array
%
% See also kfinit0, kfsetting, kffk, kfkk, kfupdate, kffeedback, psinstypedef.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/10/2013
global glv psinsdef
[Re,deg,dph,ug,mg] = ... % just for short
setvals(glv.Re,glv.deg,glv.dph,glv.ug,glv.mg);
o33 = zeros(3); I33 = eye(3);
kf = [];
if isstruct(ins), nts = ins.nts;
else nts = ins;
end
switch(psinsdef.kfinit)
case psinsdef.kfinit153,
psinsdef.kffk = 15; psinsdef.kfhk = 153; psinsdef.kfplot = 15;
[davp, imuerr, rk] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;
kf.Rk = diag(rk)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;
kf.Hk = kfhk(0);
case psinsdef.kfinit156,
psinsdef.kffk = 15; psinsdef.kfhk = 156; psinsdef.kfplot = 15;
[davp, imuerr, rk] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;
kf.Rk = diag(rk)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;
kf.Hk = kfhk(0);
case psinsdef.kfinit183,
psinsdef.kffk = 18; psinsdef.kfhk = 183; psinsdef.kfplot = 18;
[davp, imuerr, lever, r0] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+3,1)])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever]*1.0)^2;
kf.Hk = zeros(3,18);
case psinsdef.kfinit186,
psinsdef.kffk = 18; psinsdef.kfhk = 186; psinsdef.kfplot = 18;
[davp, imuerr, lever, r0] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1)])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever]*1.0)^2;
kf.Hk = zeros(6,18);
case psinsdef.kfinit193
psinsdef.kffk = 19; psinsdef.kfhk = 193; psinsdef.kfplot = 19;
[davp, imuerr, lever, dT, r0] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; [1/Re;1/Re;1]*glv.mpsh; ...
[1;1;1]*0*glv.dphpsh; [1;1;1]*0*glv.ugpsh; [1;1;1]*0.*glv.mpsh; 0])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT]*1.0)^2;
kf.Hk = zeros(3,19);
case psinsdef.kfinit196
psinsdef.kffk = 19; psinsdef.kfhk = 196; psinsdef.kfplot = 19;
[davp, imuerr, lever, dT, r0] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; [1/Re;1/Re;1]*0*glv.mpsh; ...
[1;1;1]*0*glv.dphpsh; [1;1;1]*0*glv.ugpsh; [1;1;1]*0*glv.mpsh; 0])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT]*1.0)^2;
kf.Hk = zeros(6,19);
case psinsdef.kfinit331,
psinsdef.kffk = 33; psinsdef.kfhk = 331; psinsdef.kfplot = 33;
[davp, imuerr, r0] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+15+3,1)])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; imuerr.dKga; imuerr.KA2])^2;
kf.Hk = kfhk(ins);
kf.xtau(1:psinsdef.kffk,1) = 0;
case psinsdef.kfinit346,
psinsdef.kffk = 34; psinsdef.kfhk = 346; psinsdef.kfplot = 34;
[davp, imuerr, lever, dT, r0] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+3+1+15,1)])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT; imuerr.dKga])^2;
kf.Hk = kfhk(ins);
kf.xtau(1:psinsdef.kffk,1) = 0;
case psinsdef.kfinit376,
psinsdef.kffk = 37; psinsdef.kfhk = 376; psinsdef.kfplot = 37;
[davp, imuerr, lever, dT, r0] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+3+1+15+3,1)])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT; imuerr.dKga; davp(4:6)]*10)^2;
kf.Hk = kfhk(ins);
kf.xtau(1:psinsdef.kffk,1) = 0;
otherwise,
kf = feval(psinsdef.typestr, psinsdef.kfinittag, [{ins},varargin]);
end
kf = kfinit0(kf, nts);
gabias
function bias = gabias(gbias, abias)
% Gyro & Acc constant bias setting.
%
% Prototype: Gyro & Acc constant bias setting
% Inputs: gbias - gyro bias, in deg/h
% abias - acc bias, in ug
% Output: bias - = [gbias; abias]
%
% See also imuerrset, avpset, insupdate.
% Copyright(c) 2009-2021, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 06/02/2021
global glv
bias = [rep3(gbias)*glv.dph; rep3(abias)*glv.ug];
kfupdate
function kf = kfupdate(kf, yk, TimeMeasBoth)
% Discrete-time Kalman filter.
%
% Prototype: kf = kfupdate(kf, yk, TimeMeasBoth)
% Inputs: kf - Kalman filter structure array
% yk - measurement vector
% TimeMeasBoth - described as follows,
% TimeMeasBoth='T' (or nargin==1) for time updating only,
% TimeMeasBoth='M' for measurement updating only,
% TimeMeasBoth='B' (or nargin==2) for both time and
% measurement updating.
% Output: kf - Kalman filter structure array after time/meas updating
% Notes: (1) the Kalman filter stochastic models is
% xk = Phikk_1*xk_1 + wk_1
% yk = Hk*xk + vk
% where E[wk]=0, E[vk]=0, E[wk*wk']=Qk, E[vk*vk']=Rk, E[wk*vk']=0
% (2) If kf.adaptive=1, then use Sage-Husa adaptive method (but only for
% measurement noise 'Rk'). The 'Rk' adaptive formula is:
% Rk = b*Rk_1 + (1-b)*(rk*rk'-Hk*Pxkk_1*Hk')
% where minimum constrain 'Rmin' and maximum constrain 'Rmax' are
% considered to avoid divergence.
% (3) If kf.fading>1, then use fading memory filtering method.
% (4) Using Pmax&Pmin to constrain Pxk, such that Pmin<=diag(Pxk)<=Pmax.
%
% See also kfinit, kfupdatesq, kffk, kfhk, kfc2d, kffeedback, kfplot, RLS, ekf, ukf.
% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/12/2012, 29/08/2013, 16/04/2015, 01/06/2017, 11/03/2018
if nargin==1;
TimeMeasBoth = 'T';
elseif nargin==2
TimeMeasBoth = 'B';
end
if TimeMeasBoth=='T' % Time Updating
kf.xk = kf.Phikk_1*kf.xk;
kf.Pxk = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
else
if TimeMeasBoth=='M' % Meas Updating
kf.xkk_1 = kf.xk;
kf.Pxkk_1 = kf.Pxk;
elseif TimeMeasBoth=='B' % Time & Meas Updating
kf.xkk_1 = kf.Phikk_1*kf.xk;
kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
else
error('TimeMeasBoth input error!');
end
kf.Pxykk_1 = kf.Pxkk_1*kf.Hk';
kf.Py0 = kf.Hk*kf.Pxykk_1;
kf.ykk_1 = kf.Hk*kf.xkk_1;
kf.rk = yk-kf.ykk_1;
if kf.adaptive==1 % for adaptive KF, make sure Rk is diag 24/04/2015
for k=1:kf.m
if yk(k)>1e10, continue; end % 16/12/2019
ry = kf.rk(k)^2-kf.Py0(k,k);
if ry<kf.Rmin(k,k), ry = kf.Rmin(k,k); end
if ry>kf.Rmax(k,k), kf.Rk(k,k) = kf.Rmax(k,k);
else kf.Rk(k,k) = (1-kf.beta)*kf.Rk(k,k) + kf.beta*ry;
end
end
kf.beta = kf.beta/(kf.beta+kf.b);
end
kf.Pykk_1 = kf.Py0 + kf.Rk;
kf.Kk = kf.Pxykk_1*invbc(kf.Pykk_1); % kf.Kk = kf.Pxykk_1*kf.Pykk_1^-1;
kf.xk = kf.xkk_1 + kf.Kk*kf.rk;
kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk';
kf.Pxk = (kf.Pxk+kf.Pxk')*(kf.fading/2); % symmetrization & forgetting factor 'fading'
% if kf.adaptive==1
% krrk = kf.Kk*kf.rk*kf.rk'*kf.Kk';
% for k=3:3
% krrki = krrk(k,k) + kf.Pxk(k,k) - kf.Pxkk_1(k,k);
% if krrki>kf.Qmax(k,k), kf.Qk(k,k) = kf.Qmax(k,k);
% elseif krrki<kf.Qmin(k,k), kf.Qk(k,k) = (1-kf.beta)*kf.Qk(k,k) + kf.beta*kf.Qmin(k,k);
% else kf.Qk(k,k) = (1-kf.beta)*kf.Qk(k,k) + kf.beta*krrki;
% end
% end
% end
if kf.xconstrain==1 % 16/3/2018
for k=1:kf.n
if kf.xk(k)<kf.xmin(k)
kf.xk(k)=kf.xmin(k);
elseif kf.xk(k)>kf.xmax(k)
kf.xk(k)=kf.xmax(k);
end
end
end
if kf.pconstrain==1 % 1/6/2017
for k=1:kf.n
if kf.Pxk(k,k)<kf.Pmin(k)
kf.Pxk(k,k)=kf.Pmin(k);
elseif kf.Pxk(k,k)>kf.Pmax(k)
ratio = sqrt(kf.Pmax(k)/kf.Pxk(k,k));
kf.Pxk(:,k) = kf.Pxk(:,k)*ratio; kf.Pxk(k,:) = kf.Pxk(k,:)*ratio;
end
end
end
end
流程
1.rk = poserrset([1;1;3])设置位置测量误差
2.kf = kfinit(ins, davp0, imuerr, rk)卡尔曼滤波器初始化，主要初始化一些方阵，如系统状态噪声方差阵Q、量测噪声方差阵R、误差协方差阵P以及观测矩阵H
3.kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2设置方差阵最小值，包括位置误差和陀螺、加速度计的常值零偏
4.kf.pconstrain=1表示对方差阵上下限进行约束
5.len = length(imu)求IMU原始测量值的行数
6.[avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1)循环使用变量前，先为变量分配内存，其中第一个参数为行数，第二个参数、第三个参数分别为第一个变量、第二个变量的列数
7.timebar(nn, len, '15-state SINS/GPS Simulation.')程序进度条
8.ki = 1量测更新计数
9.for k=1:nn:len-nn+1控制惯导更新循环
10.k1 = k+nn-1控制多子样数据输入
11.wvm = imu(k:k1,1:6); t = imu(k1,end)角增量、速度增量以及时间赋值
12.ins = insupdate(ins, wvm)惯导更新
13.kf.Phikk_1 = kffk(ins)计算离散时间转移矩阵
14.kf = kfupdate(kf)卡尔曼滤波器的时间更新
15.if mod(t,1)==0每整秒的时候，使用GNSS测量值进行组合导航
16.posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1)用白噪声来仿真GNSS位置
17. kf = kfupdate(kf, ins.pos-posGPS, 'M')卡尔曼滤波器的量测更新
18. [kf, ins] = kffeedback(kf, ins, 1, 'avp')卡尔曼滤波状态估计反馈给捷联惯导系统
19. avp(ki,:) = [ins.avp', t]存储姿态、速度和位置信息
20. xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'存储反馈后剩余状态、方差阵以及时间
21. ki = ki+1更新组合导航量测更新计数
22. avp(ki:end,:) = []; xkpk(ki:end,:) = [] 清除未利用的分配空间
扫一扫
371
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
