rodralez / navego Goto Github PK

Function coriolis_b.m is no longer used for other functions and must be deleted.

Hi, I'm working with an actual IMU. While generating the error parameters using allan_imu.m, I got inconsistent tau lengths.
From line.176 of allan_imu.m, the for loop repeats 3 times, in the first time, allan_overlap() returns a tau that has 48 items, but in the second time, the allan_overlap() returns a tau that has 48 items. Then the program retuens an error says
`the dimention of left side is 48×1，the dimention of left side is 47×1。

error allan_imu (line 184)
imu.fb_tau (:,i) = tau';`

And I debugged the program, it seems that in line.301 of allan_overlap.m the program returns 48 and 47 in first and second time.

So is there a walkaround of this problem?

Glad to have your reply:)

The data I used is:
https://github.com/xelmirage/my_allan

Thanks for your NaveGO, I am not good at programming, so I have trouble about getting REF DATA.
The Inertial Explorer need input binary data, I only have csv data, how can we do?
Looking forward to your reply!

A parser for RINEX files is needed to extract pseudoranges and pseudorange rates from GNSS measurements, in order to later support tightly-coupled integration.

The parser can be developed in-house or taken from another project.

When I used Allen variance for IMU calibration, if the angular velocity (WB) changed very little, I tested the condition of all 0, so an error would be reported, and the reason was found to be function [retVal, S, errorb, tau] = allan_overlap(data,tau,name,verbose)
% Screen for outliers using 5x Median Absolute Deviation (MAD) criteria
MAD = median (abs (medianfreq) / 0.6745);
if verbose >= 1 && any(abs(medianfreq) > 5MAD)
Odl = (abs (medianfreq) & gt;5MAD);
Outliers = data. Freq (odl);
Fprintf (1, 'allan_overlap: OUTLIERS: There appear to be %d OUTLIERS in the frequency data.\n', length(OUTLIERS));
Fit_line = polyval (s.l inear, (1 / data. Rate: 1 / data. Rate: length (data. The freq)/data. Rate) ') - s.m edian;
Idl = (medianfreq <(5*MAD + fit_line) );
Data. The freq = data. The freq (idl);
Medianfreq = medianfreq (idl);

Fit_line = polyval (s.l inear, (1 / data. Rate: 1 / data. Rate: length (data. The freq)/data. Rate) ') - s.m edian;
Idl = (medianFreq >;(-5*MAD + fit_line) ); &&这行代码删除掉了我全部的数值0
Data. The freq = data. The freq (idl);
Medianfreq = medianfreq (idl);
S.o utliers = length (outliers);
The else
S.o utliers = 0;
end
It will make data filtering, delete all data.freq values, and get empty [], and report an error.

NaveGo's Holy Grail ;-)

Groves' book (2nd Ed.) provides MATLAB code for an implementation of tightly-coupled integration. This code can be used as a starting point for NaveGo's own implementation.

Hi Rodrigo,

I noticed you add dt in the F matrix. I don't really understand this, can you explain why do you need to multiply the fixed bias term with dt here?

F= [Fee Fev Fep (DCMbn)*dt Z (DCMbn) Z;
Fve Fvv Fvp Z (-DCMbn)*dt Z (-DCMbn);
Fpe Fpv Fpp Z Z Z Z;
Z Z Z Z Z Z Z;
Z Z Z Z Z Z Z;
Z Z Z Z Z Fgg Z;
Z Z Z Z Z Z Faa;
];

Regards

Hi~I'm working with your wonderful work to process the data from my own UAV autopilot. But Now I am confused with the relation of the Body frame XYZ and the roll/pitch/yaw of the UAV. I tried to found the general relationship and there are many possibilities existing. So I'm wondering if there is any further definition of the relation of the Body frame XYZ and the roll/pitch/yaw of the UAV?

Given public open source projects have free support for cloud ci services, consider leveraging one of them here. 🎉

https://blogs.mathworks.com/developer/2020/12/15/cloud-ci-services/

myUD.m, referenced from ins.m@112

Dear Rodrigo,

thank you for the useful toolbox!

Maybe my question is very stupid. I am confused about the sign of the matrix DCMbn_n in ins_gps.m from the line 307 to 308. In the matlab code ins_gps.m, the sign of DCMbn_n is negative, however, in your paper

• (Gonzalez et al., 2015a) R. Gonzalez, J.I. Giribet, and H.D. Patiño. An approach to benchmarking of loosely coupled low-cost navigation systems. Mathematical and Computer Modelling of Dynamical Systems, vol. 21, issue 3, pp. 272-287, 2015.

the sign of DCMbn_n is positive, cf. eq (21e). I am a beginner in this area, so I am not sure whether I missed something or not.

Thanks again and best regards,

Haifeng Wu

vrw: 1x3 angle velocity walks (m/s^2/root-Hz).
↓
vrw: 1x3 velocity random walks (m/s^2/root-Hz).

During the forced GNSS outages, the altitude shows a sharp drop which is inconsistent with a land vehicle trajectory.

One approach could be, if the Vel D is bigger than a threshold, the altitude should be frozen until the Vel D shows reasonable values.

Hi, i found following mismatch/incorrect updation of matlab software and reference "An Approach to bench-marking of loosely coupled low-cost navigation systems"

1. Equation (17C) + sign @ fc + S*() +... but in code vel_update function fnc = fn - S*() + ... (i.e fnc = fn - Coriolis +fn)
2. F matrix in equation 22, negative sign for Cbn at attitude row but in code negative sing for Cbn in velocity row.
3. Similarly for G matrix,equation 23, negative sign for Cbn at 1st row but in code negative sing for Cbn in second row.
   Please check and update or comment..!?

It seems that llh2ecef.m, referenced in ned2ecef.m is missing.

add calibration processing for IMU data ? And also add frame transformation due to level arm effect？

@rodralez meanwhile I think the output of omega_ie_n and omega_en_n should be two vectors, not two matrices
then when we use it in specified position, we can call skewm to get a matrix.
Otherwise, we will change the dimension of the matrix as described above

Originally posted by @scott198510 in #91 (comment)

Hi, Dr Gonzalez
After I study your codes , figure out in your article , dimension of F matrix is 2121 but you programmed it 1515 , why?
and please submit a reference for more understanding a ZUPT algorithm and formula .
thank you .

Hi,
Please note that vel_update corrects the specific force by subtracting g as a vector (31) and not by a scalar as written in function description (moreover, fn_c dimensions are (31)..so g must be (3*1) )
Thanks!

Hi Rodralez,

When I tried to run navego_allan_exmaple.m. I have got following errors:

Reference to non-existent field 'a_std'.

Error in acc_gen (line 101)
a_wn(:, i) = imu.a_std(i).* wn(:,i);

Error in navego_allan_example (line 159)
fb = acc_gen (ref, ustrain); % Generate acc in the body frame

Could you please check this issue for me whether there is some problem in stim300.mat file?

Thank you very much!
Fox

Dear Rodrigo,
Firstly, thank you for your toolbox!
I am a beginner. I want to know how to determine Gyro correlation times and Acc correlation times for real-world data? Do you have suggested methods and functions?

Many thanks.
Best Regards.
Shida

Could you please post ref.mat, imu1.mat, imu2.mat, gps.mat that works with your latest navego_example in matlab. I tried to use your code on Octave and FreeMat however some functions(for example matlabrc, sgolayfilt .) are not implemented in Octave and FreeMat so I would like to bypass the code which is using them by means of using mentioned ready simulation mat files instead. Thank you.

Sincerely, Adam

Hi my friend.
I just started learning but I had a problem ...
DCMnb is 33 matrix but in ( gyro_gen_delta.m ) code you told DCMnb is N9 matrix , why?
I don't understand following code:
dcmnb = reshape(DCMnb(k,:), 3, 3);
dcmnb_old = reshape(DCMnb(k-1,:), 3, 3);

please help me to understand it. thank you

Dear Rodrigo,
many thanks for this useful toolbox!

Would it be possible to add also the function for generating the ref.mat data or are you using an external tool for generating it?

Many thanks.

Best Regards.
Paolo

Hi,

Firstly, thank you for you work! It's a very useful toolbox and it's awesome that you deliver it under gpl.

Secondly, I am trying to simulate the INS without GPS calibration to estimate how much grow the error with the reference data. As you can see in the figure,

the down component error grow up too much for a free inertial solution.

if you want to reproduce it, I used the ins_gps.m function without kalman filter.

Thank you

Hi Authors,

I was reading the code of NaveGo and I am curious about something. When you update F without valid imu.gb_corr or imu.ab_corr, the matrix of Fbg or Fba is not produced, see lines 170 to 172 and lines 178 to 180 of function F_update. However, when MAG is switched to 'OFF', these two matrices are needed to produce G, see lines 186 to 202. So I wish to know if these were intentionally written? Could you please let me know a suggestion on these two matrices for my processing?
Thank you very much!

River

Hi,
In F_update (lines 81-112):
Updating F matrix for the misalignment errors doesn't match Farrell's book (up to minus sign). Please see page 394 (CHAPTER 11. AIDED INERTIAL NAVIGATION).

What is the right implementation ?

Thanks,
Barak

Hi Dear Rodralez:
I have read most of your articles, and I have carefully studied the previous version of NaveGo code. At the same time, I have also carefully looked at the latest version of the code in recent days. You have done a good job, very Thank you; but now there is a question;
In the paper 《Performance Assessment of an Ultra Low-Cost Inertial Measurement Unit for Ground Vehicle Navigation》Table 1 and Table 2, shows the noise parameters calculated by the allan method!

In the program 'navego_example_real_ekinox.m',we can see the struct of 'ekinox_imu' , but I don't understand how you calculated ‘gb_psd’ and 'ab_psd' .
From your simulation pragram, i know the 'imu_si.gb_psd = imu_si.gb_dyn .* sqrt(imu.gb_corr)'
but when i use the dynamic error and correlation time in Table 2 to calculate it ,but i don get the same result with you !
Please help me ,thank!

Dear rodralez,

thank you for your work, I really appreciate your code.

I am a little bit confused since the function "gravity" creates a vektor g_n with the size [1x3]

g = (g0 ./ (1 + (h ./ Ro)).^2);

Z = zeros(size(lat));

g_n = [Z Z g];

First of all, in the description of the function its mentioned as size [Mx1]
But more important for me: in the function vel_update you use

fn_c = fn - coriolis - g

fn is size [3x1], coriolis is size [3x1] but g is size [1x3] which actually creates a matrix [3x3] for fn_c (which confuses me since fn_c should have size [3x1] as well).

Can you help me out please?

Thank you very much

figure 1 is the outcome of 'navego_allan_example.m',figure 2 is gyro specification in datasheet of stim300.
What are the units of arw and vrw, respectively?Can they be identical with those in datasheet?
Thank u.

Since NaveGo mathematical model is no longer completely based on the two articles by Gonzalez (Gonzalez et al., 2015, Gonzalez et al., 2015b), references to these two articles have to be eliminated to avoid confusion among NaveGo new users.

Hi,

R has already defined in line 163. It appears again in 181.

Barak

Hello Dr. Gonzalez,

Thanks for the open sourced software. It is stepping stone and great insight for the beginners.

It is mentioned in the comment section of each example file
"% NOTE: NaveGo assumes that IMU is aligned with respect to body-frame as
% X-forward, Y-right, and Z-down."

1. In "navego_example_synth.m" the Vehicle frame has same direction as of IMU?
2. For "navego_example_real_ekinox.m" & "navego_example_real_mpu6000.m". This is similar as 1, except we have lever-arm and we compensate for the lever-arm effect?
3. Does it matter if vehicle move along a different axis. For example, assuming the vehicle is moving along x-axis in case of "navego_example_real_ekinox.m". In second case, the vehicle collects data moving along y-axis. Considering such scenario should we change anything? // or we keep our model the same?

Sorry for asking so many questions.
I wish you a very happy and healthy new year ahead in advance.

Best regards,

The goal of this issue is to improve NaveGo functions for getting the Allan variance, inspired in the methods exposed in ref [1].

[1] https://www.mathworks.com/help/nav/ug/inertial-sensor-noise-analysis-using-allan-variance.html

I checked the code of ins_gnss and there is no usage of ab_sta and gb_sta.
There is only one line with this variables:

b(1,:)  = [imu.gb_sta, imu.ab_sta];

But there is no practical benefit in it. It confused me at first. Maybe remove this input or add special comment in ins_gnss.m?

dsplot.m is a very verbose function to plot downsampled data from static IMUs. It has to be deleted and a new function or code has to be implemented just to plot downsampled data from a static IMU.

Hi Dr,
I think one section of your program is wrong , am I true ?
you programed following equation in line 52 at gnss_err_profile.m :
gnss.std(2) = gnss.stdm(2) / (RN + h) / cos (lat);

but as you told in your article it must be :
gnss.std(2) = gnss.stdm(2) / ((RN + h) * cos (lat));

and I can't find the reference of this equation in the book.
thank you.

I am not sure to understand how you are using the lever arm for the computation of the position.
In fact, I have the impression that you use it for the position but not for the velocity

" zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [gps.lat(j); gps.lon(j); gps.h(j);]) ...
+ (DCMbn * gps.larm);

zv = (vel_e(i,:) - gps.vel(j,:))';    "  (from ins_gps.m)

Am I right or did I misunderstand the script ?

Thank you in advance for your answer !
And thank you for your code.

Dear NaveGo-Team, thank you for your work, I do appreciate it.

Performances of the two simulated INS/GNSS systems are compared in the file navego_example_synth.m, but the down velocity seems to be problematic (Down velocity errors of the two simulated INS/GNSS systems are always outside the margin of error).

Down velocity error of imu2:

Hi Dear Rodralez:

I'm using NaveGo to deal with my own IMU data from UAV, during the process, I found the result was ridiculously wrong, after a careful examine, I found the gravity directions are different in my IMU data and the NaveGo code and the NaveGo simulated example data.

I used both a DJI UAV and an Xsens MTi IMU to collect the original data. Their body coordinate system are all FRD, which are supposed to be the same with NaveGo. But their data are acting differently with the NaveGo simulated example data:

↑this is the DJI UAV IMU data

In both case, the body is firstly still and then moving upwards and downwards back. The upwards accelerations are negative, which meets the FRD coordinate definition well. The Z acceleration data is around -9.8,which means the gravity is -9.8 in their FRD coordinate system.

But directly providing these data to NaveGo leads to a messy result. Then I checked the simulated example data, I found the body acceleration data is like:

the Z accleration data is around +9.8, initially I thought this may be an inverted axis.But according to the simulated track, the body experienced an upward acceleration, if the axis was inverted ,the upward acceleration should be positive, however the upward acceleration is still negative with the + 9.8 gravity.

So it appears that in NaveGo the gravity is positive in the body FRD coordinate system, unlike conventional sensor's definition. Now I'm working around this problem by making:
fn_c = fn - coriolis - (g);
to
fn_c = fn - coriolis + (g);
The results get reasonable but still not perfect. So i'm wondering besides altering - (g) to + (g), is there anything else I should do to make up for the different gravity direction problem? Will this affect allan regression result?

Plot of INS/GNSS estimates has to be improved to better show when GNSS outages are forced. Functions navego_plot.m and/or navego_plot_main.m have to be modified.

It would be nice to plot two vertical lines showing the GNSS outages period and change the color of the line inside this period.

Dear Rodrigo,

First of all thank you for your code, it is very helpful.

However I noticed a strange phenomenon, at the beginning of my simulation the hybridization is actually worse than GPS alone. For almost 3min there is about 35cm of error for the hybride trajectory whereas GPS error is below 2cm. Imu data are generated from the reference trajectory, with error estimated in static with Allan-variance.

What I do not understand is that since GPS.stdm is of about 1cm for this period, the kalman filter should give more weight to GNSS, and it appears not to be the case ? I have spent days on it, I don't see what I am missing.
I tried to take only gnss when RTK is received, with no success (kalman filter diverge).
Screenshots are attached.

Thank you in advance for your answer
Sincerely,

(The vehicle starts moving after 15sec)

In a normal execution of navego_example.m, I receive a warning in gyro_gen:

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 5.892434e-33.

In sgolay (line 101)
In sgolayfilt (line 71)
In gyro_gen (line 43)

is it normal?

Hi , Dr Gonzalez
In first thank you for your best tools in navigation .
I studied your codes but I didn't understand ZUPT algorithms.
please help me to understand it .
Thank you for your attention ...

Dear rodralez,

first of all, thank you very much for your work.
I am not sure if I miss something at the velocity update (vel_update.m), because there is a difference at the signs within the code and your publication. Here is the current function:

S = skewm(vel);             % Skew matrix with velocities
 
coriolis = S * (omega_en_N + 2 * omega_ie_N);   % Coriolis 

fn_c = fn - coriolis - (g); % Corrected specific force in nav-frame

vel_n = (vel + fn_c' * dt);  

According to your paper "An approach to benchmarking of loosely coupled low-cost navigation systems" the equation for the velocity derivative should be
fn_c = fn + coriolis + (g);
I also compared it to the derivation from Paul Grove in his book "Principles of GNSS, Intertial, and Multi-sensor Integrated Navigation", which coincides with it. Did I miss any transformation or redefinition?

King regards,
Flesko

I have read your paper,Nave GO : a simulation framework for low -cost integrated navigation systems. I have a question? formulation:(23.a) have matrix G and matrix U, but In your matlab simulation code,I don't see G and U.
In Kf_prediction.m , kf.xi = kf.A *kf.xp. so I am very confused about this question? please help me understand it.

Thank you for your attention ...

Hello! At first I would like to thank you for the great tool!

But I have some trouble when I;m trying to start an example (synthetic-data, Matlab 2014):

NaveGo: generating GNSS synthetic data...
NaveGo: generating IMU1 ACCR synthetic data...
Reference to non-existent field 'DCMnb_m'.

Error in acc_gen (line 64)
acc_b = acc_nav2body(acc_ned, ref.DCMnb_m);

Error in navego_example_synth (line 271)
fb = acc_gen (ref, imu1); % Generation of acc in the body frame

Would you mind telling me about how to calculate the
eps(time interval to compare current IMU time to current GNSS time vector (s).)

Usually, how can I get it?

Dear Rodrigo,
thanks for this useful toolbox!
I would like to ask a question how to generate trajectory data?
I have some real data collected by apm or pixhawk, how to use amp or pixhawk data to your model?