uNav AHRS

[mjs513]

Morning everyone. Have a question on using Eigen. I am getting an error that is driving me crazy. I am getting a lvalue error when I do these assignments and not really sure why:

_Euler(0,0) = x_(0,0);
_Euler(1,0) = x_(1,0);
_Euler(2,0) = xh_(0,0);

Even if I change to x_(0,0) to phi, I get the error. Help.

Thanks
Mike

 

[mjs513]

Again can't edit post 251. Found the problem. Didn't register - was one of those Oh S#$% moments.

 

[brtaylor]

Brian,
I ran the latest version all night, seems to have run fine.

What are your thoughts on next steps? Are you thinking about re-visiting the quaternion 4-State or 7-State and get that going, or using this Euler version and going ahead and integrating in GPS?

I like that we can easily separate the pitch and roll attitude from the heading using Euler angles. I think this lends itself well to approaches for sensing and rejecting magnetic disturbances, while keeping the vehicle stability as a priority. There are approaches to do this with quaternions, but they are not intuitive. Plus, I haven't seen a good method for keeping the pitch / roll update from also updating the heading with quaternions. So having good methods of separating the effects based on measurement sources makes using Euler angles for an AHRS the best approach in my opinion.

This doesn't matter for the GPS case since we're using the GPS measured position and velocities as the measurement update. There are no longer magnetic disturbances to worry about; attitude and heading will always be updated simultaneously and the measurement update is from the same measurement source.

So, I think euler angles make the most sense for an AHRS and quaternions make the most sense for a GPS update. The big question to me is whether you want your GPS update algorithm to be able to degrade to an AHRS + dead reckoning filter (or AHRS + airspeed and baro altimeter, or AHRS + airspeed, etc) in the case of extended GPS outages or system faults. In those cases, then I see an advantage to building up the EKF with Euler angles as the basis for the reasons that it makes most sense for an AHRS.

I'd like to test a singularity avoidance approach first, however, to make sure that can become a non-issue.

 

[Don Kelly]

Brian,
So it sounds like your thoughts are to go back to a quaternion AHRS scheme, get that working well, and then integrate in GPS measurements. If so, sounds good to me!

I'm thinking for most applications outdoors (which is what I'm interested in), I'll be using GPS (hopefully RTK even) and probably won't fly if GPS is too badly degraded. So simple seems good... maybe if GPS is gone we do a simple dead-rekoning (or for my application I land the drone where it is if possible).

So is there interest with the group in now going back to the quaternion 4-state, then 7-state, then 15-state? Or is someone wanting to keep extending this Euler version?

 

[mjs513]

I'm thinking for most applications outdoors (which is what I'm interested in), I'll be using GPS (hopefully RTK even) and probably won't fly if GPS is too badly degraded. So simple seems good... maybe if GPS is gone we do a simple dead-rekoning (or for my application I land the drone where it is if possible).

I do mostly ground applications (rovers) if GPS is "gone" or even GPS deprived for a period I could fall back on AHRS/Odometry without GPS.

So is there interest with the group in now going back to the quaternion 4-state, then 7-state, then 15-state? Or is someone wanting to keep extending this Euler version?

My two cents go back to the 4-state quaternion, and progress from there.

 

[defragster]

