kfinit是kf的参数初始化函数，用于初始化滤波参数
本文所述的代码需要基于PSINS工具箱，工具箱的讲解：
- PSINS初学指导
- 基于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);
