uNav AHRS

Status
Not open for further replies.
where to find ? Would like to have a look. Calibration is an old topic - in my opinion from a practical standpoint possibly overstated but still important.
Source and exe can be found here: https://github.com/mjs513/FreeIMU-Updates/tree/master/FreeIMU_GUI. I also have a wiki page about it here: https://github.com/mjs513/FreeIMU-Updates/wiki/04.-FreeIMU-Calibration, a little outdated but close. With the new IMU I found that accel needs very little calibration but the magnetometer and gyro still need good calibration. You can see that by the screen shots I provided in an earlier post. With early IMUs, accel was still quite a bit offset as well as the scale - was a lot more oblong.

Is Brian working with offsets for acc in the 9250 ?
From what I can see, the answer is yes. a?b's and h?b's are the offset values. You can see that in the readSensor function in the cpp file as well as the calibrateAccel function. A little different than the way I do it but it still works.

In my (practically oriented) view its sufficient to make three measurements for acc and get the offsets stored in EEPROM. On power up I fed them in the 9250 offset register and that was it - if I remember well regardless of the scale (not sure, a year since I worked on that)
For accel that is quick and dirty at least for the MPU-9250 but more is better. I always found a good calibration resolved a lot issues with drifting in Madgwick and Mahoney.

Ooops - looking closer to yaw and mag - rapid changes in orientation lead to inconsistent results. Gave it a try with Filter.setHeadingCovariance(0.001f); without a good result.
Next I will have a closer look at calibration and put Madwick run parallel for comparison.
With the settings I posted earlier seems to work better but still seeing drifting in yaw. Still working on that. I tried the heading covariance as well but seemed to make it worse.

