uNav AHRS

Status
Not open for further replies.
Also, it seems that if I leave the board alone, everything looks pretty solid even after an hour or so. But if I hold the board up and "fly" it around a bit, and set it back down where it was, pitch and roll are back to their original values, but yaw (and heading) might have a 15 degree difference now.
 
Just tried the same thing and saw the same behavior, with the exception that the variation I saw was about half what you saw :). Trying to get Paul's nxpKalman filter working with the Brian's 9250 library for comparison. Think I have a axis alignment problem that has to be resolved to make it work. But its 4:30am here and tomorrow is another day.
 
When I line up the MPU9250 x-axis to North (using my cell phone compass), the Heading for the 7-State shows 31 degrees. Per the readme file, Heading should read 0.0 degrees with x-axis pointing North. Does your Heading printout line up to North?

I was using Paul's Mahony/Madgwick implementation and I had to play around with the axis orientation as well. I like working in NED (like the 7-State default), but Paul was using ENU I believe. Also I believe the MPU9250 uses body ENU for gyro and accel and body NED for mag.
 
Hi Don

Haven't tried Heading yet. Will give it a try in a little while. As Brian pointed out he does the axis transformation within the 9250 library. I have been doing the same type of transformation with most of the sensors if the axes are not aligned in my other libraries.

The problem I am having with linking Paul's implementation of the Kalman filter for the propshield is that I am getting pitch positive when point down and roll negative when rotated to the right. The problem with Yaw I am having with that implementation is that I get no changes in yaw.

One of the issues I was getting confused with last night is why the accel//gyro z-axis is not changed to negative -z to line up with the magnetometer and the Y-axis is not negative to keep it to a right hand rule.

Mike

EDIT. By way of example why isn't the rotation
Untransformed to transformed
ax => ay
ay => -ax
az => -az
gx => gy
gy => -gx
gz => -gz
 
Last edited:
Hi Don
When I line up the MPU9250 x-axis to North (using my cell phone compass), the Heading for the 7-State shows 31 degrees

Just gave it quick run (short time span a couple of minutes) I got about 15 degrees.
 
For this filter, the absolute heading accuracy is only as good as the heading given by the magnetometer, which is only as good as the magnetometer calibration. The filter is initializing with a heading value, using the gyro to propagate it with time and, during measurement updates, taking the magnetometer measurements to correct it.
 
Just for the record I am not using Brian's built accel/magnetometer calibration methodology. I am using a calibration file that is generated from a calibration GUI similar to one the Paul uses for the propshield that uses a ellipsoid calibration technique. The tool is essentially the one for the FreeIMU libarary, http://www.varesano.net/blog/fabio/...celerometer-calibration-gui-alpha-version-out, but with slight modifications. To get it work I had to add back a function that Brian deleted from the old MPU9250 library to get the raw counts and then modify the example to load the coefficients. That may be why I am getting slightly different values.
 
Looking at the 7-State code, I noticed that the comments refer to the Fsys matrix as Phi. For nonlinear systems, I’m more familiar with Phi being the linearized (via Taylor series expansion) of the system dynamics matrix (often called F). So the first order expansion would be

Phi = I + Fsys * dt

Where I is the identity matrix.

When I look at the C version of uNavAHRS, the Fsys matrix is initialized to Identity, but in the Arduino version it’s initialized to Zero. So I changed the uNavAHRS.h file to initialize Fsys to Identity, and now yaw and heading seem to be working better. So I’m guessing in the AHRS code, the Fsys variable may really be Phi, and it needs to be initialized to Identity to reflect that Phi = I + Fsys * dt.

Anyway, wanted to throw this out and see what others thought!
Don
 
yes, here's the code:

// state transition matrix
Eigen::Matrix<float,7,7> Fsys = Eigen::Matrix<float,7,7>::Identity();

In the sketch, I'm using these values:
Filter.setAccelCovariance(0.025f); // also try 0.025f
Filter.setHeadingCovariance(0.125f); // also try 0.025f
Filter.setMagSrd(12);
// Set 41Hz DLPF bandwidth
//Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_41HZ); // Also try 20Hz
Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ); // Also try 20Hz
// setting SRD to 9 for a 100 Hz update rate
//Imu.setSrd(9);
Imu.setSrd(12);

I tried another experiment. I set my iPhone compass to "True North off" (i.e., magnetic compass not corrected to true North), and aligned the x-axis of MPU9250 to North on iPhone as best I could. It's reading Heading between 359 and 1 degree, so seems to be doing very very well at the moment.

I still can get the whole thing to crash (all "NaN's" to the serial port) once in a while if I "fly" the board around pretty wildly, but not unlike what a drone might do. So there's still robustness issues. And I'm not absolutely sure the Fsys set to Identity is correct, but it seems like it should be (from Phi = I + Fsys * dt), but would need to derive the dynamics to be sure.
 
Forgot to mention that I'm also using I2C instead of SPI bus, so I modded the code to be:

// an MPU-9250 object on SPI bus 0 with chip select 10
MPU9250 Imu(Wire,0x68);

