uNav AHRS

[brtaylor]

Hi all,

I ported the uNav Attitude and Heading Reference System (AHRS), which is a 7 state Extended Kalman Filter (EKF) used to estimate attitude and heading from IMU data, to work well as an Arduino library.
https://github.com/bolderflight/uNavAHRS

This particular filter was developed by Jung Soon Jang at Stanford University and was used at the University of Minnesota UAS Research Labs since 2006. It hasn't been used as a primary navigation filter at the University of Minnesota in a long time, the 15 state EKF which include GPS data is used for that, but it provides good estimates of attitude and heading without requiring a GPS. It uses gyro measurements to propagate the state; accelerometers are used as a measurement update on the pitch and roll channels and magnetometers as a measurement update on the yaw channel to constrain drift. Optionally, airspeed measurements may be provided to correct apparent accelerations that can lead to errors in AHRS filters used on air vehicles.

Performance is great on a Teensy 3.6, it takes about 125 us for a time and measurement update. I've included an example of using this filter with the MPU-9250 IMU.

Best,
Brian

[defragster]

For land walking/running - does this have any application or benefits? Airspeed would be zero. Coordinate system would need resolved. Anything extra in this code that could drop out? I have a project I've been considering - have a lot to learn to get best IMU feedback I can and make use of it.

[mjs513]

Hi Brian,

First, happy holidays to all on the forum.

I just tried running the uNAV code on a T3.5 and it just seems to hang, i.e., no output whatsoever. The one change to the example sketch is that I am using I2C vs SPI. I did download your Eigen3.3.2 lib, mine was at 3.2.5 . I ran the calibration sketch no problem and I also ran the basic_i2c sketch from your updated MPU9250 library without issue. Any ideas?

@defragster - yes you can use an EKF for walking/running. Have a bunch of stuff on it. Let me know if you are interested and I will get it together for you and post the documents.

thanks
Mike

UPDATE: Figure it out. If I comment out the code for the interrupts (SPI only I guess) it runs fine. On I2C the output is showing about 705 microseconds. I am seeing quite a bit of drift in yaw values though with the example sketch. Will redo calibration just in case. I do have a couple of questions if don't mind. One of things with the kalman filter is setting the covariance values. Is there a simple way to determine these values?

regards
Mike

[brtaylor]

Hi Mike,

I just tried it out with a Teensy 3.5 on I2C, performance is a little slower, but the example runs fine. Silly question, do you have your interrupt going from the INT pin on the MPU-9250 to Pin 1 on the Teensy 3.5?

Brian

[defragster]

Hi Brian,

First, happy holidays to all on the forum.

I just tried running the uNAV code on a T3.5 and it just seems to hang, i.e., no output whatsoever. The one change to the example sketch is that I am using I2C vs SPI. I did download your Eigen3.3.2 lib, mine was at 3.2.5 . I ran the calibration sketch no problem and I also ran the basic_i2c sketch from your updated MPU9250 library without issue. Any ideas?

@defragster - yes you can use an EKF for walking/running. Have a bunch of stuff on it. Let me know if you are interested and I will get it together for you and post the documents.

thanks
Mike

Happy holidays to all indeed.

Good to know it works for sure - I suppose I should have asked if it adds anything worth extra CPU cycles for that pedestrian case? Perhaps seeing the bunch of stuff you might collect and post would help me understand. Based on my web searches so far I apparently don't know enough to find out what I don't know - so that would be a great addition to Brian's work.

[mjs513]

do you have your interrupt going from the INT pin on the MPU-9250 to Pin 1

Not a silly question. I am using a breakout board from DIYMALL and there is no breakout for INT or FSYNC available. Thought crossed my mine but guess it didn't fully register so just commented out the code for the interrupt to see if it worked.

As for the drift, after I let the chip warm up for about 15 minutes and restarted the yaw drift seemed to have steady out to a reasonable level. Use to worry about that with the 6050 when I first started with this stuff.

Mike

[mjs513]

@defragster.

Good to know it works for sure - I suppose I should have asked if it adds anything worth extra CPU cycles for that pedestrian case? Perhaps seeing the bunch of stuff you might collect and post would help me understand. Based on my web searches so far I apparently don't know enough to find out what I don't know

