uNav AHRS

Status
Not open for further replies.
No you don't need the master part. I wanted to make sure you had a eigen directory in your libraries folder that had eigen.h and the Eigen sub directory that you downloaded. Should have explained that in my previous post. Sorry for the confusion
 
After compiling the example almost as is (I2C instead of SPI on T32) I just wonder what is going on. Reaction is fast but after some wild movements it takes minutes till the values come in the region of zero. In pitch and roll the error can easily be 50 degrees right after the movements. The funny thing is that further wild movements sometimes lead to a correction.

I initially thought that the axes don't match. But the change for roll, pitch and yaw corresponds with the movement of the sensor.
 
After compiling the example almost as is (I2C instead of SPI on T32) I just wonder what is going on. Reaction is fast but after some wild movements it takes minutes till the values come in the region of zero. In pitch and roll the error can easily be 50 degrees right after the movements. The funny thing is that further wild movements sometimes lead to a correction.

I initially thought that the axes don't match. But the change for roll, pitch and yaw corresponds with the movement of the sensor.

Can you post or send your code so I can test it? Thanks!
Brian
 
Can you post or send your code so I can test it? Thanks!
Brian

Some screenshots - added columns for acc and millis(), print every 100 millis

Right after setup :

Screenshot at 2017-12-28 08:44:15.jpg

Movements :

Screenshot at 2017-12-28 08:44:38.jpg

Some time later :

Screenshot at 2017-12-28 08:44:59.jpg
 
The files I used. Changes in uNavxxxx.ino are behind a comment line //mystuff. No changes in the libs.

If you need 9250-master as well let me know. Used it as downloaded
 

Attachments

  • Eigen-master.zip
    963.4 KB · Views: 156
  • uNavAHRS-master.zip
    12.1 KB · Views: 232
  • uNavAHRS_MPU9250.ino
    4.1 KB · Views: 132
Brian, Please keep me updated on your progress with getting the 15 state EKF ported to Teensy. I'm avail to help if you'd like some assistance with beta testing.

I'm getting ready to try out the 7 state EKF on my Teensy 3.6 in the next day or two. Thx for posting it!
Don
 
@hw999 and @brtaylor. I am seeing the same behavior on the T3.2. On a T3.5 I do not see the same behavior. One possibility is that the T3.5 has a FPU for floats as opposed to a T3.2. This is just a guess on my part. Brian would have to confirm that or not. May be something different.

@brtaylor. Same here for me as a beta tested for the 15 state EKF if you need it.

Mike
 
@mjs513 tested with a T36 with similar strange results. I admit that I did not calibrate the MPU - maybe this strange behavior has to do with bad calibration. Acceleration values were however in an reasonable range.

The Madgwick computation I used so far is not upset by bad calibration for yaw, pitch and roll. Mag is a different story of course. I wait for some feedback from @brian and maybe put the two computations side by side for comparison.
 
Thanks for the code and for trying out the filter! I can reproduce the issue with a Teensy 3.6 on I2C; I have a Teensy 3.2 sitting here and I'm sure I could reproduce it there as well. The issue is that by default we have a very conservative value set for accelerometer covariance; it should work well for relatively slow, smooth motion, but it's slow to recover the attitude if an error is introduced. Essentially, we're using the accelerometer covariance value to tell the filter how much to trust the accelerometers during a measurement update: the gyros are used to propagate the state and the accelerometers are used to correct it, for pitch and roll. By decreasing the accelerometer covariance value, we can tell the filter to trust the accelerometers more during a measurement update, which will snap the attitude to the correct value more quickly.

The default accelerometer covariance is 0.96177249 (0.1g)^2 or (0.9807m/s/s)^2. You can try setting the value to 0.001 (0.01g)^ or (0.09807m/s/s)^2. I would recommend calibrating the IMU. I would also recommend using a DLPF setting on the IMU of either 41 Hz or 20 Hz for a 100 Hz filter update rate; I should have included this in the example.

This means that the setup code now includes:
Code:
  // setting the accelerometer covariance
  Filter.setAccelCovariance(0.001f);
  // setting a 41 Hz DLPF bandwidth
  Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_41HZ);
  // setting SRD to 9 for a 100 Hz update rate
  Imu.setSrd(9);
  // enabling the data ready interrupt
  Imu.enableDataReadyInterrupt();
  // attaching the interrupt to microcontroller pin 1
  pinMode(1,INPUT);
  attachInterrupt(1,runFilter,RISING);

Thanks for the offers for beta testing the 15 state filter! I'll start another thread for that one later today.
 
Hi Brian. I gave your changes a shot on a T3.2 exactly as show above using hw999 sketch as a base (printing data every 100 ms). Yaw is drifting terribly.

