38 replies to this topic

[Jonathan Brandmeyer]

I have seen in the "filtering" thread that you are planning on implementing an EKF for the AHRS sensor fusion algorithm. Can you be more specific? There are a large number of navigational Kalman filter designs. Using the terminology of "A survey of nonlinear attitude estimation methods" by Crassidis, Markley, and Cheng, which of the standard methods most closely resembles what you are planning on implementing and why did you choose that method?

[PeterG]

Hi and welcome.

I don't think much more has been decided on EKF than what you find here in the forums, the project is still in an early phase.

Peter

[CheBuzz]

I also believe that an EKF is the proven best method. It is more computationally intensive, but for that cost you get a better state estimate. Somebody correct me if I'm wrong.

[Jonathan Brandmeyer]

I also believe that an EKF is the proven best method. It is more computationally intensive, but for that cost you get a better state estimate. Somebody correct me if I'm wrong.

There are many different EKF designs. There is the classic additive EKF, the family of multiplicative EKF's (using either rotation vectors, modified Rodrigues parameters, or Gibbs vectors in either the inertial or local frame), the modified additive EKF by Danial Choukroun, recursive QUEST, etc. I was just curious if one in particular had been settled on, but it looks like the answer is, "we haven't quite decided yet."

[dankers]

Any advice Jonathan? Are there any that stand out as the right way for us to go?

[CheBuzz]

Keeping in mind, of course, that the STM32 does not have a hardware floating-point unit. So floating point math is CPU costly.

[Jonathan Brandmeyer]

Keeping in mind, of course, that the STM32 does not have a hardware floating-point unit. So floating point math is CPU costly.

1) I would pick one of the square-root filters rather than a standard EKF. The square-root filters track the square root of the covariance matrix, which effectively halves the number of bits that you need in any given value. Since taking the square root will be somewhat slower than performing fixed-point division, the U-D factorization would probably be preferred. For the number of states involved in an AHRS (or GPS-INS), a U-D filter will cost roughly 10-20% more arithmetic ops than the equivalent 'standard' Kalman filter. The savings in this application comes from using fixed-point math. If OpenPilot was being developed on a microcontroller with floating-point hardware and sqrt() instruction (or even reciprocal sqrt approximation), then I would probably go with Carlson's triangular sqrt filter, instead.
2) I personally prefer the numerical characteristics of the multiplicative EKF over the additive EKF. It doesn't really matter that much if you track the error part in the body frame or the inertial frame. For my research, I picked the inertial frame.
3) Use rank-1 updates to avoid taking a matrix inverse when computing the Kalman gain.

Using all of those steps, I think that you can manage an AHRS alone in Q.15 fixed point. However, processing the accelerometer values is still somewhat tricky, thanks to dynamic acceleration on the vehicle when doing anything other than constant velocity flight. There are compensation methods available, like the one in ArduIMU, but I'm not thrilled with them. It might be possible to implement a complete kinematic GPS-INS, using floating point for the mean and Q.31 fixed-point for the U-D factored covariance. In that case, you will be using the GPS velocity report to figure out the attitude, because the cross-covariance term between velocity and orientation will be strong enough to carry that information.

The best references that I can think of right now would be
1) Attitude Error Representations for Kalman Filtering, by F. Landis Markley. Discusses the multiplicative EKF, with an emphasis on using the modified Rodrigues parameters for the error term. The focus is on the state propagation. I have a preference for using rotation vectors instead of MRP's, but the principles are essentially the same. If you use the MRP's then you can avoid trig functions.
2) Kalman Filtering of Spacecraft Attitude and the QUEST model, by Malcolm. D. Shuster. Discusses the measurement model for incorporating vector observations in the filter.
3) Triangular Covariance Factorizations for Kalman Filtering, by Catherine L. Thornton. Discusses the U-D factorization, including how to turn the standard propagation and measurement models into their U-D factored versions, and formulas for estimating their computational cost. I prefer her dissertation/report to JPL over her journal article describing the method, simply because it goes into much greater depth, with an eye towards implementation details. Her dissertation also covers the state propagation method published by Bierman, again in greater detail. She also covers all of the update mechanisms using rank-1 update mechanizations.
3) a) Gram-Schmidt algorithms for covariance propagation, by Catherine L. Thornton and Gerald J. Bierman. International Journal of Control, 1977
3) Measurement Updating using the U-D Factorization, by Gerald J. Bierman. Automatica, 1976