Its been awhile since I looked at that stuff but have a few good sources that you could use as a start. Just have to find the docs that I found the most useful. Give me a day to pull it together.

Mike

UPDATE: Here are a few links for you to ponder.

Open source embedded foot-mounted INS

Gait-Tracking-With-x-IMU

Personal dead reckoning system with shoe mounted inertial sensors

FootSLAM and PlaceSLAM

I have a nice overview article but I have to scan it.

[defragster]

@defragster.

UPDATE: Here are a few links for you to ponder.

Thanks, I popped them open and saved links. In following I came across this note that an SD card socket with and without a card in changes IMU behavior - so dummy SD cards were made to have one always in: github.com/xioTechnologies/Dummy-Micro-SD-Card.

I noticed those were 'foot' and pace based - I'll have to get hooked up and see how much of that comes through the hand/arm I was looking to mount to.

[mjs513]

Here is a link to folder I created on google drive. The article I mentioned is the Fischer one, https://drive.google.com/drive/folde...Hl?usp=sharing

Think this one is probably what you are looking for your application: http://umassgv.blogspot.com/2013/08/...rm-motion.html

[defragster]

Thanks - good info - maybe enough to confirm my suspicions of overall complexity. UMass is where my buddy got his Phd doing vision stuff. Funny it isolates the two arm imu's and ignores the extra translation of body movement. And they were $200 Bluetooth units tied to a computer doing 'Processing' - where the IMU had sensor fusion returning the orientation data.

Scanning the docs - foot mounted sensors offer some zero motion point for correction - and the EKF offers another level of smoothing in the paper-07 - but based on a stopped foot and determining running versus walking pauses. All of which is out the window with a single arm sensor - where the arm is randomly engaged in various unstable motions. So it seems indeed the EKF may offer some help for my task. Of course it quickly shows my underserved math skills with 30 years of atrophy will be limiting.

