uNav AHRS

Status
Not open for further replies.
Tim,
Here's Brian's MPU9250 library. He calls this for the 7-State and 15-State sketches.

https://github.com/bolderflight/MPU9250

Be aware that this library is titled the same as other MPU9250 libraries, so could cause confusion at compile time!

Yes, that's the library I have - lots of github grief over that the other day - but it doesn't hold that BaroSensor.h file unique to onehorse hardware.

I suppose it was built into the onehorse all-in-one sample code - and the barometer when I looked at that was not really stable as it was implemented for some reason. it would seem to get my elevation right ... in passing - it drifted and suggested "sea stays steady and the buildings keep washing up and down". (hitchikersguidetothegalaxy)
 
Mike: Edited out the BaroSensor and put in:
calibratedData.temp = 25; // BaroSensor.getTemperature();
calibratedData.pressure = 1100; // BaroSensor.getPressure();

Got init with my address and i2c update and then:
IMU initialization successful
Baro sensor initialization successful
nan, nan
nan, nan
nan, nan
nan, nan
nan, nan
nan, nan
 
@defragster. Did you run the Brian's calibration sketch that he included in the uNav library. Probably should be moved to the MPU9250 library in the future, hope Brian is reading this :)
 
Last edited:
Looks like I missed a few posts while I napped :)

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, anytime you feel comfortable sharing I am willing to give it a try.

I don't see that file on his site - checking my system? - if you could share that I could use it.
defragster I am attaching right now. Forgot where I got it from though. Onehorse had the baro calls all built into the mpu9250 sketch.
 

Attachments

  • MS5637-1.zip
    18.6 KB · Views: 314
I missed more than you @mjs :)

I updated calibration sketch to my i2c - though missed the steps for switch and MAG - but just doing something got me this:
Done
Starting Magnetometer Calibration
Done
IMU initialization successful
Baro sensor initialization successful
0.000000, 1.000000
0.000000, 1.000000
-0.070336, 1.001861
14.568401, 1.313210
-13.453047, 0.809301
-10.031553, 0.997136

I'll pull the newer zip to get the BaroSensor.h file. Indeed it is something my system never saw - as noted - onehorse published his all in one graffiti test sketch. Was so great to find Brian's Class'y update for the MPU9250 as noted weeks back.

In going by to pick up the .h file on onehorse's Tindie site - it look like he's now published a new board for STM and 10 DOF:
breakout board for ST Microelectronics' latest state-of-the-art suite of MEMS sensors including the LSM6DSM combination accel/gyro with embedded sensor hub, the LIS2MDL magnetometer, and the LPS22HB barometer/temperature sensor. ...
Preliminary tests show the LSM6DSM to be a worthy successor to the MPU6500, the accel/gyro combo embedded in the MPU9250 along with the AK8963C magnetometer. ... The combination of the LSM6DSM accel/gyro and the LIS2MDL magnetometer seems to produce results as good or better than the MPU9250 in terms of jitter and accuracy.
 
Thanks defragster - think I hate you - just ordered one. When I did a full calibration I got:
-0.056617, 1.010798
0.000000, 1.000000
-0.395821, 1.008275
28.611334, 0.993263
4.730581, 0.989000
-19.071671, 1.018231

By the way for local sea level pressure I usually get it from http://www.aviationweather.gov/metar?gis=off, metars/adds site.

Brian's library is without a doubt the best 9250 library I have seen. I use pieces of it for some of my other stuff that I work on.

EDIT: almost forgot - I changed from the 3.2 to the 3.5 and getting drift again - looks like I have to adjust some of the numbers again :)

Mike
 
Thanks defragster - think I hate you - just ordered one. When I did a full calibration I got:


By the way for local sea level pressure I usually get it from http://www.aviationweather.gov/metar?gis=off, metars/adds site.

Brian's library is without a doubt the best 9250 library I have seen. I use pieces of it for some of my other stuff that I work on.

EDIT: almost forgot - I changed from the 3.2 to the 3.5 and getting drift again - looks like I have to adjust some of the numbers again :)

Mike

Feeling's mutual ... in that I'm going to hate me when I feel I have to buy one too :)

I found three airports in the list making a line just N of my house - one shared Y and one shared X and Elevation from what I know.
"29.98 inches Hg (1015.3 mb) [Sea level pressure: 1015.7 mb]"
So I'd put the in here : "calibratedData.altitude = ((pow(((1015.7) ..."

Pulled off my GPS header wires so I could sit on desk and redid calibration to (not sure what this says):
0.000000, 1.000000
0.000000, 1.000000
0.000000, 1.000000
16.555841, 1.008553
23.351448, 1.007892
-0.213739, 0.983951
 
A quick update on my 7-State work...
- the 7-state still running very solidly for me
- I can get it to crash if I raise pitch to 90 deg, which is the typical Euler angle singularity issue. So I guess we need some code to work around that
- I've added a DCM routine to the sketch that converts accel body-to-inertialNED, and then removes gravity. So accel in inertialNED looks correct, but I'll want to try somethings using TerminalViewer to verify
- I added code to put data to serial port in CSV format so I can view data in real-time using TerminalViewer

I'll test with TerminalViewer in a bit, and then post a copy of the sketch for anyone that ones to try it.
 
Don: I downloaded TelemetryViewer_v0.4.jar and JRE [9.0.1 amd64] and double clicking TView does nothing? Wrong version or did I miss a step on my Win10x64 box?

@mjs: connected proper INT pin to 9250 and started running data stream. Seemed reasonable last night on motion - return to same desk orientation somewhat stable near same numbers. This AM it was streaming but numbers failed to track motion? On Reset it went back to tracking in some fashion.
 