Don't let the fact that these papers discuss spacecraft fool you into thinking that they won't work on a UAV. The principles are absolutely identical. The one catch, is that a filter designed with this particular model doesn't converge very well from an unknown initial attitude, especially under the influence of attitude disturbances (beating this problem is the subject of my research). If you can start up with an initial state that is "mostly upright, with zero initial angular velocity", then the initialization problem mostly goes away. Just don't expect the system to converge if you have to reboot in the air, or while on a rubber raft in rough water, etc.

[flixr]

Hi,

nice summary Jonathan :-)

1) I would pick one of the square-root filters rather than a standard EKF. The square-root filters track the square root of the covariance matrix, which effectively halves the number of bits that you need in any given value. Since taking the square root will be somewhat slower than performing fixed-point division, the U-D factorization would probably be preferred. For the number of states involved in an AHRS (or GPS-INS), a U-D filter will cost roughly 10-20% more arithmetic ops than the equivalent 'standard' Kalman filter. The savings in this application comes from using fixed-point math. If OpenPilot was being developed on a microcontroller with floating-point hardware and sqrt() instruction (or even reciprocal sqrt approximation), then I would probably go with Carlson's triangular sqrt filter, instead.

Do you think it is actually worth doing that when you already have bad MEMS sensors? Of course you have to be careful that your covariance matrix stays symmetric, but you could just enforce that constraint now and then again by brute force so to speak. Just wondering if the extra complexity would give us any visible benefits here?

2) I personally prefer the numerical characteristics of the multiplicative EKF over the additive EKF.

dito!

It doesn't really matter that much if you track the error part in the body frame or the inertial frame. For my research, I picked the inertial frame.

You really want to track your attitude error in the inertial frame and not the body frame. If you estimate the error in the body frame you will have the full body dynamics in your error differential equations regardless of the fact that the errors themselves have low dynamics.

Cheers, Felix

[Jonathan Brandmeyer]

Do you think it is actually worth doing that when you already have bad MEMS sensors? Of course you have to be careful that your covariance matrix stays symmetric, but you could just enforce that constraint now and then again by brute force so to speak. Just wondering if the extra complexity would give us any visible benefits here?

Yes, I do think it is worth it for this application. Using the U-D filter gives you better numerical performance for the same datatype, or the same numerical performance with a narrower datatype. The benefit is in the reduced precision requirements on the terms in the covariance matrix. The implementation complexity isn't that bad, really. Now, if you actually have a double-precision FPU, then I wouldn't bother. But you don't. Not that I'm blaming you, uC's with decent FPUs on the market today tend to be difficult to use or expensive for low-budget short-run products.

You really want to track your attitude error in the inertial frame and not the body frame. If you estimate the error in the body frame you will have the full body dynamics in your error differential equations regardless of the fact that the errors themselves have low dynamics.

Cheers, Felix

You have some of the dynamics in the state transition matrix in either case. Assume that the discretization is just basic Euler integration.

If the error is in the body frame, then the state transition matrix is defined to be:
[toRotationMatrix(conjugate(step)), eye(3)*dt; zero(3), eye(3)] # matlab-ish syntax
where step is the quaternion computed by exp((omega - bias) * dt), omega is the measured gyro rate, and bias is the estimated bias, and toRotationMatrix(x) is a function that computes the 3x3 rotation matrix associated with the quaternion x. This assumes that the error vector is [dq', bias']', where dq is the rotation error.

If the error is in the inertial frame, then the state transition matrix becomes (to a first approximation):
[eye(3), toRotationMatrix(conjugate(q)); zero(3), eye(3)]
where q is the estimated attitude. At high sampling rates, it doesn't really matter if you use q(t) or q(t+1) or 0.5*(q(t)+q(t+1)).

So, in both cases, you have to compute a state transition matrix that varies over time, but the cost of that computation is about the same. Right?

[RoyLB]

