Detailed explanation of 19 dimensional integrated navigation module sinsgps in psins (initial assignment part)

Initial assignment part

1.[nn, ts, nts] = nnts(2, diff(imu(1:2,end))); Used for setting up nn Number of samples 、ts The sampling interval and the previous two are multiplied nts
2.if size(gps,2)<=5, gpspos_only=1; pos0=gps(1,1:3)'; else, gpspos_only=0; pos0=gps(1,4:6)'; end Judge the incoming GPS Whether the data has speed results , Only position ,gpspos_only=1, And speed ,gpspos_only=0
3.

if ~exist('rk', 'var'),
        if gpspos_only==1, rk=poserrset([10,30]);
        else, rk=vperrset([0.1;0.3],[10,30]); end
    end

The above code block is used to assign the initial variance to the measured value , If there are only position observations , Only position error is needed ; If there are velocity observations , The variance of speed position should be set ; Note that there , The value of position error is entered in meters , The unit of value of speed error is meters per second
4.

    if ~exist('dT', 'var'), dT = 0.01; end;   if length(dT)==1, dT(2,1)=1; end
    if ~exist('lever', 'var'), lever = rep3(1); else, lever=rep3(lever); end;   if length(lever)==3, lever(4)=1; end
    if ~exist('imuerr', 'var'), imuerr = imuerrset(0.05, 500, 0.001, [10;10;100]); end
    if ~exist('davp', 'var'), davp = avperrset([10;300], 1, [10;30]); end
    if ~exist('ins', 'var'), ins=100; end
    if ~isstruct(ins)  % sinsgps(imu, gps, T);  T=ins align time
        [~, res0] = aligni0(imu(1:fix(ins/ts),:), pos0);  imu(1:fix(ins/ts),:)=[];
        ins = insinit([res0.attk(1,1:3)'; 0;0;0; pos0], ts); ins.nts=nts;
    end

The above code block is to assign initial values to variables that are not provided ：
dT Default (0.01,1)s; lever Default [1;1;1]m; Others, such as default settings ;
most important of all ins The default is 100, It is used to judge whether to carry out ins Initialization of
5. ins.lever = lever(1:3); ins.tDelay = dT(1); Set the lever arm and time delay parameters
6. ins = inslever(ins, -ins.lever); ins.vn = ins.vnL; ins.pos = ins.posL; Compensation of lever arm error for speed and position
7. psinstypedef(196-gpspos_only*3); Preparing for the integrated navigation dimension type
8.

kf = [];
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1); 0])^2;

The above code block is the construction of system noise variance matrix .imuerr.web Random walk for angle ;imuerr.wdb Random walk for speed ; imuerr.sqg Is the angular rate random walk correlation deviation ;imuerr.sqa Is the correlation deviation of specific random walk .
9.kf.Rk = diag(rk)^2; To set the measurement noise matrix .
10. kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever(1:3)*lever(4); dT(1)*dT(2)]*1.0)^2; To set the state variance matrix
11. kf.Hk = zeros(length(rk),19); Set the coefficient matrix for
12. kf = kfinit0(kf, nts); Initialize the Kalman filter
13.

if exist('Pmin', 'var'),
        if sum(Pmin)<=0, kf.pconstrain=0;
        else kf.Pmin = Pmin; kf.pconstrain = 1; end
    end
    kf.adaptive = 1;
    if exist('Rmin', 'var'), 
        if sum(Rmin)<=0, kf.adaptive=0; end
        if kf.adaptive==1,
            if length(Rmin)==1, kf.Rmin = kf.Rk*Rmin;
            else kf.Rmin = diag(Rmin); end
        end
    end

The above code is to set the lower limit of the limit state variance and the lower limit of the measurement noise matrix of the adaptive filter
14.

if exist('fbstr', 'var'), kf.fbstr=fbstr; end
    kf.xtau = [ [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; 1]*1;

The above code is to set relevant parameters

Time synchronization initialization part

15. imugpssyn(imu(:,end), gps(:,end)); Time synchronization will be introduced in detail in the next article