uNav INS

@brtaylor - @Don Kelly

Looks like its working. Getting a nice steady Yaw most of the time when not moving. I added a option to zero the gyro when I start seeing yaw drifting. Use it once or twice after starting the Filter and yaw stays steady. If moving there doesn't seem to be problem with yaw.
 
@brtaylor - @Don Kelly

Looks like its working. Getting a nice steady Yaw most of the time when not moving. I added a option to zero the gyro when I start seeing yaw drifting. Use it once or twice after starting the Filter and yaw stays steady. If moving there doesn't seem to be problem with yaw.

Great to hear! Yeah, with the platforms I've been testing for, they're always moving some.
 
How to choose these parameters? Are there any methods and skills
// Sensor variances (as standard deviation) and models (tau)
float aNoiseSigma_mps2 = 0.05f; // Std dev of Accelerometer Wide Band Noise (m/s^2)
float aMarkovSigma_mps2 = 0.01f; // Std dev of Accelerometer Markov Bias
float aMarkovTau_s = 100.0f; // Correlation time or time constant

float wNoiseSigma_rps = 0.00175f; // Std dev of rotation rate output noise (rad/s)
float wMarkovSigma_rps = 0.00025; // Std dev of correlated rotation rate bias
float wMarkovTau_s = 50.0f; // Correlati1on time or time constant

float pNoiseSigma_NE_m = 3.0f; // GPS measurement noise std dev (m)
float pNoiseSigma_D_m = 6.0f; // GPS measurement noise std dev (m)

float vNoiseSigma_NE_mps = 0.5f; // GPS measurement noise std dev (m/s)
float vNoiseSigma_D_mps = 1.0f; // GPS measurement noise std dev (m/s)
 
For me i usually do a web search but you will have to get an understanding of noise and standard deviation. Heres a couple of references:

Those values seem to work fairly well for me with a T4.0, MPU9250 and a Ublox Zed-F9P.

ALso sometimes what i do is just to tweak the numbers and see what they do.
 
thank you

For me i usually do a web search but you will have to get an understanding of noise and standard deviation. Heres a couple of references:

Those values seem to work fairly well for me with a T4.0, MPU9250 and a Ublox Zed-F9P.

ALso sometimes what i do is just to tweak the numbers and see what they do.

thank you for your reply!!
 
Attached is a zip of the corrected code. It was being used with some CMake build tools, but should be easy enough to get it working as an Arduino library. Requires Eigen.

Yaw tracking has been good while moving. Here is a plot of a test I did where I was walking on a north-south aligned street and compared the EKF results with the DJI A3 flight control system, a VN-200 and a VN-300. The EKF was using an MPU-9250 IMU (hence the label) and a F9P GNSS receiver. The weird data dropouts on the VN-300, I think were due to a specific cable - this test was performed in February and in April, I tried to reproduce the data dropouts to debug with VectorNav engineers, but couldn't reproduce the issue, which is why I think it was an intermittent cabling fault between the Teensy and the VN-300. Prior to this test, all of the filters were warmed up by moving the platform in pitch, roll, heading circles and changing position for about 5 - 10 minutes.
@brtaylor
Well finally got the whole test bed set up with a simple logger and a button to end logging and close the file.

Plot is a bit confusing but put a little plot of the lat/long from google maps. Basically started of pointing North (approx) and let it run for a little bit, the did a turn to point to about west, the picked it up and moved to the East - walked backward so still point west then walked forward a couple of time, then turned walked north - then south backwards a couple of time. The pitch down was intentional to act as marker for when I started the walk.

Test really started about 90s in. Ok walked outside with a compass and the angles are about right.
Picture1.jpg
 
@brtaylor
Well finally got the whole test bed set up with a simple logger and a button to end logging and close the file.

Plot is a bit confusing but put a little plot of the lat/long from google maps. Basically started of pointing North (approx) and let it run for a little bit, the did a turn to point to about west, the picked it up and moved to the East - walked backward so still point west then walked forward a couple of time, then turned walked north - then south backwards a couple of time. The pitch down was intentional to act as marker for when I started the walk.

Test really started about 90s in. Ok walked outside with a compass and the angles are about right.
View attachment 20755

Thanks for posting! I'm going to have to think through this a bit more.
 
Hello!
Im trying to use this uNav INS with my GPS uBlox neo6 and a BNO055.
With my current code, i already have every variable needed to send to the Filter.update function, but im getting very random numbers of roll, pitch and yaw.
I have all the parameters on the requerid units like m, m/s, m/s/s, rad, rad/s and uT.
Maybe theres any way to get help here?
Thank you.
 
Also, i think i have not the current libraries for the UBLOX compatible with the latest version of this uNav and the Eigen library neither.
 
Im using a BNO055, but the same variables and units with no results.

All tests using UNAV-INS was done with a MPU9250 which gives you the raw acceleration, gyro and magnetometer values. With the BNO055 there are different modes of operations. You want to make sure you are not using the BNO055 in fusion mode. Since you didn't post any of your code its difficult to tell exactly what you are doing with the sensors, you also haven't said what Teensy you are using or what libraries you are using. But on page 20 of the datasheet:
Capture.PNG
 
