ZUPT DETECTION ALGORITHM

[scott198510]

%% ZUPT DETECTION ALGORITHM
idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time

if ( i > idz )
    
    % Mean velocity value for the ZUPT window time
    vel_m = mean (vel_e(i-idz:i , :));
    
    % If mean velocity value is under the ZUPT threshold velocity...
    if (abs(vel_m) < gnss.zupt_th)
        
        % Current attitude is equal to the mean of previous attitudes
        % inside the ZUPT window time
        roll_e(i)  = mean (roll_e(i-idz:i , :));
        pitch_e(i) = mean (pitch_e(i-idz:i , :));
        yaw_e(i)   = mean (yaw_e(i-idz:i , :));
        
        % Current position is equal to the mean of previous positions
        % inside the ZUPT window time
        lat_e(i) = mean (lat_e(i-idz:i , :));
        lon_e(i) = mean (lon_e(i-idz:i , :));
        h_e(i)   = mean (h_e(i-idz:i , :));

In the above codes, attitude is equal to the mean of previous states, so I think index "i" is not in previous indexes.
Because when the conditions are met， “ if ( i > idz ) ”，
index of " i " is no longer in the appropriate range.

Then we should edited all indexes " :i " to: “:i-1”, such as below:

    % Mean velocity value for the ZUPT window time
    vel_m = mean (vel_e(i-idz:i-1 , :));
    
    % If mean velocity value is under the ZUPT threshold velocity...
    if (abs(vel_m) < gnss.zupt_th)
        
        % Current attitude is equal to the mean of previous attitudes
        % inside the ZUPT window time
        roll_e(i)  = mean (roll_e(i-idz:i-1 , :));
        pitch_e(i) = mean (pitch_e(i-idz:i-1 , :));
        yaw_e(i)   = mean (yaw_e(i-idz:i-1 , :));
        
        % Current position is equal to the mean of previous positions
        % inside the ZUPT window time
        lat_e(i) = mean (lat_e(i-idz:i-1 , :));
        lon_e(i) = mean (lon_e(i-idz:i-1, :));
        h_e(i)   = mean (h_e(i-idz:i-1 , :));

What is your opinion? @rodralez

[venkat057]

Yes, it is.

Hi, i have query for you, for real time implementation this INS which uC/uP is preferable. The uC/uP should have double precision floating point .. right..!?

[rodralez]

The ZUPT algorithm calculates the mean of both attitude and position variables between the current inertial time (i) and the past zupt window time (i-idz), if the mean of the velocity is under a threshold (gnss.zupt_th).