Onehorse mentioned a GPS might be a fit ( generally outdoors - perhaps a better compliment than a second remote foot sensor as the IMU will be lucky to track the hand like the UMass link - let alone project location. But it gives something new to look at it with.

[mjs513]

Onehorse mentioned a GPS might be a fit ( generally outdoors - perhaps a better compliment than a second remote foot sensor as the IMU will be lucky to track the hand like the UMass link - let alone project location. But it gives something new to look at it with.

Even outdoor GPS is not very accurate, found you can be about 30 feet off without a problem. Started playing around with RTK using old Neo-M8N's. Probably will upgrade those to m8t's soon. Anyway, you can get down to 20cm or so with those. If you are interested check out the rtkexplorer site. For indoor positioning was checking out awhile ago a few different approaches:

1. https://www.pozyx.io/ - fairly expensive
2. http://www.instructables.com/id/Loca...System-Arduin/
3. https://www.youtube.com/watch?v=PVMz9zop4Dg
4. http://evothings.com/diy-arduino-beacons/
5. https://inmagicwetrust.wordpress.com...acking-sensor/
6. https://hackaday.com/2016/01/23/amaz...nto-a-cartoon/

There was something else I saw but I can not find it now. EDIT: Found what I was looking for, actually had the unit already referenced, i.e., DWM1000 from Decaware. Here is another link you might be interested in: https://sites.google.com/site/wayneh...1000---part-ii

Scanning the docs - foot mounted sensors offer some zero motion point for correction - and the EKF offers another level of smoothing in the paper-07 - but based on a stopped foot and determining running versus walking pauses. All of which is out the window with a single arm sensor - where the arm is randomly engaged in various unstable motions. So it seems indeed the EKF may offer some help for my task. Of course it quickly shows my underserved math skills with 30 years of atrophy will be limiting.

Think the approach you want to take will be based on what your application is going to be. Remember the old saying the right tool for the job....

And they were $200 Bluetooth units tied to a computer doing 'Processing' - where the IMU had sensor fusion returning the orientation data.

Was actually looking at implementing this with Brian's library. Had two of Onehorse's 9250 boards attached to his 8266 board which then sent the orientation data back to the desktop using UDP. Other projects got me sidetracked.

[mjs513]

Hi Brian

Quick question. Do you have a copy of the jang&licardo paper you can share. Can't seem to find a free copy online?

thanks
Mike

[brtaylor]

Hi Mike,

Sure, here's the references:
https://www.dropbox.com/s/gyliyofmq8...06325.pdf?dl=0
https://www.dropbox.com/s/x9q8lgsds3...15066.pdf?dl=0

Not much detailed information, but the only information I could find on the origins of the algorithm. Seems pretty straightforward though.

Regarding interrupts from the MPU-9250, did we get that issue resolved? Is it just due to your breakout board manufacturer? They should work for both SPI and I2C given the proper breakout and I'd like to make sure it's not something with the library.

Interesting that you're seeing yaw drift, mine's locked on, but I did a pretty good calibration with a long USB cable to get away from all the electronics on my desktop. The best way to get an idea of the covariance is to compute the heading angle and print it to the screen.

    // initial attitude and heading
    theta = asinf(ax/G);
    phi = asinf(-ay/(G*cosf(theta)));
    // magnetic heading correction due to roll and pitch angle
    Bxc = hx*cosf(theta) + (hy*sinf(phi) + hz*cosf(phi))*sinf(theta);
    Byc = hy*cosf(phi) - hz*sinf(phi);
    // finding initial heading
    if (-Byc > 0) {
      _psiInitial = PI/2.0f - atanf(Bxc/-Byc);
    } else {
      _psiInitial = 3.0f*PI/2.0f - atanf(Bxc/-Byc);
    }

Which is from this reference:
https://aerocontent.honeywell.com/ae...netometers.pdf

But note their coordinate system is different than mine, which is why hy is negative in my code. I tried to pick a value that encompassed the maximum error that I was seeing with some padding (30 degrees), then converted to radians and squared it to get the default value of 0.27415570336144.

I'm working on converting our 15 state EKF to work well on Teensy. Once the filter is warmed up with GPS data, it should track heading angle much better. In the past, I've done some research using multiple IMU's on a fixed platform (i.e. multiple IMU's attached to a PCB). There's some cool things you can do there to eliminate errors, but I haven't had a chance to take some of that work to actual hardware yet.

[mjs513]

Hi Brian

Thanks for the reference material. Yes the library is working fine, the issue is just with particular breakout board I have, no INT available. I picked this one because it had a pressure sensor on the breakout board.

Will have to give the 15 state EKF a try when you get it going. Putting together a test platform (moded rc-car) to test out an RTK solution. Winding up with three different comm paths - I know I am crazy. I tested the connections in a static fashion and it seems to work. I am using the NeoGPS lib to decode the NMEA strings I am using. Working on some waypoint code - the code uses a waypoint flyby approach. Unfortunately winter has sent here so outside time will be limited.

Thanks again

[manitou]

FWIW, on T3.5@120mhz (Faster), timing just the Filter.update(), I get about 102 us. That compares with Paul's NXP filter update at 395 us. (T3.6@180mhz takes 268 us). And one can see the advantage for T3.5/3.6 hardware float, because on T3.2@120mhz Brian's Filter.update() takes 1642 us! Of course, at 100 hz, there is a lot of headroom, so speed is not as important as effectiveness. The uNav I2C data collection requires 654 us (SCL @ 400khz, really 340khz).

Curious. Doesn't RTK require a nearby sited base-station GPS+transmitter? I know there are some public base stations, but not densely populated.

EDIT: the library is optimized for float so the T3.2 is further penalized

[mjs513]

Just ran the example sketches with Onehorse's MPU-9250 and getting about 735 us on I2C. Redid the calibration of course and the yaw is rock solid as opposed to my initial comment. I don't have a 9250 with an INT and SPI available so I will have to leave that to others to try out.

Doesn't RTK require a nearby sited base-station GPS+transmitter? I know there are some public base stations, but not densely populated.

@Manitou - I am using rtklib and you can set up your own base station using a second GPS (m8t or m8n) or using NTRIP caster (Network Transport of RTCM data over IP is a protocol used to transport RTK correction data over the Internet). There are actually a lot of CORS base stations around you can use - https://www.ngs.noaa.gov/CORS_Map/ . With the M8Ns I am getting the solution on a PC then transmitting the solution back to rover in nmea so I am not actually sending a correction signal back to the m8n.

Mike

[manitou]

Just ran the example sketches with Onehorse's MPU-9250 and getting about 735 us on I2C. Redid the calibration of course and the yaw is rock solid as opposed to my initial comment. I don't have a 9250 with an INT and SPI available so I will have to leave that to others to try out.

Most (all?) of onehorses' MPU9250 breakouts do have the INT pin accessible. They are all hardwired for I2C. He has an optimized version of his library that uses I2C, DMA, and interrupts that runs really fast on his STM32L4 family (dragonfly, etc). No CPU cycles required for I2C transfers.

https://forum.pjrc.com/threads/40343...for-Teensy-3-6

https://forum.pjrc.com/threads/24633...l=1#post108823

[mjs513]

Most (all?) of onehorses' MPU9250 breakouts do have the INT pin accessible.

That's why I switch over to that breakout so I could test the sketch with the INT pin since the other one I had does not have it broken out. Just remembered I have another 9250 for spi only - will give that one a try - its the invense breakout.

He has an optimized version of his library that uses I2C, DMA, and interrupts that runs really fast on his STM32L4 family (dragonfly, etc). No CPU cycles required for I2C transfers.

Thanks for the info, wasn't aware of that. I heard about the Ladybug but had other things on my plate (still do for that matter), just saw his flight controller - interesting. I will be checking it out further. EDIT: If i2c_t3 is used couldn't you set the mode to I2C_OP_MODE_DMA for faster access?

[mjs513]

Thought I would give the MPU9250 lib and uNAV a shot on a ESP32. After a few modifications to adjust for no EEPROM and different wiring pins would up with 813us using I2C. This is just a FYI in case anyone is interested. I am using Onehorse's ESP32 and his MPU9250 breakout board so I am using the INT.

[hw999]

Hi

wanted to compile the example and received the following error message :

Arduino: 1.8.5 (Windows 10), TD: 1.40, Board: "Teensy 3.2 / 3.1, Serial, 96 MHz (overclock), Faster, US English"

In file included from C:\Users\Laptop\AppData\Local\Temp\arduino_build_899162\sketch\uNavAHRS.h:41:0,

                 from E:\Test_9250\uNavAHRS_MPU9250\uNavAHRS_MPU9250.ino:24:

C:\Users\Laptop\AppData\Local\Temp\arduino_build_899162\sketch\Eigen.h:61:22: fatal error: Eigen/Core: No such file or directory

compilation terminated.

Eigen/Core is locatedin Arduino\libraries\Eigen.

I admit I am in no way a champ in setting all up - to make it short I need some help

[hw999]

Hi

wanted to compile the example and received the following error message :

Arduino: 1.8.5 (Windows 10), TD: 1.40, Board: "Teensy 3.2 / 3.1, Serial, 96 MHz (overclock), Faster, US English"

In file included from C:\Users\Laptop\AppData\Local\Temp\arduino_build_899162\sketch\uNavAHRS.h:41:0,

                 from E:\Test_9250\uNavAHRS_MPU9250\uNavAHRS_MPU9250.ino:24:

C:\Users\Laptop\AppData\Local\Temp\arduino_build_899162\sketch\Eigen.h:61:22: fatal error: Eigen/Core: No such file or directory

compilation terminated.

Eigen/Core is located in Arduino\libraries\Eigen. The Eigen.h file is also in this directory.

I admit I am in no way a champ in setting all up - to make it short I need some help.

[manitou]

try getting rid of the Arduino version, and clone the latest in your sketchbook/libraries
https://github.com/bolderflight/Eigen

[hw999]

I downloaded Eigen from the link you provided. Do you mean there was an Eigen library around before I put the Eigen from bolderflight into the libraries folder ?

[mjs513]

try getting rid of the Arduino version, and clone the latest in your sketchbook/libraries
https://github.com/bolderflight/Eigen

That's exactly what I did and worked no problem.

Update:

I downloaded Eigen from the link you provided. Do you mean there was an Eigen library around before I put the Eigen from bolderflight into the libraries folder ?

Make sure you have Eigen-master in you Arduino libraries folder. Also, if you downloaded a previous version either move it out of the libraries folder or delete it.

[hw999]

Make sure you have Eigen-master in you Arduino libraries folder. Also, if you downloaded a previous version either move it out of the libraries folder or delete it.

Problem solved - I had Eigen and not Eigen-master in the folder. Thanks.

If a library comes as xyz-master - does it usually belong as xyz-master in the libraries folder ? Was not familiar with that