Thank you for the answer.
Im getting the acc, gyro and mag raw data from the sensor with the same units, so in theory this uNav should work fine but im getting those values.
Im using the mode that gives the 9 degrees of freedom without fusion.
All tests using UNAV-INS was done with a MPU9250 which gives you the raw acceleration, gyro and magnetometer values. With the BNO055 there are different modes of operations. You want to make sure you are not using the BNO055 in fusion mode. Since you didn't post any of your code its difficult to tell exactly what you are doing with the sensors, you also haven't said what Teensy you are using or what libraries you are using. But on page 20 of the datasheet:
View attachment 27584
 
Hi Juan, I received your email last night. My guess is either it's a units issue or an axis issue. The expected positive axes for the IMU are positive X axis forward out the nose, positive Y axis out the right, and positive Z axis down. I would double check that your IMU is outputting the correct units in the correct axes for all of the measurements.
 
Another theory is that I’m using a very earlier version of this uNav INS because I tried the ver6 and it is not compiling due older versions of UBLOX library.
I double checked the units and they are in the units required by your code.
Maybe I’m outdated with the libraries.

Hi Juan, I received your email last night. My guess is either it's a units issue or an axis issue. The expected positive axes for the IMU are positive X axis forward out the nose, positive Y axis out the right, and positive Z axis down. I would double check that your IMU is outputting the correct units in the correct axes for all of the measurements.
 
Can you point me to the link for unav-ins v6 you are using?

Its been a long time since I played with this :)
 
Another theory is that I’m using a very earlier version of this uNav INS because I tried the ver6 and it is not compiling due older versions of UBLOX library.
I double checked the units and they are in the units required by your code.
Maybe I’m outdated with the libraries.

This would be a lot easier if you posted your code. All versions I have use the MPU-9250 and to re-structure it for your setup would take awhile and no guarantee we would get different results.

Also what library are you using for the BNO055 or did you create your own?
 
I’m using the last link of the uNav on this thread.
Just asume that the input is in the right units because they work well on the Madwick filter.

Maybe can you share me the last stable version of this to try again?
I’m getting compile errors with the version 5.

This would be a lot easier if you posted your code. All versions I have use the MPU-9250 and to re-structure it for your setup would take awhile and no guarantee we would get different results.

Also what library are you using for the BNO055 or did you create your own?
 
Just asume that the input is in the right units because they work well on the Madwick filter.

That's not a good assumption. These are all just math algorithms, which are then widely implemented into code by different people. I may choose to use a certain set of units (i.e. rad/s) for my EKF and some other implementation may choose units more convenient to them (i.e. deg/s). I may choose a frame with the x forward, y to the right, and z down. Another implementation may use x forward, y left, and z up. Or they might not even use a right handed frame and opt to have everything input how it comes off the sensor. I've seen Madgwick implementations that are x forward, y right, and z up...

Help us help you, because we can't with the information being provided. We need to see your code, the uNavINS library used, and the BNO055 library used. Helps to know the microcontroller used also. We'll also probably need to see raw data from the IMU and GNSS to understand that they are physically connected properly and providing reasonable data. If you're getting errors, post the error messages, because I can't guess why it's not compiling.
 
I’ll upload the files I’m using this night.
For example the Madwick filter uses the same units of your EKF and the input variables are consistent with the frames used in your code.
I was looking for the GitHub repository but it’s offline, that’s why I think I’m using a very old version of the uNav and UBLOX libraries.
That's not a good assumption. These are all just math algorithms, which are then widely implemented into code by different people. I may choose to use a certain set of units (i.e. rad/s) for my EKF and some other implementation may choose units more convenient to them (i.e. deg/s). I may choose a frame with the x forward, y to the right, and z down. Another implementation may use x forward, y left, and z up. Or they might not even use a right handed frame and opt to have everything input how it comes off the sensor. I've seen Madgwick implementations that are x forward, y right, and z up...

Help us help you, because we can't with the information being provided. We need to see your code, the uNavINS library used, and the BNO055 library used. Helps to know the microcontroller used also. We'll also probably need to see raw data from the IMU and GNSS to understand that they are physically connected properly and providing reasonable data. If you're getting errors, post the error messages, because I can't guess why it's not compiling.
 
View attachment uNavINS_CB_Ver6.zipView attachment uNavINS-master.zip
The master version compiles perfect but i have the graphic i uploaded earlier. The ver6 doesn't compiles to my teensy 4.

That's not a good assumption. These are all just math algorithms, which are then widely implemented into code by different people. I may choose to use a certain set of units (i.e. rad/s) for my EKF and some other implementation may choose units more convenient to them (i.e. deg/s). I may choose a frame with the x forward, y to the right, and z down. Another implementation may use x forward, y left, and z up. Or they might not even use a right handed frame and opt to have everything input how it comes off the sensor. I've seen Madgwick implementations that are x forward, y right, and z up...

Help us help you, because we can't with the information being provided. We need to see your code, the uNavINS library used, and the BNO055 library used. Helps to know the microcontroller used also. We'll also probably need to see raw data from the IMU and GNSS to understand that they are physically connected properly and providing reasonable data. If you're getting errors, post the error messages, because I can't guess why it's not compiling.
 
THis is the kind of raw data i have.
Gyro in rad/s, accelerations (total, including gravity) in the 3 axis in m/s2 and magnetometer in uT.
The linear acc on the picture is like an estimation removing gravity vector but it doesn't work for this purpose so i don't use it.


Screen Shot 2022-02-20 at 9.57.03 PM.png
 
Back
Top