Are you running I2C or SPI? If you're running SPI, how did you wire it to get it to work? I'm on a Teensy 3.6 by the way.
 
Sorry, I am running it as a I2C device on a T3.1. Probably going to try it on a T3.5 and see what happens. Got it run with your changes and seems fairly stable and off about my mag declination in NYC which is about 13 degrees. Still not happy with the variation in heading when stable. Almost needs a complimentary filter on the heading to get it somewhat stable. Have to try that tomorrow.
 
Great, glad it’s stable looking! Mine seems locked in.

A couple of things i want to try are:
- reduce the serial output to a rate of 2-5 hz and begin plotting parms real-time using TelemetryViewer
- change the loop to execute at a constant rate (maybe 250 hz) vs interrupt driven
- think about how to integrate in gps with the 7-state, or move to the 15-state
 
Don - seems you want to rely on interrupts or the data could be stale or miss readings with changes? Better to adjust the IMU output rate ( as best it can be ) and pull it on interrupt?


I was wondering about the value of timing - was not sure if dt ( delta time ) from tnow = (float) micros()/1000000.0f; was used from the device config - or since the last reading. Indeed that shows dt is based on sample arrival/incorporation. If the System can get a PPS signal the work I've started seems to get that to a finer degree ( with less system overhead ) than using micros. Without PPS the T_3.6 RTC can be used the same way - but will be subject to uncorrected drift over time. I started my sketch on RTC timing before I got the GPS added stats refined, then moved on when GPS w/PPS showed. As noted the offset within the PPS second is 99.8% in [ +/- 13 of ~240,000,000 cycles]. Stats over 55 hrs at the same time showed cycles counted per PPS were -300 to +142 of initial average [239,998,046]- included area inside the outer of three peaks [-100, 0 , +100] is only 44% and a lesser peak at -280 show the CPU clock shifts in relation to PPS {though 442/240,000,000 is pretty small}.

That time stamp can apply to incoming IMU data interrupt and the start bit of the GPS msg (when serial]. I see the UBlox GPS uses i2c for its independent MAG - and doesn't give an INT pin on data ready - so that MAG data may time differently.

I should finish the sketch to incorporate stamping the MPU9250 interrupt and GPS and post that - in writing the above have gone to seeing if the Rx Start bit can provide a replacement for PPS. If the GPS is determined to be regular on Serial updates - then ideally it will clock out the message on (multiple of) PPS clock.

yes, here's the code:
Code:
...
  // Set 41Hz DLPF bandwidth
  //Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_41HZ); // Also try 20Hz
  Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ); // Also try 20Hz
...

Using the "#" CODE encapsulation in the post stops such HTML finery
 
Tim,
Yes, if I just grab IMU values without interrupts, I'll just have to make sure that I'm running the void loop slower than I'm sampling the IMU. I've used the MPU6050 on drones at up to 400Hz without issues, although my default has been 250Hz. I've typically preferred to run my KFs / complimentary filters at even increments for most cases, makes the analysis and tuning easier for me. But doing it with interrupts has advantages too!
 
Hi Don

Been working on the approach used in the thesis that I sent you. The previous version is not the one described in the thesis. It was a matlab file that I thought was converted. When all said and done its very similar to that of Brian's uNAV version. I am attaching it in case you want to give it a shot. Haven't got heading implemented yet though.

Mike
 

Attachments

  • EKF_7State.zip
    7.1 KB · Views: 206
@defragster. BaroSensor.h is from a MS5637 library I use with Onehorse's mpu-9250. You don't really need it to run the filter. Forgot all about it, I was so engrossed with the getting the Filter to work. Just comment out the references to BaroSensor and calibratedData.temp, press and altitude. Sorry for the confusion. Going to do some more testing on both filters tomorrow.
 
Mike,
Sounds interesting, looking forward to seeing how it progresses! I read through the thesis, it's a great write-up. Thx!

I'm seeing what seems to be really good results with the 7-State right now, it has been running most of the day and appears very stable. I changed the serial prints to only send data to the serial port at 5Hz, and I've written a direction cosine matrix routine for converting the body-NED accel values to inertial-NED accel. I'll start testing tomorrow to see if the accel inertial-NED values seem to make sense. After that, I'm hoping to run a parallel slower Kalman that integrates the IMU and GPS (accel readings will be control inputs, GPS posn and vel will be measurement inputs, so very simple approach).

I'd also like to see the 15-state running, since it'll be a more tightly-coupled approach than me running two KFs.

I went through Brian's MPU9250.cpp code today, very impressive. Looks very well-written!!

Don
 
forgot to mention... am more than willing to share my code mods at any point as I plod along... just let me know and I'll post a version...

Don
 
@defragster. BaroSensor.h is from a MS5637 library I use with Onehorse's mpu-9250. You don't really need it to run the filter. Forgot all about it, I was so engrossed with the getting the Filter to work. Just comment out the references to BaroSensor and calibratedData.temp, press and altitude. Sorry for the confusion. Going to do some more testing on both filters tomorrow.

I'm using that onehorse mpu9250 too - I don't see that file on his site - checking my system? - if you could share that I could use it.
 
Status
Not open for further replies.
Back
Top