PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-卡尔曼滤波设置+导航解算

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<