Not sure my input is good for 2 cents - but getting nearly usable detail with no GPS may be nicer. So if the fundamental value of the IMU if maximized and tuned that might allow that. It seems starting here on the basics has already improved the first pass values - so stepwise refinement building on that would allow trusting it to judge the GPS quality. Given my house seems to be moving about 6 inches/second was it? Perhaps vertical drift of 20 meters (+/- 10) over time with 7 or more satellites and HDOP near or under 1 [IIRC VDOP was not much higher in uCenter?] for short spatial moves the noise will be larger than the true change. That just made a thought: the incoming GPS stream could be piped out another Serial port [#3] to uCenter [debug uBlox lib edit for that] in parallel to another PROXY Teensy if that has any good feedback/monitoring of the data.

 

[Don Kelly]

One thing that might be interesting to try (if the T3.6 can handle the processing) would be to run the Euler EKF that we have now in parallel with the 4-state quaternion EKF. So with each void loop, we would

- read new IMU data
- run the Euler EKF
- run the 4-state quaternion EKF
- compare results (troubleshoot the quaternion until it works as well as Euler)

Then once the quaternion 4-state works smoothly, go to 7-state, then 15-state (with GPS).

Either way, it's cool work!

 

[defragster]

A quick recompile of the working code now to 48 MHz should show you it works unchanged - leaving some part of 180 Mhz (if you then jump to 240) to reprocess the data already in memory from IMU - Seems I had the Euler running at the 250 Hz rate and reading processing the GPS data stream. And at 24 MHz, other than losing the unclockable 2 Mbaud Debug output, the Tviewer was still its getting data - but that was ONLY the 250 Hz updates. Compile optimizations are a mixed bag at times - but indications were that FASTEST with LTO were nothing but the fastest based on the spare cycles I saw to pass through loop() going from under 1M to like 2.5 Million times with nothing to do to slow it down. And again without jumping to TThreads that was with the 'void yield(){};' added. Also making "RAMFUNC void xxx_isr()" can help assure the tiny IMU pin notification code stays resident can help it be more responsive where the T_3.6 ( IIRC and T_3.5?) have 32K on hand to do that.

 

[brtaylor]

Brian,
So it sounds like your thoughts are to go back to a quaternion AHRS scheme, get that working well, and then integrate in GPS measurements. If so, sounds good to me!

I'm thinking for most applications outdoors (which is what I'm interested in), I'll be using GPS (hopefully RTK even) and probably won't fly if GPS is too badly degraded. So simple seems good... maybe if GPS is gone we do a simple dead-rekoning (or for my application I land the drone where it is if possible).

So is there interest with the group in now going back to the quaternion 4-state, then 7-state, then 15-state? Or is someone wanting to keep extending this Euler version?

I was thinking of actually continuing with Euler based attitude in order to have a smoother transition in degraded states (i.e. extended GPS outage). I *think* the Euler singularity can be countered by creating a virtual sensor rotated something like 30 degrees in pitch from the physical IMU. Then we can run the EKF on both the real and virtual sensor, switching between them as necessary for avoiding the singularity, and rotating the final output back to the actual orientation.

So the time update would be:
1. Gyro measurement to Euler attitude
2. Euler to NED
3. Integrate accelerometers to NED velocity
4. Integrate accelerometers to NED position
5. 2D wind estimation

The measurement update would be:
1. GPS position and velocity
2. Airspeed and baro altitude
3. Magnetometer
4. Gravity vector

We could run all of the measurement updates based on measurement availability (i.e. if airspeed is supplied and updated, do a measurement update with airspeed). If the covariances are set appropriately, then the EKF should weight the measurements correctly making optimal use of all of the supplied information.

Any thoughts?

 

[Don Kelly]

Sounds like a really good approach. My only caution would be to do it in "baby steps" vs. a couple big steps. So perhaps we could bite off just 1-2 things at a time...

 

[brtaylor]

Sounds like a really good approach. My only caution would be to do it in "baby steps" vs. a couple big steps. So perhaps we could bite off just 1-2 things at a time...

Definitely agree with that!

 

[mjs513]

Hi all, just a few rambling thoughts.

We could run all of the measurement updates based on measurement availability (i.e. if airspeed is supplied and updated, do a measurement update with airspeed).

That's probably the best approach - based on availability of sensors including GPS.

I *think* the Euler singularity can be countered by creating a virtual sensor rotated something like 30 degrees in pitch from the physical IMU. Then we can run the EKF on both the real and virtual sensor, switching between them as necessary for avoiding the singularity, and rotating the final output back to the actual orientation.

My thoughts on this is that you adding another layer of complexity on the filter. While the quaternion approach is not necessarily intuitive it is more direct. Would be advisable to get the 4-state version yaw drift working first and then move on just in case?

 

[mjs513]

Hi Brian,

I started playing around the quaternion version of your Kalman filter again. I changed the dt calculation to use elapsed millis and also forced constant updates to the gyro mag and accel. Things did improve significantly from my past tests. However, I let it run for about a hour and after about 35 minutes the yaw started drifting again. I was keeping a close eye the gyro biases and noticed that there was a noticeable shift of the means in at least gy and gz. When plotted you can see the trend quite clearly. The gx bias was quite stable on the other hand. I was looking at the ekf filter that ardupilot uses and noticed that there is a gyro drift correction that they use. Maybe I will give something like that a try. Any thoughts on this approach?

Thanks
Mike

 