Does anyone have any experience or at least knowledge of the following Kalman Filter "competitors"?

1)Double Exponential Smoothing-Based Prediction (Double Exponential Smoothing: An Alternative to Kalman Filter-Based Predictive Tracking - Joseph J. LaViola Jr.)

2) Divided Difference Filters (Attitude Estimation By Divided Difference Filter-Based Sensor Fusion - Peyman Setoodeh, Alireza Khayatian, Ebrahim Farjah)

3) Uncertainty Ellipsoids (Deterministic Global Attitude Estimation - Taeyoung Lee, Amit Sanyal, Melvin Leok, and N. Harris McClamroch)

4) Symmetry Preserving Observer (An Embedded Attitude and Heading Reference System Based on a Nonlinear Filter - Philippe Martin and Erwan Sala¨un)

- Roy

[Lew Payne]

I also believe that an EKF is the proven best method. It is more computationally intensive, but for that cost you get a better state estimate. Somebody correct me if I'm wrong.

On a machine with single cycle multiply and hardware divide (which the CORTEX-M3 line has), previously "computationally intensive" tasks become ordinary and mundane. As far as EKF providing a better state estimate, that's pretty subjective. An unscented Kalman filter can provide just as accurate, if not more, an estimate and yields a correct mean up to the 3rd order, and covariance up to the 2nd order. In addition, various other non-Kalman filters yield results which are more than adequate for hobbyist AHRS/MARG/IMU use. It's also derivative-free, thus requiring less computation even in crippled uC's without any hardware mul/div support.

[CheBuzz]

Ah yes, but the CORTEX-M3 does not have a hardware floating point unit. You will find that doing floating point multiplication and division on such chips are, in fact, computationally intensive.

[Lew Payne]

Ah yes, but the CORTEX-M3 does not have a hardware floating point unit. You will find that doing floating point multiplication and division on such chips are, in fact, computationally intensive.

That's what I get for working with both the TMS320F28335 (TI Delfino DSP processor with floating point) and the STM32 at the same time. Cross-wired and mixed up. I love the STM32, especially for the OpenPilot AHRS project (and I can't wait to order one). I'm going to enjoy writing different fusion code for the STM32 (AHRS), and hopefully testing the resultant flights in an objective and repeatable manner. I happen to be a fan of the OpenPilot project, due to the overall hardware choices that were made. Well, more than that really... when I saw the modularization and abstraction that was put into the software layers (including PiOS), I was sold.

Back to the subject at hand... with fixed point math, it's entirely feasible to write Kalman code on the STM32 with cycles to spare. On the Delfino (TI) chip, everything's a breeze of course (full floating point). I'm collaborating on a project, similar to OpenPilot, using the Delfino. I'd love to, when the time comes, port over some options (giving the user a choice of fusion algorithms) into the OpenPilot software library.

[PeterG]

First, thanks to everybody for sharing their knowledge. Many people here know way more about this topic than I do and I really appreciate it.

Does anyone here have any real world experience with the Unscented Kalman Filter? I haven't so far found anything negative about it as compared to an EKF, and it does appear to be more robust and handle nonlinearities better. This combined with similar computational complexity to EKF would make it an attractive alternative, but I don't know if there is more to the truth than what I've found so far in papers..

[Bani Greyling]

I found this reference work (Matlab) a while back on various kalman filters. Some of the demos are quite cool...

http://www.lce.hut.f...arch/mm/ekfukf/

The license is GPL

[Lew Payne]

This is an abstract from the Journal of Navigation, 2009, vol. 62, no3, pp. 365-382 [18 page(s) (article)] (1 p.3/4):

