PSINS滤波初始化函数修改，151

背景

PSINS的kfinit没有对应的15为状态、1维观测的滤波，但是实际上仅高度可观测的情况在实际上是需要的（无GNSS、只有气压高度计），这里给出出去改后的kf初始化函数。

函数

function kf = EV_kfinit(kfinit, ins, varargin)
% 自制的KF初始化程序，应对151的情况（GNSS拒止，仅有气压高度这一个观测）
global glv
[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(kfinit)
    case 151
        psinsdef.kffk = 15;  psinsdef.kfhk = 151;  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 = [0,0,0,0,0,0,0,0,1,0,0,0,0,0,0];
    otherwise
        warning('选择滤波模型出错');
end
kf = kfinit0(kf, nts);

用法

按原先的kfinit一样用，在输入量里面，在前方添加一个“151”即可，之所以要加一个151的选择，是因为预留接口，后面可能会做152/191等