Working on getting Madwick algorithm sorted but think they have a bug (which I've found and solved) in one of the big matrices. Will need to email them to make sure I it is actually a bug
Authored by: macster110 2015-05-13
Parent: [r129]
Child: [r131]