[brtaylor]

Hi Brian,

I started playing around the quaternion version of your Kalman filter again. I changed the dt calculation to use elapsed millis and also forced constant updates to the gyro mag and accel. Things did improve significantly from my past tests. However, I let it run for about a hour and after about 35 minutes the yaw started drifting again. I was keeping a close eye the gyro biases and noticed that there was a noticeable shift of the means in at least gy and gz. When plotted you can see the trend quite clearly. The gx bias was quite stable on the other hand. I was looking at the ekf filter that ardupilot uses and noticed that there is a gyro drift correction that they use. Maybe I will give something like that a try. Any thoughts on this approach?

Thanks
Mike

How do you mean gyro drift correction?

We know that in practice the gyro bias will drift, but assuming the drift is slow enough, I would think you could treat the bias as a constant and let the EKF update it over time.

 

[mjs513]

We know that in practice the gyro bias will drift, but assuming the drift is slow enough, I would think you could treat the bias as a constant and let the EKF update it over time.

I agree. If you look at the quick and dirty chart from excel I made for gbz (degrees), btw the x-axis is in minutes. The black line is about a .4 second moving average (max is 255 points for excel). Red line is just a linear fit. Anyway, the ekf adjusted accordingly, until it the deviation appeared to increase beyond a certain threshold that is in the second chart. Still want to run a couple of more test cases and print out the gyro data itself. If there is anything else you can suggest I take a look at please let me know.

How do you mean gyro drift correction?

Still have to trace where and what exactly they are doing. Ardupilot is extensive and things come from all over. Give me a day or two on that one.

 

[Don Kelly]

I let the Euler EKF run overnight a couple of times, and it seems like the innovation means (which are y-hx in accel and the one mag) had drifted a bit, but not too badly. So am thinking that eventually adding back in gyro and accel biases as constants would work well.

I was out of pocket today... spent most of day trying to get RTK working on the PixHawk2.1 IMU with Here+ u-blox (m8p I think) gps. pretty frustrating, tough to keep RTK lock, and the limited testing I did today didn't seem much more accurate than DGPS. Think there's a real need to get a solid RTK solution that works reliably...

Tomorrow I'll go back and re-look at Brian's thoughts on next steps. Am thinking we might want a smoothed set of body accel values that we then convert to NED accel values as control inputs to a second KF that uses GPS posn and vel as measurements. I tried this earlier, with smoothing the accel values first, and the NED accel values really jumped around...

 

[brtaylor]

One last option I hadn't considered earlier is using DCM matrices. Based on what I've read they lie somewhere between quaternion and euler angle based approaches in terms of accuracy vs performance (with quaternions having the highest performance, euler angles being the most accurate). Plus they shouldn't have gimbal lock issues. This is a really good article on a DCM based approach:
https://www.hindawi.com/journals/ijno/2015/503814/#B29

 

[mjs513]

Brian.

his is a really good article on a DCM based approach:
https://www.hindawi.com/journals/ijno/2015/503814/#B29

Took a quick scan looks interesting. Somehow I found this last night, that is related, https://github.com/hhyyti/dcm-imu. May give you a heads up on getting it set up. Going through some of the ardupliot stuff it looks like they run DCM in parallel to their EKF,

Plane and Rover will fall back from EKF2 or EKF3 to DCM if the EKF becomes unhealthy or the EKF is not fusing GPS data despite the GPS having 3D Lock. There is no fallback from EKF3 to EKF2 (or EKF2 to EKF1)

Edit: Kind of reminds me of an approach by tkj electronics, http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

Don
Yes setting up RTK can be challenging. I hope you have two here+'s. One on the copter and one for the base station. According to some stuff I read qgroundcontrol will help you get it set up. Not sure the details know you need two. There may be away to get it going with 1 but you need a internet connection to a NTRIP server. Found its pretty accurate, with my tests in a very challenging environment (almost no clear skies) its can get to less than a foot. First thing is to make sure you have a good lock to base.

 

[Don Kelly]

Brian,
Very cool paper. Great find. Nice comparison to Mahony and Madgwick. I could see some cool applications of this approach. For example, the authors claim this gives a very good pitch and roll estimate with noisy MEMS devices, but not so great at yaw (or heading). So (like Mike mentioned) this could be run in parallel with some other approach for yaw and heading. Maybe even getting heading from moving rover via GPS if rover moving enough and GPS is accurate enough (DGPS or RTK).