The first screen shot is just leaving the board motionless and you can see the effect of the changes on yaw.Capture0.PNG

The next shot shows rotating it rapidly about the z-axis and then stopping it. You can see that the yaw continues to diverge, looks almost asymptotically. It continues to about 150 degrees and then levels outCapture1.PNG

The final screen shot shows me rotating the board so yaw is about 0 and you can see it now is diverging in the opposite direction. Capture3.PNG

When I get a little more time think I am going to have to try a few values for accel covariance and dig into the code some more.

Cheers
Mike
 
@Brian looks good so far on T32 and T36:)

@mjs513 yaw drift is normal, mag should be stable. Pitch and roll look fine during short tests. Will let it run for a longer time and look at drift
 
I used these settings on the T3.2 and seems to made a big difference.
Code:
  // setting the accelerometer covariance
  Filter.setAccelCovariance(0.025f);
  // setting a 41 Hz DLPF bandwidth
  Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
 
I'm not sure I'm getting a solid calibration with the uNavAHRS calibration sketch. Sorry if this is a dumb question, but is this roughly what you all are doing during calibration?

Accel Calibration:
1. Lay breadboard flat
2. (switch) Lay breadboard on left side
3. (switch) Lay breadboard on right side
4. (switch) Lay breadboard nose down
5. (switch) Lay breadboard nose up
6. (switch) Lay breadboard upside down
... accel calibration complete...

For Mag Calibration
1. Lay breadboard flat, move in figure 8 for ~5s
2. Lay breadboard on left side, move in figure 8 for ~5s
3. Lay breadboard on right side, move in figure 8 for ~5s
4. Lay breadboard nose down, move in figure 8 for ~5s
5. Lay breadboard nose up, move in figure 8 for ~5s
6. Lay breadboard upside down, move in figure 8 for ~5s
... mag calibration complete...

I'll try it outside next to make absolutely sure I'm away from anything metal.

Thx!
 
I'm not sure I'm getting a solid calibration with the uNavAHRS calibration sketch. Sorry if this is a dumb question, but is this roughly what you all are doing during calibration?

Accel Calibration:
1. Lay breadboard flat
2. (switch) Lay breadboard on left side
3. (switch) Lay breadboard on right side
4. (switch) Lay breadboard nose down
5. (switch) Lay breadboard nose up
6. (switch) Lay breadboard upside down
... accel calibration complete...

For Mag Calibration
1. Lay breadboard flat, move in figure 8 for ~5s
2. Lay breadboard on left side, move in figure 8 for ~5s
3. Lay breadboard on right side, move in figure 8 for ~5s
4. Lay breadboard nose down, move in figure 8 for ~5s
5. Lay breadboard nose up, move in figure 8 for ~5s
6. Lay breadboard upside down, move in figure 8 for ~5s
... mag calibration complete...

I'll try it outside next to make absolutely sure I'm away from anything metal.

Thx!

Hi Don,

This is my recommendation for the calibration:
https://forum.pjrc.com/threads/37891-MPU-9250-Teensy-Library?p=163250&viewfull=1#post163250

Hope that helps.
Brian
 
Hi Brian. I was looking at the calibration code for the accelerometer but it looks like you are calibrating the accelerometer at 2g and then resetting the accelerometer range back to whatever range was selected by the user. Shouldn't you be calibrating at your selected range? Think you do the same for the gyro? I always did the cal's at the range I selected.

Also, can you confirm that the default settings are 16g for the accel and 2000 dps for the gyro?

Thanks
Mike
 
Hi Brian. I was looking at the calibration code for the accelerometer but it looks like you are calibrating the accelerometer at 2g and then resetting the accelerometer range back to whatever range was selected by the user. Shouldn't you be calibrating at your selected range? Think you do the same for the gyro? I always did the cal's at the range I selected.

Also, can you confirm that the default settings are 16g for the accel and 2000 dps for the gyro?

Thanks
Mike

Hi Mike,
I debated with myself about that and ended up putting the accel and gyro at their lowest ranges for calibration. It really depends on the error sources. If we consider the source of the error to be the MEMS sensor itself (temperature sensitivity, orthogonality, alignment, etc) then it makes sense to change the range to the most sensitive value for estimating the error sources. If we consider the error source to be part of the MPU9250 ADC, then we would likely want to calibrate the sensor at the selected range.

I haven't done the testing yet to see whether one approach is best, yet.

Yes, the default is 16G and 2000 DPS.

Brian
 
Brian, I loaded up my sketch that allows me to output to a ellipsoid model for accel and magnetometer calibration. I ran a test for 2g/250dps and 16g/2000dps. As could be expected the magnetometer was exactly the same but the accels were slightly different. First the obligatory screen shots for both cases:

