How to calculate Heading using Gyro and magnetometer

trigonometry gyroscope magnetometer heading sensor-fusion

Pratik · Jan 28, 2016 · Viewed 10k times · Source

I have raw data of Gyroscope and magnetometer. on basis of this data, i have to calculate Heading information with an accuracy of 1Deg.

I would like to know, is there any algorithm which can help me to get heading info using available information with mentioned accuracy?

Any article or any link also will be helpful.

Thanks

Answer

I am assuming you are using phone to collect gyroscope+magnetometer data.

You cannot calculate Heading with Gyro+Magnetometer unless you know initial state of device. You have to try Accelerometer+Magnetometer. You can get heading values from rotation-matrix' oryaw` component from Motion sensor API of Android/IOS device.

If you want to implement of your own try tilt-compensation algorithm. All you have to do is implement tilt- compensated ecompass using accelerometer and magnetometer. You get accelerometer and magnetometer readings in device frame. Accelerometer and Magnetometer gives readings as a function of rotation of device. This rotation contains information of Roll, Pitch and Heading of device. Following is explanation and derivation of heading estimation using accelerometer+magnetometer.

Tilt compensation algorithm

Let accelerometer and magnetometer give you

$\begin{bmatrix} G_x\\ G_y\\ G_z \end{bmatrix} and \begin{bmatrix} H_x\\ H_y\\ H_z \end{bmatrix}$

readings respectively. They are in device frame, and device is rotated through

$\begin{bmatrix} \theta (roll) & \phi (pitch) & \psi (yaw) \end{bmatrix}$ .

Where $\psi (yaw)$ is heading you are looking for. So rotation matrix of device will be

$R = R_y(\theta)*R_x(\phi)*R_z(\psi)$

$\begin{bmatrix} G_x\\ G_y\\ G_z \end{bmatrix} = R_y(\theta)*R_x(\phi)*R_z(\psi)*\begin{bmatrix} 0\\ 0\\ g \end{bmatrix}$

$\begin{bmatrix} H_x\\ H_y\\ H_z \end{bmatrix} = R_y(\theta)*R_x(\phi)*R_z(\psi)*\begin{bmatrix} Bcos\delta\\ 0\\ Bsin\delta \end{bmatrix}$

We have to solve for $\psi$ . So step by step solving for $\psi$ .

$R_x(-\phi)*R_y(-\theta)*\begin{bmatrix} G_x\\ G_y\\ G_z \end{bmatrix} = R_z(\psi)*\begin{bmatrix} 0\\ 0\\ g \end{bmatrix}$

From above, equating to zero we get

$tan\theta = (-G_x/G_z)\\ tan\phi = G_y/(-G_x*sin\theta + G_z*cos\theta)$

Having $\theta, \phi$ from above

$resB = R_x(-\phi)*R_y(-\theta)*\begin{bmatrix} B_x\\ B_y\\ B_z \end{bmatrix}$

$tan\psi = ResB_x/ResB_y$

This is basic method of obtaining Heading value from accelerometer+magentomete. Yaw/Heading value is provided by Motion sensor APIs present in mobile phones.

You have to look at few things while implementing sensor-fusion algorithms. Accelerometers doesn't always gravity direction, it gives direction of ovarall acceleration experienced by the sensor eg, devices under linear accelerations. Magnetometer readings are subjected distortion due to ferromagnetic substances in vicinity. Accuracy depends on systematic bias component in gyroscope sensor too.

There are numerous algorithms to estimate accurate heading values. Some of them are

Madgwick AHRS
Complementary filter
Kalman filter
Compensation of Magnetic Disturbances, this algorithm gave best results among all.

How to calculate Heading using Gyro and magnetometer

Answer

Related questions