I'll download the author's code today and start studying it. Very neat!

Mike,
Yes, I'm using the whole Here+ system, which consists of an M8P for the reference ("base station") and an M8P for the rover (drone). I use a 915 MHz radio link to send RTCM corrections from the base to the rover using the Mission Planner software. My backyard setup has open skies, really good satellite visibility. "qgroundcontrol" is an alternative to Mission Planner software (maybe it works better...). Mission Planner is the defacto standard for drone work, but I might need to check out qgroundcontrol as well. I'll do some more testing today, but it seems like the Mission Planner RTK approach is not especially robust, and it's difficult to tell why (for example, it might be that the RTCM corrections are either inconsistent in my telemetry link or not at a sufficient rate, but can't easily pull this out of Mission Planner). I'm also near Ellington Field here in Houston, so I may be getting some EM field interference which could cause intermittent GPS dropouts. Hard to tell!

 

[mjs513]

Don

If you want to do a quick test try using rtklib using just the two Here+'s. It will show you a lot of details about the solution outside of Mission Planner. May help you in trouble shooting. rtkexplorer website has a lot of good info on rtk if you are interested. Don't think they have much on the M8P. Mostly they use M8T's. Don't know much about the m8p so can't be of much help there. the M8P is a little expensive for something I am just going to play with out of curiosity.

I took a look at the code and it is pretty straight forward. Will need some tweaking to incorporate Brian's methodology for bias calculations, etc. But not too bad overall. I use a DCM implementation that I found from Sparkfun for their old razor 9-axis board. It does have a drift correction routine embedded into it as well. If interested I can post.

Edit: Not sure if this pertains but it might. https://forum.u-blox.com/index.php/4315/c94-m8p-ublox-rtk-solution-compared-with-rtklib-rtk-solution

 

[mjs513]

Don-Brian

I converted the code from paper and put it into an runnable format on the Teensy. Not working and I know I am missing something - guess I need to read the paper . I am attaching in case you want to play with it while I go and take my nap .

 

[Don Kelly]

Thx for the RTK links, Mike. Good info there!

I'm going to spend a couple of hours this afternoon and resurrect my Mahony and Madgwick code (Winer's code originally) and get it working using Brian's MPU9250 library. I also have a nice complementary filter routine that only (Joop Brokking's) solves for Pitch and Roll, but seems very stable. Thought these might be useful as references/comparison at some point if anyone else interested.

 

[defragster]

If anyone goes to a side by side filtering of the data - as noted a second serial port could be used to pipe to a second screen/computer running Tviewer. In an "A" and "B" or intermixed so the same values were side by side without cluttering on a small display.

Of course tethering it to multiple wires compromises 'flying' more than one - might be good to have an offline mode where data is logged to SD while on a USB battery pack - do some choreography - then return to the computer and feed the stored data to TViewer. That would speak to using Serial# port to keep the unit powered - or the VUSB line could be cut to power from Bread board to allow USB to come and go with Teensy staying live. When it comes to GPS testing - that could end up being really useful to leave the desk and take a walk. So could adding an ILI9341 or similar display for live testing.

 

[Don Kelly]

Tim,
I was thinking of seeing how they look side-by-side, if the processor can handle it. Then I'd have them there to compare later too if needed as we add bias states and GPS inputs.

I haven't tried logging to this (T3.6) SD card, is it pretty straight-forward? Is there a particular SD card speed or format that works best with it? Can data be streamed at the same time to the Serial terminal (like for use by TelemetryViewer) and the T3.6 SD card?

 

[brtaylor]

I haven't tried logging to this (T3.6) SD card, is it pretty straight-forward? Is there a particular SD card speed or format that works best with it?

That's a big can of worms. It's easy enough to do simple logging to the SD card, but my understanding is that to do so in a low latency way takes a bit of work and you'll need an onboard buffer so you can do large writes instead of many small writes to the card. One approach that is simpler is perhaps using a second Teensy that just buffers data it receives from the first teensy (which is running the actual code) and writes to the SD card.

For simple logging for debug purposes, my favored approach on a linux system is to use minicom instead of the arduino serial monitor and to stream the minicom data to a file.

For my flight software stack, I stream data to a BeagleBone Black which writes the data to its EMMC.