Another AHRS algorithm

[Monroe King]

I was wondering if anyone had compared this algorithm with the original DCM code developed for ardupilot?

http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf

I was also wondering if it has been used in any other autopilot.

I'm interested if anyone has any info on this algorithm

[Robert Lefebvre]

I certainly remember hearing the name Madgwick quite a bit about 3-4 years ago. I think we did look at it.

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Monroe King]

Yeah I wonder if Bill used Mahoney's original algorithm or they came up with one of their own and if Bill's is a filter like Madgwick's and how they compare?

[Tom Pittenger]

I'm not entirely sure of this, but I think Mahoney came up with something first and then he worked together with Bill to refine it into a uav aircraft application.

[Chris Anderson]

As I recall,  the progression in the APM project was:

1) DCM
2) Experiments with quaternion - based solutions,  which didn't outperform DCM
3) Experiments with Madgwick,  which didn't outperform other quaternion solutions
4) Extended Kalman filters,  with more and more states, which is where we are now.

[Daniel Frenzel]

Unfortunately I am not a mathematician, but aren't quaternions just extensions of the the number system.

Consequently, the whole Kalman filter could be implemented based on quaternions?! 

[Monroe King]

Here is a paper on DCM from 1967 http://www.pdas.com/refs/tmx1384.pdf

It's taken a very long time but my understanding has gotten better. There is a guy that the "problem" is named after but it eludes me at the moment.

There is an equation that is the basis for all these solutions and these solutions are all variations of filters to smooth out the base equation.

DCM is not Bills creation but the filter used to smooth out the equation was if my understanding is truly progressing. Kalman is another type of filter there are particle filters and Madgwick, Bill and others all try to smooth out the base equation.
 
quaternions are a faster way to get a solution that can be converted to euler angles that are more useful for control.

[Robert Lefebvre]

Quaternions are another coordinate system other than our current.... cartesian?  They would solve the problem we currently have with gimbal lock at 90 deg pitch.  Quaternions are a different topic than Madgewich vs. DCM vs. EKF.  Jonathan Challinger has done some work integrating Quaternions into Ardupilot.  I think he even had something fully working?

--

[Monroe King]

"Quaternions are another coordinate system other than our current.... cartesian?  They would solve the problem we currently have with gimbal lock at 90 deg pitch."

True Quaternions do not gimbal lock.

Quaternions are used in all of these solutions I believe? It is the conversion to euler that causes the gimbal lock.

[Monroe King]

Wahba's problem is the base formula

https://en.wikipedia.org/wiki/Wahba's_problem

[Stephen Foy]

I've done couple of years on research of inertial sensors and KM filters a few years back,

The DCM the pure minimum way to combine data!
The full state extended kalman filter model is better estimate of sensor noise!
But at the end to the day, it down to the quality of the sensors and your processing power. Big topic!

A project back in 2002 at source forge http://autopilot.sourceforge.net, (the forgotten hero group for me)
shows you AVR code,  DCM type logic and then there dream was to have the power to have EKM full state model.
There are some great matlab simulations of full state EKM model, nearly the same code is now in pixhawk!!

[Daniel Frenzel]

Is the current EKF using quaternions?

[Monroe King]

DCM doesn't but I'm pretty sure the others do.

Things are starting to tie in for me now. Kinematics are used in 5 axis CNC for tool pathing. I never considered that the tool was "flying" over the stock. A spline is similar to a tool path.

Quaternions can be used to determine the path of sub atomic particles in physics.

Correct me if I'm wrong here I'm trying very hard to grasp this concept but the Kalman Filter is not solving the equation but filtering the result? DCM is solving the equation without filtering anything?

The speed of the solution also has a smoothing effect. The faster you solve the problem the smoother the flight. If other words an FPGA would be a better solution?

[jdennings]

My understanding is that Bill Premerlani  followed Mahony et al. original paper,  and presented an implementation of their filter using DCM representation (with one simplifying modification). As such  he  implemented a complementary (high pass/low pass)  filter for gyro and accel fusing (with a PI controller).

