Improved accelerometer and magnetometer-based solution to solve quaternion

Light overloaded small aircraft, such as ducted fan MAV, could use accelerometer and magnetometer to measure acceleration and earth magnetic field in the body coordinate frame. There is a quaternion that relates the measured accelerations and earth magnetic field in the body coordinate frame to calculated values in the earth coordinate frame. An overdetermined nonlinear equations which could get the quaternion need to be solved. So far, the Gauss-Newton iteration algorithm was being used most commonly to solve the overdetermined equations. However, when the inverse of Jacobian matrix is singular or conditioned, the iteration would be diverge. To overcome the problem, first, transferring the overdetermined problem to determined one by a premultiply transposition of Jacobian matrix, then using Correctional Newton with Parameter iteration algorithm to solve the determined equations. Compared to Gauss-Newton iteration algorithm, this method not only could converge more fast, but also could overcome the situation which Jacobian matrix is singular or conditioned.