1. 2g/250dps (first pic is uncalibrated and the second is calibrated):
Capture0.PNGCapture1.PNG
2. 16g/2000dps
Capture2.PNGCapture3.PNG

Can't really tell much from the pictures. Except that the accel has very little offset and the scale factors should be relatively close to 1 (if you normalize). Anyway. If you put it in a spreadsheet and apply the correct scale factors and multipyly by G (like you do), there are differences, may be small but previous experience with Madgwick and Mahoney, probably enough to cause some drift at some point. Heres the table, sorry for the screen shot but not sure how to insert table here:
Capture.PNG

Maybe this will help in your calibration decision.

Mike
 
Added a few lines of code at the end of uNavAHRS::update for computing z-axis acceleration corrected for gravity regardless of board orientation. I use it for improving variometer responsiveness.
Code:
+++++++++++++++++
    // scaling of quertonian,||q||^2 = 1
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    for(size_t i=0; i < 4; i++) {
      xs(i,0) = xs(i,0)*1.0/sqrtf(xs(0,0)*xs(0,0)+xs(1,0)*xs(1,0)+xs(2,0)*xs(2,0)+xs(3,0)*xs(3,0));
    } 

    double ux, uy, uz;
    double rax, ray, raz;
    float accelwog;

    ux = 2 * (xs(1,0) * xs(3,0) - xs(0,0) * xs(2,0));
    uy = 2 * (xs(2,0) * xs(3,0) + xs(0,0) * xs(1,0));
    uz = 2 * (xs(0,0) * xs(0,0) + xs(3,0) * xs(3,0)) -1;  
  
    rax = ax/G + ux;
    ray = ay/G + uy;
    raz = az/G + uz;

    accelwog = (float)(ux * rax + uy * ray + uz * raz)*G ; 

  
    // obtain euler angles from quaternion
    theta = asinf(-2*(xs(1,0)*xs(3,0)-xs(0,0)*xs(2,0)));
    phi = atan2f(2*(xs(0,0)*xs(1,0)+xs(2,0)*xs(3,0)),1-2*(xs(1,0)*xs(1,0)+xs(2,0)*xs(2,0)));
    psi = atan2f(2*(xs(1,0)*xs(2,0)+xs(0,0)*xs(3,0)),1-2*(xs(2,0)*xs(2,0)+xs(3,0)*xs(3,0)));
    _magCount++;
  }
}

Looks like it works.
 
I am somewhat surprised by the yaw output compared to mag output. I expect mag output to remain more or less steady over one two hours. Not so with yaw. Expect to reflect at least earth rotation in best case - plus random gyro drift.
In a test both values stay rock steady - is yaw value not true and only "gyro"-yaw ?
 
Brian, I loaded up my sketch that allows me to output to a ellipsoid model for accel and magnetometer calibration. I ran a test for 2g/250dps and 16g/2000dps. As could be expected the magnetometer was exactly the same but the accels were slightly different. First the obligatory screen shots for both cases:

1. 2g/250dps (first pic is uncalibrated and the second is calibrated):
View attachment 12451View attachment 12452
2. 16g/2000dps
View attachment 12453View attachment 12454

Can't really tell much from the pictures. Except that the accel has very little offset and the scale factors should be relatively close to 1 (if you normalize). Anyway. If you put it in a spreadsheet and apply the correct scale factors and multipyly by G (like you do), there are differences, may be small but previous experience with Madgwick and Mahoney, probably enough to cause some drift at some point. Heres the table, sorry for the screen shot but not sure how to insert table here:
View attachment 12455

Maybe this will help in your calibration decision.

Mike

Hi Mike

nice coding for the calibration. Any chance you share the software ?
 
Its part of my FreeIMU library. If you are all interested I can modify the Brian's calibration sketch to make it compatible with his calibration method. There are slight differences in the way they work.

Edit: forgot - have a windows version derived from python and a straight up python 2.7 code if you have python.
 
@Brian Is the speed input for the filter IAS or TAS ? Might be somewhat accademic but at higher altitudes IAS becomes more and more less - but accel and gyro remain at first glance the same. Might well be that I am overlooking something - just a question.

Is there also a filtered/computed output for speed (taking into account acceleration) ? Would be interested in this for a better speed estimation.

Happy new year and thanks again for sharing your work. Like to read your code because I can read it and it's easy to use.
 
Hi Mike

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. Is Brian working with offsets for acc in the 9250 ? Can't follow the approach he makes at the moment but probably he has his reasons. 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)

by the way - I used this Filter.setAccelCovariance(0.001f); - I just wonder about the yaw, almost to good to be true
 
Last edited:
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.
 
Status
Not open for further replies.
Back
Top