How to calculate Heading using Gyro and magnetometer

前端未结

关注

 1  472

余生分开走 2021-02-06 13:50

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 an

1条回答

迷失自我 (楼主)

2021-02-06 14:23
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.
0 讨论(0)
发布评论:

提交评论
- 加载中...