Going to give to give Madgwick/Mahoney a shot later. My desktop died and I have it all apart :(.

Cheers
Mike
 
@mjs513, Thanks for running that calibration test. Seems like the more conservative approach is to calibrate at the desired full scale range.

@hw99, I estimate both an offset and a scale factor for the calibration. My guess is that the airspeed would be TAS if you have that available. I still need to go through the derivation of those equations to understand them fully. We aren't propagating the accelerometers to velocities, only using them to correct for gyro drift.

I'm not super happy with the heading performance after reducing the accelerometer covariance. I'm wondering if I made an error porting the code or if it's inherent to this approach. We tended to use values closer to the defaults and expected the AHRS to only perform well in relatively low dynamic environments. Plus we were using a much more expensive IMU, the AD16405. By the time I started working in the lab, the 15 state EKF was the primary navigation algorithm used, with the 7 state only as backup or for aircraft testing indoors before we installed GPS repeaters (the 15 state relies on GPS measurements for the measurement update). If any one is interested in the original code, it can be found here:
https://github.com/UASLab/OpenFlight/blob/master/FlightCode/navigation/micronav_ahrs.c
 
Hi Brian. Took your suggestion and went ahead and compared the GitHub code to the uNav code. Except for a couple of questions and observations everything looks ok:

1. Question: Where did you get they equations for euler to quaternions? Different than the one I am use to, just curious.
2. Observation: You kind of lost me in the code section commented as "second stage kalman filter update to estimate the heading angle". Looks like you combined several lines from the GitHub code to the uNAV code to get to Hpsi.
3. Question. Redid the calibration and yaw seems to hold steady for 5-6minutes or so seconds and then starts diverging with the new calibration and the covariance set to 0.025. You don't think dt is changing somehow?

That's all I got right now. Have to get ready for tonight.

Have a Happy and safe New Year.
Mike

Quick UPDATE:
Remembered that the one difference I saw was the heading covariance(magnetometer). In the uNAV code it was set to 0.27415570336144f. In the microNav code on GitHub it is 0.014924. If I change it to the GitHub value the yaw divergence gets dramatically better. I finally got down to the following settings that seem ok:
Code:
  // setting the accelerometer covariance
  Filter.setAccelCovariance(0.025f);
  Filter.setHeadingCovariance(0.0125f);
  // setting a 41 Hz DLPF bandwidth
  Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 9 for a 100 Hz update rate
  Imu.setSrd(9);

Give it a try and see if it is any better for you all.

UPDATE2: 1/1/18. I recoded the section to match what you had in your microNav code and no change to behavior of yaw. I did try one of the madgwick/Mahoney filters and the yaw stayed steady. Let it run for about 15 minutes with no issue
 
Last edited:
Hi Brian.
I can't seem to let anything go until it works. Going through the code I noticed that you had the following in the micro-Nav code:
Code:
	// new calibration data (uNAV#?, tested on 10=06=07)
	static float K_G[3][3] = {{0.53803532638536,   0.02134656564593,   0.00382173456905},
			{0.02134656564593,   0.53583969599251,  -0.01755859489163},
			{0.00382173456905,  -0.01755859489163,   0.52788394795182}};

	static float be[3]  = {0.06997459270763,   0.05852923762322,  -0.08405181072893};
Since I re did that section of code I gave set the be terms to what you had in the micro-NAV code. Adding a be term seems to have improved the yaw divergence. EDIT: Assume be term is the magnetometer offset ?

Thanks
MIke
 
Last edited:
Hi Brian.
I can't seem to let anything go until it works. Going through the code I noticed that you had the following in the micro-Nav code:
Code:
	// new calibration data (uNAV#?, tested on 10=06=07)
	static float K_G[3][3] = {{0.53803532638536,   0.02134656564593,   0.00382173456905},
			{0.02134656564593,   0.53583969599251,  -0.01755859489163},
			{0.00382173456905,  -0.01755859489163,   0.52788394795182}};

	static float be[3]  = {0.06997459270763,   0.05852923762322,  -0.08405181072893};
Since I re did that section of code I gave set the be terms to what you had in the micro-NAV code. Adding a be term seems to have improved the yaw divergence. EDIT: Assume be term is the magnetometer offset ?

Thanks
MIke

Hi Mike, in the original code (https://github.com/UASLab/OpenFlight/blob/master/FlightCode/navigation/micronav_ahrs.c), be is magnetometer offset and K_G is magnetometer scale factor. I neglected those terms in the port I did and assumed that the IMU had been calibrated beforehand and that corrected magnetometer measurements were being given (be and K_G are static values and not updated during the filter running).
 
Brian. Figured but had to make sure. I tried several variations but not matter what I tried I always saw yaw diverging when the 9250 is flat and at rest. Can't think of anything else.
 
Mike, Brian,
Same here on the yaw drift. My pitch and roll look really stable, although roll seems to always start with a -4 deg offset. Yaw starts drifting very slowly, but accelerates, and after 15 min or so is pretty far off.
Don
 
Yeah, I'll try to post the 15 state EKF in the next couple days. Was going to post it last week, but noticed some weird bugs. It's still buggy, but something to play with and start from.

I'm not sure about the 7 state AHRS performance either. It was fun to port it and educational, but longer term I'm planning on working up my own AHRS and INS EKF or UKF.
 
Don - Brian
One other thing I just noticed was the magSRD value was set to 0 while in the original code it was set to 5. I changed the parameters to the following:
Code:
  Filter.setAccelCovariance(0.025f);
  Filter.setHeadingCovariance(0.125f);
  Filter.setMagSrd(12);

and the drift was significantly improved. If this is the case then the issue is something with the magnetometer update/calibration, I think.
 
Don - Brian
One other thing I just noticed was the magSRD value was set to 0 while in the original code it was set to 5. I changed the parameters to the following:
Code:
  Filter.setAccelCovariance(0.025f);
  Filter.setHeadingCovariance(0.125f);
  Filter.setMagSrd(12);

and the drift was significantly improved. If this is the case then the issue is something with the magnetometer update/calibration, I think.

I agree. I originally had the magnetometers updating at 10 Hz (an SRD of 9 for a 100 Hz time update, or 4 for a 50 Hz time update). The original Power PC code had a 50 Hz time update and 10 Hz measurement update. The way the SRD was defined in the original code was a little different, so 5 was used to get the 10 Hz rate whereas in my port, 4 would be used.

It definitely appears to be an issue with the magnetometer measurement update.
 
Hi Brian

I hope I may ask this question - do you plan to finish the work on this library ? I apologize that I can't contribute. The math behind is too much for me.
 
Hi Brian

I hope I may ask this question - do you plan to finish the work on this library ? I apologize that I can't contribute. The math behind is too much for me.

Yes, I plan to continue working on and debugging this library. I recently did a little tuning, the process noise seemed to be set by default way too low for the potential dynamics between time updates. I increased the assumed process noise value and the filter appears to be much more stable.

Brian
 
Great work, people. Math is beyond my level unfortunately.

I have a MPU-9250 and I'm trying to implement a position logger which does not use a GPS, only data from the 9250.
Starting point is known (initial XYZ position is known, initial XYZ speed is 0,0,0).

Do you think I may use this 7-axis library to get some kind of (rough) position estimation over time (with proper initial calibration)?
In case, any reference you could point me to?

Many thanks.

Fernando
 
Great work, people. Math is beyond my level unfortunately.

I have a MPU-9250 and I'm trying to implement a position logger which does not use a GPS, only data from the 9250.
Starting point is known (initial XYZ position is known, initial XYZ speed is 0,0,0).

Do you think I may use this 7-axis library to get some kind of (rough) position estimation over time (with proper initial calibration)?
In case, any reference you could point me to?

Many thanks.

Fernando

Hi Fernando,

This library doesn't output position. The 15 state EKF (https://forum.pjrc.com/threads/48856-uNav-INS) does, but it's not meant to go for more than a few seconds without a GPS update. You can google for "IMU Dead Reckoning" to try to find some solutions. In general though, without an external input of position (such as GPS or an odometer) or speed (airspeed indicator, speedometer), you won't be able to get estimate position accurately for more than 10's of seconds. An IMU is measuring linear acceleration and rotational velocities. For position, you need to integrate linear acceleration to linear velocity and then integrate again to get position. Every time you integrate, you are amplifying any noise present in the system. Estimating position over longer periods of time using only a low cost IMU, without any external inputs, doesn't tend to work well.

Brian
 
Hi Brian,
I just updated the library to your latest version and there still seems to be a problem with yaw. Did dig into yet much but as they say a picture is worth a thousand words:
Capture.PNG
 
Hi Brian. I adjusted the process noise as follows everything else is the same as your example sketch except dlpf is 20:
Code:
      aQ(0,0)=aQ(1,1)=aQ(2,2)=aQ(3,3)=0.000005; 
      aQ(4,4)=aQ(5,5)=aQ(6,6)=0.0000001f;

and managed to get the yaw variation to about +/-0.6 degrees. Here is a plot of a long duration static test:
Capture1.PNG

The black lines are the mean for each axis.
 
Mike,
I've decided to fall back to the 7-State and see if I can get that stable before going back to the 15-State. Your 7-State results look really good, stable yaw for 50 min!

Any chance of summarizing your settings? I'm still getting fairly bad yaw drift, and I've tried:

setting aQ per your last post
dlpf to 20
AccelCovariance to 0.025f
HeadingCovariance to 0.125f
SetMagSrd to 12
SetSrd to 9

Thx!
Don
 
Mike,
I updated to Brian's latest version, and his mods seemed to have helped a lot. I'll go in and check out what he changed.
Don
 
Hi Don

With the changes made by Brian I am actually now using the libraries default values for AccelCovariance, HeadingCovariance and MagSrd. I have SetSRD = 9 and DLPF to 20 along with the aq changes I made.

So my setup now looks like this:
Code:
  Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  Imu.setSrd(9);

Something is still off but I can't figure it out as any changes too far away from those values sets off yaw drift. By something off I mean that yaw is not as steady as pitch and roll. Also, variation in the settings start the difting again.

Cheers
Mike
 
Last edited:
Great, thx much. I see he has SetSrd = 9, so I'll try DLPF = 20 as well. Think I'll also just print out the serial data at 2 or 5 Hz and view it using TelemetryViewer (http://www.farrellf.com/TelemetryViewer/). It's a neat little program and easy to use once set up. Wish he'd add serial commanding to it, then we could change values on the fly too.
 
Thanks for the link to the Telemetry viewer, definitely want to give that a try, never came across that one before. Good luck with the changes. By the way, just for reference I am using a T3.2 for these tests.

ADDED COMMENT: Look closely at roll, there is actually a long term drift in yaw of about 0.2 degree drift in roll before it steadies out again. Know its not much but ....
 
Last edited:
Status
Not open for further replies.
Back
Top