By the way  the term DCM has always been confusing when used in the context of navigation and IMU sensor fusion.  After all Direction Cosine Matrices are only a  mathematical way to represent rotations, just like Euler representations or quaternions, and one can convert back and forth. Useful for IMU fusion and navigation algorithm implementation, but only as a representation and doesn't say much  about the algorithm itself (which could also be implemented with quaternions).  In the context of his presentation and implementation  I think Bill probably  meant "IMU navigation  algorithm implementation  with DCM representations", or something like that,  but this just ended up nicknamed  "DCM".

Olliw has a great article  discussing IMU fusing and various  filters, including Madgwick's filter. Also gets into details as to the slight difference between Bill's implementation and the original Mahony algorithm
http://www.olliw.eu/2013/imu-data-fusing/

[Pando]

Extended implicates quaternions in this case, no?
Quaternions are better in all senses, simplifies operations, improve accuracy and remove problems like gimbal lock. Correct me if I'm wrong.

[Leebs]

'Extended' means that the underlying dynamics model being used to predict the next 'state' of the aircraft is nonlinear, as opposed to the non-extended Kalman Filter which uses a linear model. The EKF could use quaternions, DCM, Euler or whatever, but doesn't require the use of quaternions to be an EKF by definition. 

Quaternions are usually a good way to go and they do have those advantages as you say, but they are non-intuitive to us and thus can lead to issues debugging/validating. This is pretty much the only drawback as I see, so as long as you code diligently and write some conversion functions to express them in Euler angles to stop your mind melting.

[Jonathan Challinger]

Yes Robert, I have integrated Quaternions into the "DCM" AHRS in order to save CPU time on AVR. They are more computationally efficient in the application and are generally superior. You are correct that it is a completely different topic.

[Jonathan Challinger]

Regarding gimbal lock issues, they do not occur in the AHRS. Euler angles can represent any orientation. They're just another rotation formalism, but they have some bad properties. The gimbal lock issues happen in the controls, where we compute angle errors by subtracting euler angles. Gimbal lock is not the only issue that results - we also fail to take the straightest path between two orientations and that can mean we rotate hundreds of degrees to recover from a 90 degree error. Additionally, things like Copter's acceleration limits are applied very... improperly.

[Daniel Frenzel]

I feel, the Quaternion class need a revise. 

Especially, a type cast operator to and from Vector3f And Matrix3f would make sense, 

if you either want the vector or the rotation matrix.

It would be better to make a template class out of it, too.

These guys don't follow the logic of a "math" class.

	float get_euler_roll() const;
	float get_euler_pitch() const;
	float get_euler_yaw() const;

To be honest, now since AVR support was removed, 

I would replace the whole AP_Math library with a third party library for vector/matrix and quaternion operation.

Usually, they are more optimized, have more functionality and contain less mistakes. 

Optimum would be to use boost :D, 

but if it needs to be small the Eigen Library port for AVR (https://github.com/vancegroup/EigenArduino) would also do the job.

[Tom Pittenger]

Daniel,

Interesting idea but please start a new thread for this. We're drifting off-topic.

[Jonathan Challinger]

Daniel, absolutely agree, our conventions are completely bizarre and we need to fix that.

On Tue, Sep 15, 2015 at 11:39 AM, Daniel Frenzel <dgda...@googlemail.com> wrote:

[Monroe King]

Well I'd like to thank you guy's for keeping me sane I've been looking at this for years and some of the code makes me nuts because some of it makes no sense to someone learning this the hard way (without formal education in higher math) I believe however it is not impossible and I've been taking it in little chunks. I'm still not there yet but I do see it possible for me to get there sooner rather than later.

I don't think any discussion about any AHRS is off topic on this thread. But indeed specifics probably deserve a new one.

I'm finding this very helpful indeed.    

[Monroe King]

One important question to me, is our current EKF built on Mahony's original work on top of Bill's filter?

[Robert Lefebvre]

No, I don't think so.  I think the concepts are mutually exclusive.

[Monroe King]

From my current understanding I think you can build a filter based on any solution to Wahba's problem.

Surely it's not based solely on the original solution and Bill's filter. But it could be a mixture of both and applying the EKF to that.

Right now I'm at the stage where I'm trying to pull apart what we really have here.