kfinit是kf的参数初始化函数,用于初始化滤波参数
本文所述的代码需要基于PSINS工具箱,工具箱的讲解:
使用方法
kfinit这个函数的字面意思是:kf的初始化。这里面的
"
k
f
"
"kf"
"kf"表示的不是狭义的线性卡尔曼滤波,而是所有卡尔曼滤波类的滤波方法,后面不管是EKF,还是UKF、CKF,都是用这个kfinit来初始化的。
此函数的使用格式如下:
kf = kfinit(ins, davp0, imuerr, rk);
其中ins为惯导数据、davp0为初始的avp误差,imuerr为imu误差,rk为观测向量各元素的误差标准差。
例程实践
运行代码
运行下列代码,获得所需要的量:
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]);
再运行kfinit的代码:
kf = kfinit(ins, davp0, imuerr, rk);
运行结果
可得结构体kf,其值包括:
其中,Qt和Rk分别为状态转移协方差矩阵和观测协方差矩阵。
Pxk是当前时刻(也就是初始时刻)的状态协方差矩阵。
Hk和Qk是观测矩阵和状态转移矩阵。
函数解析
函数完整代码如下:
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.kfinit246
psinsdef.kffk = 24; psinsdef.kfhk = 246; psinsdef.kfplot = 24;
[davp, imuerr, 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; zeros(9,1)])^2;
kf.Rk = diag(r0)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; imuerr.dKga(1:9)]*1.0)^2;
kf.Hk = zeros(6,24);
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);
里面每一个case表示不同的滤波形式,按照PSINS惯用的命名规则,psinsdef.kfinit后面的三位数,前两位(左侧的两位)表示状态量的维度,最后一位(最右边一位,也就是个位数)表示观测量的维度。
以153距离,其每一行的代码作用如下:
- 将各个tag标注为153:
psinsdef.kffk = 15; psinsdef.kfhk = 153; psinsdef.kfplot = 15; - 将函数输入的第2~4个元素分开
[davp, imuerr, rk] = setvals(varargin); - 根据IMU误差特性求解状态转移噪声协方差
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2; - 根据观测噪声求解观测噪声协方差
kf.Rk = diag(rk)^2; - 根据初始误差求解初始的状态协方差
P
x
k
P_{xk}
Pxk
kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;
% 求观测矩阵
kf.Hk = kfhk(0);