卡尔曼滤波与SINS/GNSS组合导航
用于Kalman滤波的函数有：
psinstypedef(nnm)：KF状态维数nn/量测维数m定义（一般可直接用于SINS/GNSS组合），或用户自定义字符串，在kfinit/kffk/kfhk/kfplot等函数中作为类型区分标识；
kfinit：滤波器主要参数初始化，再用kfinit0更多的参数初始化；
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);
kfinit0函数
function kf = kfinit0(kf, nts)
% Always called by kfinit and initialize the remaining fields of kf.
%
% See also kfinit, kfupdate, kffeedback, psinstypedef.
kf.nts = nts;
[kf.m, kf.n] = size(kf.Hk);
kf.I = eye(kf.n);
kf.Kk = zeros(kf.n, kf.m);
if ~isfield(kf, 'xk'), kf.xk = zeros(kf.n, 1); end
if ~isfield(kf, 'Qk'), kf.Qk = kf.Qt*kf.nts; end
if ~isfield(kf, 'Gammak'), kf.Gammak = 1; kf.l = kf.n; end
if ~isfield(kf, 'fading'), kf.fading = 1; end
if ~isfield(kf, 'adaptive'), kf.adaptive = 0; end
% if kf.adaptive==1
if ~isfield(kf, 'b'), kf.b = 0.5; end
if ~isfield(kf, 'beta'), kf.beta = 1; end
if ~isfield(kf, 'Rmin'), kf.Rmin = 0.01*kf.Rk; end
if ~isfield(kf, 'Rmax'), kf.Rmax = 100*kf.Rk; end
if ~isfield(kf, 'Qmin'), kf.Qmin = 0.01*kf.Qk; end
if ~isfield(kf, 'Qmax'), kf.Qmax = 100*kf.Qk; end
% end
if ~isfield(kf, 'xtau'), kf.xtau = ones(size(kf.xk))*eps; end
if ~isfield(kf, 'T_fb'), kf.T_fb = 1; end
if ~isfield(kf, 'fbstr'), kf.fbstr = 'avped'; end
if ~isfield(kf, 'xconstrain'), kf.xconstrain = 0; end
if ~isfield(kf, 'pconstrain'), kf.pconstrain = 0; end
kf.Pmax = (diag(kf.Pxk)+1)*1.0e10;
kf.Pmin = kf.Pmax*0;
kf.xfb = zeros(kf.n, 1);
% kf.coef_fb = (1.0-exp(-kf.T_fb./kf.xtau));
% kf.coef_fb = ar1coefs(kf.T_fb, kf.xtau);
xtau = kf.xtau;
xtau(kf.xtau<kf.T_fb) = kf.T_fb; kf.coef_fb = kf.T_fb./xtau; %2015-2-22
kfsetting：直接设置几种特定类型IMU的滤波器参数；
kffk/kfhk：计算状态转移矩阵/量测矩阵，etm惯导误差传播函数；
function [Fk, Ft] = kffk(ins, varargin)
% Create Kalman filter system transition matrix.
%
% Prototype: [Fk, Ft] = kffk(ins, varargin)
% Inputs: ins - SINS structure array, if not struct then nts=ins;
% varargin - if any other parameters
% Outputs: Fk - discrete-time transition matrix, = Phikk_1
% Ft - continuous-time transition matirx
%
% See also kfhk, kfinit, kfupdate, kfc2d, insupdate, etm, psinstypedef, ekffk.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 06/08/2012, 01/02/2014, 02/08/2016
global psinsdef
%% get Ft
if isstruct(ins), nts = ins.nts;
else nts = ins;
end
switch(psinsdef.kffk)
case 15,
Ft = etm(ins);
case {18,19} % psinsdef.kffkxx, xx=18,19
Ft = etm(ins);
Ft(psinsdef.kffk, psinsdef.kffk) = 0;
case {33}
Ft = etm(ins);
Ft(psinsdef.kffk,psinsdef.kffk) = 0;
% 15+dKg(9)+dKa(6)+KA2(3)
Ft(1:3,16:24) = [-ins.wib(1)*ins.Cnb, -ins.wib(2)*ins.Cnb, -ins.wib(3)*ins.Cnb];
Ft(4:6,25:33) = [ins.fb(1)*ins.Cnb, ins.fb(2)*ins.Cnb(:,2:3), ins.fb(3)*ins.Cnb(:,3), ins.Cnb*diag(ins.fb.^2)];
case {34, 37}
Ft = etm(ins);
Ft(psinsdef.kffk,psinsdef.kffk) = 0;
Ft(1:3,20:28) = [-ins.wib(1)*ins.Cnb, -ins.wib(2)*ins.Cnb, -ins.wib(3)*ins.Cnb];
Ft(4:6,29:34) = [ins.fb(1)*ins.Cnb, ins.fb(2)*ins.Cnb(:,2:3), ins.fb(3)*ins.Cnb(:,3)];
otherwise,
% Ft = feval(psinsdef.typestr, psinsdef.kffktag, {ins, varargin});
Ft = feval(psinsdef.typestr, psinsdef.kffktag, [{ins},varargin]);
end
%% discretization
Fk = Ft*nts;
if nts>0.1 % for large time interval, this may be more accurate.
Fk = expm(Fk);
else % Fk = I + Ft*nts + 1/2*(Ft*nts)^2 , 2nd order expension
Fk = eye(size(Ft)) + Fk;% + Fk*Fk*0.5;
end
kfhk函数
function Hk = kfhk(ins, varargin)
% Create 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
%
% See also kffk, kfinit, kfupdate, kfc2d, insupdate.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 27/03/2014, 02/02/2015
global psinsdef
switch(psinsdef.kfhk)
case 153,
Hk = [zeros(3,6), eye(3), zeros(3,6)];
case 156
Hk = [zeros(6,3), eye(6), zeros(6,6)];
case 183 % glv.psinsdef.kfhkxx3
Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb];
case 186
Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW;-ins.MpvCnb]];
case 193 % glv.psinsdef.kfhkxx3
Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb, -ins.Mpvvn];
case 196
Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW,-ins.an;-ins.MpvCnb,-ins.Mpvvn]];
case 331
Hk = zeros(1,33);
case 343 % glv.psinsdef.kfhkxx3
Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb, -ins.Mpvvn, zeros(3,15)];
case 373
Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb, -ins.Mpvvn, zeros(3,18)];
case 346
Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW,-ins.an;-ins.MpvCnb,-ins.Mpvvn], zeros(6,15)];
case 376
Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW,-ins.an;-ins.MpvCnb,-ins.Mpvvn], zeros(6,15), [-eye(3);zeros(3)]];
% case {186, 196, 346} % glv.psinsdef.kfhkxx6
% Hk = [zeros(6,3), eye(6), zeros(6,fix(psinsdef.kfhk/10)-9)];
% Hk(1:3,16:19) = [-ins.CW, -ins.an];
% Hk(4:6,16:19) = [-ins.MpvCnb, -ins.Mpvvn];
% case 376
% Hk = [zeros(6,3), eye(6), zeros(6,fix(psinsdef.kfhk/10)-9)];
% Hk(1:3,[16:19,35:37]) = [-ins.CW, -ins.an, -eye(3)];
% Hk(4:6,16:19) = [-ins.MpvCnb, -ins.Mpvvn];
otherwise,
Hk = feval(psinsdef.typestr, psinsdef.kfhktag, [{ins},varargin]);
end
kfupdate：KF时间/量测更新，采用序贯量测自适应/方差限制方法；
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 - Kalman filter structure array，表示卡尔曼滤波的结构数组
yk - measurement vector，量测向量
TimeMeasBoth -T表示只进行时间更新、M表示只进行量测更新，B表示两者同时进行
kffeedback：滤波状态反馈，可用部分反馈方法；
function [kf, ins, xfb] = kffeedback(kf, ins, T_fb, fbstr)
% Kalman filter state estimation feedback to SINS.
%
% Prototype: [kf, ins] = kffeedback(kf, ins, T_fb)
% Inputs: kf - Kalman filter structure array
% ins - SINS structure array
% T_fb - feedback time interval
% fbstr - feedback string
% Outputs: kf, ins - Kalman filter & SINS structure array after feedback
% xfb - feedback state value
%
% See also kfinit, kffk, kfhk, kfplot, psinstypedef.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 05/10/2013, 06/02/2021
其中参数含义为：
kf - Kalman filter structure array，卡尔曼滤波结构数组
ins - SINS structure array，SINS结构数组
T_fb - feedback time interval，反馈时间间隔
fbstr - feedback string，反馈字符串，若要返馈位置，则输入p，返馈姿态则输入a,速度则是v，具体可参见源码
kfstat：滤波误差分配与可观测度分析（协方差分析）；
滤波误差分配与可观测度分析：
做双位置对准的时候，选择了三个失准角，三个速度误差，三个陀螺漂移，三个加表零偏，对其进行初始对准；最后可以得到状态的 可观测都度
kfplot/xpplot/rvpplot：滤波结果绘图；
sinsgps：典型的193或196维SINS/GNSS松组合函数；
POSProcessing：343或346维SINS/GNSS双向滤波松组合函数，POSFusion对双向滤波结果做加权平均，posplot作图
典型的SINS/GNSS组合滤波
该功能可由sinsgps函数实现，源码中给出了使用该函数的示例:
19-state SINS/GNSS integrated navigation Kalman filter.
% The 19-state includes:
% [phi(3); dvn(3); dpos(3); eb(3); db(3); lever(3); dT(1)]
% The 3- or 6- measurements are:
% [dvn(3)] or [dvn(3); dpos(3)]
%
% Prototype: [avp, xkpk, zkrk, sk, ins, kf] = sinsgps(imu, gps, ins, davp, imuerr, lever, dT, rk, Pmin, Rmin, fbstr, isfig)
% Inputs: imu - IMU array [wm, vm, t]
% gps - GNSS array [vn, pos, t] or [pos, t];
% ins - ins array, set by function 'insinit'
% davp - AVP array for P0 setting
% imuerr - set by function 'imuerrset', for P0 and Qk setting
% lever - lever arm from IMU to GNSS, if lever(4)=0 then Pk(lever)=0 for no lever estimation
% dT - time delay from IMU to GNSS, if dT(2)=0 then Pk(dT)=0 for no time delay estimation
% rk - measurement noise std(dpos) or std([dvn;dpos])
% Pmin - Pmin setting, Pmin<=0 for no Pmin constrain
% Rmin - Rmin setting, Rmin<=0 for no adaptive KF, Rmin=0~1 scale for adaptive KF and Rmin = Rk*Rmin
% fbstr - KF feedback string from 'avpedLT'
% isfig - figure flag
%
% Example 1:
% [avp1, xkpk, zkrk, sk, ins1, kf1] = sinsgps(imu, gps, 300);
%
% Example 2:
% ins = insinit([yaw;pos], ts);
% avperr = avperrset([60;300], 1, 100);
% imuerr = imuerrset(0.03, 100, 0.001, 1);
% Pmin = [avperrset([0.1,1],0.001,0.01); gabias(0.1, [10,30]); [0.01;0.01;0.01]; 0.0001].^2;
% Rmin = vperrset(0.001, 0.01).^2;
% [avp1, xkpk, zkrk, sk, ins1, kf1] = sinsgps(imu, gps, ins, avperr, imuerr, rep3(1), 0.01, vperrset(0.1,10), Pmin, Rmin, 'avp');
%
% Example 3:
% t0 = 1; t1 = 916;
% avp0 = getat(avp,t0);
% ins = insinit(avp0, ts);
% avperr = avperrset([60;300], 1, 10);
% imuerr = imuerrset(0.5, 1000, 0.1, 25);
% Pmin = [avperrset([0.2,1.0],0.01,0.2); gabias(0.01, [10,10]); [0.01;0.01;0.01]; 0.001].^2;
% Rmin = vperrset(0.1, 0.3).^2;
% [avp1, xkpk, zkrk, sk, ins1, kf] = sinsgps(imu(t0/ts:t1/ts,:), gps, ins, avperr, imuerr, rep3(1), 0.1, vperrset(0.1,10), Pmin, Rmin, 'avped');
%
% See also kfinit, kfupdate, imugpssyn, igsplot, insupdate, posprocessing.
该函数实现的是19维状态，3维量测的组合导航
POS处理
包括正反向滤波，以及利用POSFusion的正反向融合
可参见仿真例子test_POS_fusion.m