I started by watching his TelemetryViewer post on youtube. It was a bit frustrating getting it going at first. I remember having an issue with TelemetryViewer if the correct JRE (32 bit vs 64 bit) is not installed. I know I contacted the author via his YouTube on TelemetryViewer, and I ended up re-installing JRE a couple of times until I got the right version, and then suddenly it all worked. I recommend setting up a sketch that puts a single variable to the serial port, and use that to make sure TelemetryViewer is working. Then going to two variables. I'm not a Windows IT guru, so if there's a way I can verify my version of JRE for you, let me know how and I'll try!

7-State Update: I added code to the uNavAHRS cpp and h file to prevent singularity on the theta (pitch) calculation, and that appears to have fixed the crashing problem.
 
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

Hi Mike,

Can you email me the thesis that this code is from as well? brian.taylor(at)bolderflight.com

I re-derived the 4 state EKF equations and implemented them from scratch. I take a couple minutes of IMU data during initialization, compute the variances for the different sensors and use that in the EKF. So far, it works great with the time update and accelerometer measurement update. However, I'm using heading for the magnetometer measurement update and there are a couple issues with this:
1. It seems like in taking three measurements and consolidating into a heading value, you're losing some useful information about the system state.
2. Taking the partial derivative of the heading equation with respect to the magnetometer variance (in order to transform the magnetometer variance into a heading variance) is non-linear and it's not clear to me yet how I can apply it.

I looked through the code you attached and it appears like the author is using a better approach to the magnetometer measurements.
 
I removed JRE 9 and installed JRE 8 and TViewer opened up.

<edit> Just decided to try that - it wasn't noted anywhere. I watched the video - impressive looking display capabilities. Would be nice to have same TViewer Layout file with sketch rather than streaming numbers. I'll try it with a PPS .vs. RTC .vs. CPU_Cycles
 
Last edited:
Attached is a preliminary 4 state EKF. I developed the time update and accel measurement update. The magnetometer measurement update is from Matthew Watson's thesis. Right now, I take two minutes during initialization to gather IMU statistics. The filter update should run at 100 Hz with new accelerometer and magnetometer data every frame. The update function just prints the result.

There are still some issues with drift, but I'm not sure exactly where to go with it. Hoping the 4 state code helps others.

Brian
 

Attachments

  • uNav4StateAHRS.zip
    5.4 KB · Views: 138
Hi Brian

Will give it a try later but I do have a question for you on the time step. When I converted Matt Watson's version of the EKF I passed it dt as measured from predict start to update end then did all the prints. Using this time step I kept getting terrible drift. When I changed the dt to between predict and serial print completes (larger time step) I got significant improvement in the drift (about 1 deg per hour or so. Could the problem be related to this.

Mike
 
Attached is one more version of the 4 state EKF. This is the same as above, except using measured heading for a magnetometer measurement update instead of Matthew Watson's approach. I also added the measured heading as a printout to see how the filter is comparing to the raw measurement. It's pretty interesting, at least in my environment, the raw heading angle is quite noisy, but the filter seems to hang in there ok.
 

Attachments

  • uNav4StateAHRS.zip
    5.4 KB · Views: 136
Hi Brian

Will give it a try later but I do have a question for you on the time step. When I converted Matt Watson's version of the EKF I passed it dt as measured from predict start to update end then did all the prints. Using this time step I kept getting terrible drift. When I changed the dt to between predict and serial print completes (larger time step) I got significant improvement in the drift (about 1 deg per hour or so. Could the problem be related to this.

Mike

That's interesting, I'll have to look into whether I'm over-running the frame or not.
 
It's pretty interesting, at least in my environment, the raw heading angle is quite noisy, but the filter seems to hang in there ok.
The raw measurements is about what I was seeing in uNavAHRS code after I did some tweaking of the aQ but the measured heading is hanging in there like you said.

Using the first version of the 4state I see drift but it seems to be very gradual not like the exponential growth we were seeing before. Now these were only short term tests will look a little more closely later.

Really like your approach on getting the IMU statistics automatically. Forgot how long 2 minutes can be :).

Great job as always.

Mike
 
Good morning all.

Just to let you know I tried out Tviewer with Brian's latest sketch and rather impressive for plotting. Thanks, Don for the reference. Biggest challenge with it is what to really plot to see what's going on with the Filter besides just yaw, pitch and roll.

Was also looking at printing out the statistics from the 4-state and found that the Eigen library as a IO format capability, io.h (https://eigen.tuxfamily.org/dox/IO_8h_source.html and https://eigen.tuxfamily.org/dox/structEigen_1_1IOFormat.html) the only question I have is what to do with ostream. So the question I have is will ostream as it is work with teensy and if not what do I do to change it?

EDIT: Did find a ostream.h and ostream.cpp that should work on Teensy. It was written by Bill Greiman for his SDFat library - maybe. Still looking.

thanks
Mike
 
Last edited:
While deriving a 7 state EKF, I found some errors with my 4 state EKF attached above. Standby for a fix.
 
Hi Brian

Quick question, is there a reason you are not using doubles instead of floats for the matrices?
 
Hi Brian

Quick question, is there a reason you are not using doubles instead of floats for the matrices?

Just processing speed; however, the literature recommends using as high of precision as possible for Kalman filters. Trying doubles might be worth a shot, I haven't checked to see if we're hitting any issues that way.
 
When you do the update I will give it a shot = both ways to see what happens.

The attached is updated. I *think* the equations are correct now, but it's still a work in progress.
 

Attachments

  • uNav4StateAHRS.zip
    5.4 KB · Views: 109
Status
Not open for further replies.
Back
Top