"Efficient and precise geolocation can be achieved by integrating a ranging system, such as GPS, with inertial sensors in order to bridge short outages, enhance accuracy degradation, and increase the temporal resolution in the ranging system. Optimal integration depends on appropriate filter methods that can accommodate the particular short-term dynamics experienced by platforms, such as UXO ground-based detection systems. The traditional extended Kalman filter was designed to integrate data from a linearized system excited by Gaussian noise. We compared this filter to modern filters that obviate these prerequisites, including the unscented Kalman filter, the particle filter, and adaptive variations thereof, using simulated IMU/ranging systems that follow a typical trajectory with both straight and curved segments. The unscented filter performed significantly better than the extended Kalman filter, particularly over the curved segments, yielding up to 50% improvement in the position accuracy using medium-grade inertial measurement units. Similar improvement was obtained for the unscented particle filter, and its adaptive variant, over the unscented Kalman filter (which performed comparably to the extended Kalman filter) when the statistical distribution of the IMU noise was non-symmetric (i.e., essentially non-Gaussian). While the few-centimetre geolocation accuracy goal for highly dynamic UXO characterization applications remains a challenge if tactical grade IMUs are integrated with a significantly degraded ranging system, using filters appropriate to the inherent nonlinear dynamics and potential non-Gaussian nature of the sensor noise tend to reduce overall errors compared to the traditional filter."

My favorite paper that talks about error rates, and an efficient non-tradition means of fusion, is this one:

An Efficient Orientation Filter for IMUs and MARG Sensor Arrays by Sebastian Madgwick

with C code available in this repository.

I'm curious to implement the above distinct approaches, as well as Sigma-Point, but I'm not quite sure how to create an accurate and repeatable testing standard. Any hints would be appreciated.

[Lew Payne]

One more thing... several studies, including the one by Ranjan Vepa [1], make it clear that accuracy of the typical Kalman filter can be improved in situations involving the transformation of a Gaussian distributed vector random variable. The problem is that when it gets converted into a quaternion, it loses its Gaussian distribution. In the case of an IMU/AHRS/MARG, that's exactly what we're dealing with. Vepa proposes a new predictor-corrector form of the unscented Kalman filter. His method maintains the normalization of the unscented mean quaternion estimate, even in the presence of additive disturbances. He was able to obtain a marked improvement (around 60%) compared to standard Kalman techniques.

[1] Spacecraft Large Attitude Estimation Using a Navigation Sensor, Ranjan Vepa (University of London)

[PeterG]

This is an abstract from the Journal of Navigation, 2009, vol. 62, no3, pp. 365-382 [18 page(s) (article)] (1 p.3/4):

This seems to be a report that covers the same information as the abstract you quoted :

http://www.geology.o.../report_493.pdf

[Bani Greyling]

This is an abstract from the Journal of Navigation, 2009, vol. 62, no3, pp. 365-382 [18 page(s) (article)] (1 p.3/4):



My favorite paper that talks about error rates, and an efficient non-tradition means of fusion, is this one:

An Efficient Orientation Filter for IMUs and MARG Sensor Arrays by Sebastian Madgwick

with C code available in this repository.

I'm curious to implement the above distinct approaches, as well as Sigma-Delta, but I'm not quite sure how to create an accurate and repeatable testing standard. Any hints would be appreciated.

(I hope I understood your reference to 'repeatable testing standard' correctly)
What about three orthogonal angular encoders, one mounted on the other. You then mount the sensor onto that. You will then have an accurate reference of the actual orientation, and be able to measure the phase difference between actual orientation and inertially estimated orientation.

An interesting exercise might be to wire up the OpenPilot AHRS with the demonstration application mentioned above. It have the filter implemented in C#, so just implementing simple firmware for the AHRS to stream the raw readings to a PC would show the filter in action.

[Lew Payne]

(I hope I understood your reference to 'repeatable testing standard' correctly)
What about three orthogonal angular encoders, one mounted on the other. You then mount the sensor onto that. You will then have an accurate reference of the actual orientation, and be able to measure the phase difference between actual orientation and inertially estimated orientation.

By "repeatable testing standard" I mean the ability to create and execute a standardized and thorough test with repeatable accuracy. I can then put the device through a hundred iterations or so, and observe disparities in trajectory (and sensory) data. The test would include various acceleration rates, at various angles (and multiple attitude translations throughout). I don't want to do the standard thing of rotating it in my hand about the various axes; I want the test itself to be more robust. I'm also not sure of how to accurately measure or even ascertain the "correct" answer (as far as sensory outputs) to the test. Therein lies the dilemma.

Of course, the above has to be repeated for the different filtering techniques under test. Producing a test with repeatable accuracy and known correct measurements along its entire path... that's something I don't yet know how to handle.