uNav AHRS
- Thread starter brtaylor
- Start date
- Status
mjs513
Senior Member+
Just managed to get it hooked up and running yesterday. Hookup was a little confusing since he has interrupts set for mag, accel and baro. Also, had to fixed some I2C coding issues in the library which wasn't too bad. Have to do some more testing and want to hook it into the filter sketch to really test. But that's a project when we have all the kinks worked out. I really like Onehorse's board since I use i2c most of the time. I will keep you posted
Mike,
Shall I add the original Mahony filter in as MARG 2? I have a version from the Madgwick site, which is what Kris was using as well I believe.
EDIT: Just saw your other note... yes, a filter restart is a good option to have (for an operational implementation). Not thinking it's really needed for a multi-filter library, or it could be added later...
Don
Shall I add the original Mahony filter in as MARG 2? I have a version from the Madgwick site, which is what Kris was using as well I believe.
EDIT: Just saw your other note... yes, a filter restart is a good option to have (for an operational implementation). Not thinking it's really needed for a multi-filter library, or it could be added later...
Don
mjs513
Senior Member+
Go for more the merry
Should be the same or close to it. One of the things Kris does is iterate on the filter 10 times before the next update. Since the Madgwick and Mahoney filters are iterative to get to a solution it makes it more stable. He goes fun bore of course on the sampling. Another thing to consider in the future.Yep I agree just wanted to throw the thought out there while I remembered.
Not clear on what you mean by Kris iterates the filter 10 times before the next update. Does he set dt = 0, and then just loops through the filter 10 times?
On the sampling side, I need to get into that deeper. I suspect there's a lot to be gained by doing some smoothing of the IMU data before filtering, but care has to be taken not to cause a lag by doing so. In a tracking system I implemented last year, I did some real-time smoothing of optical data using a second order filter, and it worked really great. We also used these routines to "time-align" the data to make it easier to implement (sync) the KF update stages. I need to port those routines over from my (python and cpp) tracking code and make that into an arduino library as well.
On the sampling side, I need to get into that deeper. I suspect there's a lot to be gained by doing some smoothing of the IMU data before filtering, but care has to be taken not to cause a lag by doing so. In a tracking system I implemented last year, I did some real-time smoothing of optical data using a second order filter, and it worked really great. We also used these routines to "time-align" the data to make it easier to implement (sync) the KF update stages. I need to port those routines over from my (python and cpp) tracking code and make that into an arduino library as well.
mjs513
Senior Member+
I stand corrected I just went back into the GitHub code and its not there anymore. Think I need to update my copy.
I used a butterworth filter on the IMU data on accel and magnetometer data - didn't do it on the gryo for some reason - have to go back and check again. Know ardupilot uses filtering on the gyro data for sure. So yes at some point it would probably be good to do.
mjs513
Senior Member+
Brian
Going through the code slowly so I am going to ask questions as I go through as opposed to the end if that is all right.
In your F_ matrix for elements L_(1,4) through L_(1,6) your have qdot (my term for q*dt/2) qdot0, -qdot2 and qdot3. In the L_ matrix for L_(1,0) through L_(1,2) you have the subscripts as 0, 3 and 2 respectively. Looking at the original Nav code I thing the F_ matrix subscripts for those rows/columns should be changed to 0, 3,2? EDIT: Signs would to be changed as well. I hope my eyes didn't go cross when I read them.
Also, as another question, Q_ holds the gyro variances but I never see anywhere Q_ gets updated or is that intentional? I know P_ contains them also and that gets update here P_ = F_*P_*F_.transpose() + L_*Q_*L_.transpose(); Guess I need to dig into understand more about the update. Oh well - more reading.
Thanks for the patience
Mike
Going through the code slowly so I am going to ask questions as I go through as opposed to the end if that is all right.
In your F_ matrix for elements L_(1,4) through L_(1,6) your have qdot (my term for q*dt/2) qdot0, -qdot2 and qdot3. In the L_ matrix for L_(1,0) through L_(1,2) you have the subscripts as 0, 3 and 2 respectively. Looking at the original Nav code I thing the F_ matrix subscripts for those rows/columns should be changed to 0, 3,2? EDIT: Signs would to be changed as well. I hope my eyes didn't go cross when I read them.
Also, as another question, Q_ holds the gyro variances but I never see anywhere Q_ gets updated or is that intentional? I know P_ contains them also and that gets update here P_ = F_*P_*F_.transpose() + L_*Q_*L_.transpose(); Guess I need to dig into understand more about the update. Oh well - more reading.
Thanks for the patience
Mike
Last edited:
View attachment MPU9250_FiltersV7.zip
Mike,
Here's V7. I added the Mahony (which looks to be a slight variant of what you already had as AHRS), and verified its PRY output is correct. I also went through and cleaned up the code a bit (spacing, adding comments, title at top of each tab).
Looks really cool! We have 7 working AHRS filters!!
Note: I did not go back and mess with the DCM filter. It still starts off with biases in pitch and yaw, but the signs seem correct.
Mike,
Here's V7. I added the Mahony (which looks to be a slight variant of what you already had as AHRS), and verified its PRY output is correct. I also went through and cleaned up the code a bit (spacing, adding comments, title at top of each tab).
Looks really cool! We have 7 working AHRS filters!!
Note: I did not go back and mess with the DCM filter. It still starts off with biases in pitch and yaw, but the signs seem correct.
Mike,
In Brian's code, the L _* Q_ *L_.transpose() is the accumulated process noise term added at each propagation step. Sometimes this whole term is written as the process noise "Q", but in his case he has fixed noise variances (in Q_) and he "propagates" that variance by a function of the time step (L_ is function of dt). So the larger the time step dt for the propagation step, the larger the L_ and thus the larger the accumulated process noise, LQL.
In Brian's code, the L _* Q_ *L_.transpose() is the accumulated process noise term added at each propagation step. Sometimes this whole term is written as the process noise "Q", but in his case he has fixed noise variances (in Q_) and he "propagates" that variance by a function of the time step (L_ is function of dt). So the larger the time step dt for the propagation step, the larger the L_ and thus the larger the accumulated process noise, LQL.
mjs513
Senior Member+
Hi Don.
Just downloaded the file. Thanks for cleaning the code up and commenting. Now you can compare filters all in one spot. By the way thanks for the explanation on LQL term. Makes sense now. I saw that L_ and P_ are a function of dt but didn't keep going. Still have no idea what driving the drift. Watching the q-terms it looks like its trying to correct but there is not enough force to drive it fully in the opposite direction of drift. I keep going back to the gyro for some reason.
Thanks again
Mike
UPDATE: Your comment on LQL gave me an idea that I am in the process of trying. Will let you know if it works.
Just downloaded the file. Thanks for cleaning the code up and commenting. Now you can compare filters all in one spot. By the way thanks for the explanation on LQL term. Makes sense now. I saw that L_ and P_ are a function of dt but didn't keep going. Still have no idea what driving the drift. Watching the q-terms it looks like its trying to correct but there is not enough force to drive it fully in the opposite direction of drift. I keep going back to the gyro for some reason.
Thanks again
Mike
UPDATE: Your comment on LQL gave me an idea that I am in the process of trying. Will let you know if it works.
Last edited:
Mike,
I'll create us a "final" MPU9250_Filters (version 8) that will allow the user to select some number (maybe 3?) of filters and then compare them using TelemetryViewer. How's that sound?
I've also just set myself up a github account so I can put several of my code examples there. Might be something we'd want to do with this library at some point. What are your thoughts on that? I've been meaning to start using github so that I can see whether it's something I'll want to use more.
I'll create us a "final" MPU9250_Filters (version 8) that will allow the user to select some number (maybe 3?) of filters and then compare them using TelemetryViewer. How's that sound?
I've also just set myself up a github account so I can put several of my code examples there. Might be something we'd want to do with this library at some point. What are your thoughts on that? I've been meaning to start using github so that I can see whether it's something I'll want to use more.
defragster
Senior Member+
Cool - I just pulled down V7 - and there is V8 . Nice to see the #defines - for my case I should get it working with the other edits I need to moving the i2c pins. I should also get back to the Serial# isr code for when the GPS gets added and the RTC code to see if the time base improves using cycle counter versus micros.
GitHub is awfully useful - and also can be just awful - but better than posted code for tracking/merging changes and getting current version and having a current readme doc.
. Nice to see the #defines - for my case I should get it working with the other edits I need to moving the i2c pins. I should also get back to the Serial# isr code for when the GPS gets added and the RTC code to see if the time base improves using cycle counter versus micros.GitHub is awfully useful - and also can be just awful - but better than posted code for tracking/merging changes and getting current version and having a current readme doc.
mjs513
Senior Member+
Don
By the way ran the quatKalman and had one good run with an srd of 9 and 20hz dlpf - trying to repeat but can't (yaw stay wihin +/- 1 degree for about an hour then started diverging. Can't repeat it though. Tried a butterworth filter on the gyro but no luck. Anyway, something else is work. We could always just do a quaternion filter on pitch and roll and then get a tilt compensated heading separately. You could always do a kalman just on the heading to reduce the noise in the heading. i'm about ready to throw in the towel on this one.
EDIT: found a sparkfun library for DMP with the 9250, thinking about giving it a try.
Sounds good. It will definitely be a good thing to do instead of running each filter individually.
I use GitHub all the time - have tons of my project repositories plus others I cloned. You'll like it when you get use to it. Great tool for collaboration and getting changes incorporated.
By the way ran the quatKalman and had one good run with an srd of 9 and 20hz dlpf - trying to repeat but can't (yaw stay wihin +/- 1 degree for about an hour then started diverging. Can't repeat it though. Tried a butterworth filter on the gyro but no luck. Anyway, something else is work. We could always just do a quaternion filter on pitch and roll and then get a tilt compensated heading separately. You could always do a kalman just on the heading to reduce the noise in the heading. i'm about ready to throw in the towel on this one.
EDIT: found a sparkfun library for DMP with the 9250, thinking about giving it a try.
I'm also looking forward to integrating in GPS soon, which I think Brian is investigating options right now. My vote would be to start with a simple loosely-coupled KF running after the AHRS. Then later moving to a more tightly-coupled where the AHRS and GPS processing done in a single EKF.
If we do the loosely-coupled, we could pretty much use any of these 7 AHRS filters I guess. I'm thinking that the GPS KF would bring in accel readings from the IMU (transformed into NED-inertial) for the propagation step and then we blend that with both GPS position and velocity (both in NED-inertial) for the update step. If the transformations are done ahead of the KF, then the KF is linear and not even an EKF. Not sophisticated, but simple enough to start with. Or we could jump right into a more tightly-coupled EKF to start with...
Any other thoughts out there on this? The only trick I see for the loosely-coupled KF is getting IMU accel (Brian puts them out in NED-body) into NED-inertial. I tried this using a simple direction cosine matrix (using PRY values from the AHRS), but wasn't getting it to work well. So either I'm missing a step or the IMU accel values need intensive smoothing, or both. Any ideas on this? How have others done this? Do we kick off another thread for GPS Integration?
P.S. This thread has been especially interesting to me, have really learned a lot from y'all about software, timing, hardware, and algorithms!!!
If we do the loosely-coupled, we could pretty much use any of these 7 AHRS filters I guess. I'm thinking that the GPS KF would bring in accel readings from the IMU (transformed into NED-inertial) for the propagation step and then we blend that with both GPS position and velocity (both in NED-inertial) for the update step. If the transformations are done ahead of the KF, then the KF is linear and not even an EKF. Not sophisticated, but simple enough to start with. Or we could jump right into a more tightly-coupled EKF to start with...
Any other thoughts out there on this? The only trick I see for the loosely-coupled KF is getting IMU accel (Brian puts them out in NED-body) into NED-inertial. I tried this using a simple direction cosine matrix (using PRY values from the AHRS), but wasn't getting it to work well. So either I'm missing a step or the IMU accel values need intensive smoothing, or both. Any ideas on this? How have others done this? Do we kick off another thread for GPS Integration?
P.S. This thread has been especially interesting to me, have really learned a lot from y'all about software, timing, hardware, and algorithms!!!
mjs513
Senior Member+
Hi Don,
GPS integration - I will go along with the group consensus. As for NED body to NED inertial I have to think about that one. Thought I saw something on that somewhere.
GPS integration - I will go along with the group consensus. As for NED body to NED inertial I have to think about that one. Thought I saw something on that somewhere.
I agree completely on this one. By the number of views I think others like it as well. A good learning experience. Always wanted get into Kalman, just didn't think I would be doing now.
brtaylor
Well-known member
The loose coupling like you describe could be really interesting. With respect to the accelerometer integration not working well, are you subtracting gravity after transforming accel from body to NED? Also, accels will need to be in m/s/s; a lot of my AHRS code was normalizing them. You can find an example of this in the uNavINS code:
https://github.com/bolderflight/uNavINS/blob/master/uNavINS.cpp#L141
The 7 AHRS filters sound great! I'll have to take a look when I get a chance. A bit busy at the moment writing some flight code and putting together grant proposals.
Brian,
I was taking the accel values (NED-body) directly out of your MPU9250 library, removing gravity, then using a DCM (using PRY from AHRS) to transpose the accel values to NED-inertial. I thought that would work, but when I tried it out on my benchtop aligned to mag North, it seemed off. So I am removing gravity, and I'm not using normalized accel values from the ARHS.
I'll resurrect the sketch and try it again. Guessing I'm either needing to smooth the accel values first, or I'm missing a step or something.
Don
I was taking the accel values (NED-body) directly out of your MPU9250 library, removing gravity, then using a DCM (using PRY from AHRS) to transpose the accel values to NED-inertial. I thought that would work, but when I tried it out on my benchtop aligned to mag North, it seemed off. So I am removing gravity, and I'm not using normalized accel values from the ARHS.
I'll resurrect the sketch and try it again. Guessing I'm either needing to smooth the accel values first, or I'm missing a step or something.
Don
mjs513
Senior Member+
Don,
You talking about something like this, http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus
You talking about something like this, http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus
Mike,
Yes, I'm guessing something like this approach is the way to go, if the AHRS outputs quaternions. If it's an Euler AHRS, I think a DCM rotation is applied.
I'll give it a try. I'll need to figure out what reference frame he's using (NED, ENU, NWU, etc) as that might matter.
Yes, I'm guessing something like this approach is the way to go, if the AHRS outputs quaternions. If it's an Euler AHRS, I think a DCM rotation is applied.
I'll give it a try. I'll need to figure out what reference frame he's using (NED, ENU, NWU, etc) as that might matter.
View attachment MPU9250_FiltersV8.zip
Here's an AHRS Filters Version8, all I did was simplify the serial print so that it just prints PRY now.
I decided to just leave the sketch as just a "single-filter-select" sketch for now so I can go ahead and see if I can produce good values for accel NED-inertial.
My plan is to select one of the quaternion-based AHRS algorithms (maybe Madgwick, it seems pretty steady), and go from there to produce accel NED-inertial. Thought I'd try the DCM approach and that quaternion algorithm that Mike posted.
Here's an AHRS Filters Version8, all I did was simplify the serial print so that it just prints PRY now.
I decided to just leave the sketch as just a "single-filter-select" sketch for now so I can go ahead and see if I can produce good values for accel NED-inertial.
My plan is to select one of the quaternion-based AHRS algorithms (maybe Madgwick, it seems pretty steady), and go from there to produce accel NED-inertial. Thought I'd try the DCM approach and that quaternion algorithm that Mike posted.
- Status