search

rodralez / NaveGo

NaveGo: an open-source MATLAB/GNU Octave toolbox for processing integrated navigation systems and performing inertial sensors analysis.
Other
584 stars 212 forks source link

Some errors in NaveGo #101

Closed scott198510 closed 2 years ago

scott198510 commented 2 years ago

  1. earth_rate.m
    AS we all know, omega_ie_n is often obtained by projecting the vector omega_ie_e to the n-frame , not a matrix. Although it can be turned into an skew matrix, this name is not quite reasonable.

  2. transport_rate: calculates the transport rate in the navigation frame. The same problem exists there as earth_rate.m

3.kf_update.m kf.Pp = kf.Pi - kf.K kf.S kf.K'; %kf.Pp = kf.Pi - kf.K kf.H kf.Pi;

Why not the second line in your code, usually the first one is not used in books or papers. If kf. S is an orthogonal matrix, the results of the upper and lower rows are consistent,otherwise they are different.

4.ins_gnss.m

fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta'; fn = DCMbn fb_corrected; wn = DCMbn imu.wb(1,:)' - gb_dyn - imu.gb_sta';

should edited to:

fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta'; fn = DCMbn fb_corrected; wn = DCMbn (imu.wb(1,:)' - gb_dyn - imu.gb_sta');

5.att_update.m %% Gyros output correction for Earth and transport rates

wb = ( wb - DCMbn' * (omega_ie_n + omega_en_n));

->>> If omega_ie_n and omega_en_n are matrices, The above writing is wrong, because the output wb is a matrix, not a vector. If omega_ie_n and omega_en_n are edited to two vectors as No.1 & No.2 proposal, Such a program is right, otherwise there will be a problem.

  1. F_update.m The state transition matrix in the program is 15 by 15, which is different from the original paper,this in itself has nothing to say.

In the program： F = [F11 F12 F13 DCMbn Z ; F21 F22 F23 Z -DCMbn ; F31 F32 F33 Z Z ; Z Z Z Fgg Z ; Z Z Z Z Faa ; ];

G = [DCMbn Z Z Z ; Z -DCMbn Z Z ; Z Z Z Z ; Z Z Fbg Z ; Z Z Z Fba ; ];

why not:

F = [F11 F12 F13 -DCMbn Z ; F21 F22 F23 Z DCMbn ; F31 F32 F33 Z Z ; Z Z Z Fgg Z ; Z Z Z Z Faa ; ];

G = [-DCMbn Z Z Z ; Z DCMbn Z Z ; Z Z Z Z ; Z Z Fbg Z ; Z Z Z Fba ; ];

Only in this way it is reasonable. Is there any special treatment or transformation? It is also the latter rather than the former in corresponding paper about NaveGo.

  1. For the paper "An approach to benchmarking of loosely coupled low-cost navigation systems" In eq. 17c ，there should be a minus sign in front second item "S(2w_ie_n + w_en_n)" Only in this way it is reasonable as your code and any other papers.

  2. ins_gnss.m

    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)

vel_m is a vector, gnss.zupt_th is a scalar What is the significance of such comparison such as “ if (abs(vel_m) < gnss.zupt_th)”

  1. ins_gnss.m **# Current attitude is equal to the mean of previous attitudes

    inside the ZUPT window time**

note: The previous state should mean excluding the current moment.

so I think all of the indexes " i-idz:i " should edited to "i-idz:i-1" ,such as :

if ( i > idz )

    % 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 , :));
rodralez commented 2 years ago

Hello Scott,

Too many individual issues to address! It would have been great if you had created one issue per each item in your list (1. , 2. , etc.). Maybe I could split this collection of issues by myself.

rodralez commented 2 years ago

With respect to point number 7, that paper no longer exposes the current formulas used by NaveGo. I should be taken as a guide for the structure of NaveGo. It could be helpful some contributor to update this document.

rodralez commented 2 years ago

New issues are created by splitting this issue. This issue will be close.

scott198510 commented 2 years ago
  1. earth_rate.m AS we all know, omega_ie_n is often obtained by projecting the vector omega_ie_e to the n-frame , not a matrix. Although it can be turned into an skew matrix, this name is not quite reasonable.
  2. transport_rate: calculates the transport rate in the navigation frame. The same problem exists there as earth_rate.m

3.kf_update.m kf.Pp = kf.Pi - kf.K kf.S kf.K'; %kf.Pp = kf.Pi - kf.K kf.H kf.Pi;

Why not the second line in your code, usually the first one is not used in books or papers. If kf. S is an orthogonal matrix, the results of the upper and lower rows are consistent,otherwise they are different.

4.ins_gnss.m

fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta'; fn = DCMbn fb_corrected; wn = DCMbn imu.wb(1,:)' - gb_dyn - imu.gb_sta';

should edited to:

fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta'; fn = DCMbn fb_corrected; wn = DCMbn (imu.wb(1,:)' - gb_dyn - imu.gb_sta');

5.att_update.m %% Gyros output correction for Earth and transport rates

wb = ( wb - DCMbn' * (omega_ie_n + omega_en_n));

->>> If omega_ie_n and omega_en_n are matrices, The above writing is wrong, because the output wb is a matrix, not a vector. If omega_ie_n and omega_en_n are edited to two vectors as No.1 & No.2 proposal, Such a program is right, otherwise there will be a problem.

  1. F_update.m The state transition matrix in the program is 15 by 15, which is different from the original paper,this in itself has nothing to say.

In the program： F = [F11 F12 F13 DCMbn Z ; F21 F22 F23 Z -DCMbn ; F31 F32 F33 Z Z ; Z Z Z Fgg Z ; Z Z Z Z Faa ; ];

G = [DCMbn Z Z Z ; Z -DCMbn Z Z ; Z Z Z Z ; Z Z Fbg Z ; Z Z Z Fba ; ];

why not:

F = [F11 F12 F13 -DCMbn Z ; F21 F22 F23 Z DCMbn ; F31 F32 F33 Z Z ; Z Z Z Fgg Z ; Z Z Z Z Faa ; ];

G = [-DCMbn Z Z Z ; Z DCMbn Z Z ; Z Z Z Z ; Z Z Fbg Z ; Z Z Z Fba ; ];

Only in this way it is reasonable. Is there any special treatment or transformation? It is also the latter rather than the former in corresponding paper about NaveGo.

  1. For the paper "An approach to benchmarking of loosely coupled low-cost navigation systems" In eq. 17c ，there should be a minus sign in front second item "S(2w_ie_n + w_en_n)" Only in this way it is reasonable as your code and any other papers.

8.ins_gnss.m

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)

vel_m is a vector, gnss.zupt_th is a scalar What is the significance of such comparison such as “ if (abs(vel_m) < gnss.zupt_th)”

Anyway, in this line : vel_m = mean (vel_e(i-idz:i , :));

I think it should be edited to :

vel_m = mean (vel_e(i-idz: i-1 , :));

because while i> idz,

The calculated idx should be before this i