How to calculate accurate yaw using Magnetometer and Accelerometer

I’m currently using mobile device accelerometer+gyro+magnetometer attitude to get roll, pitch, and yaw. However, yaw is drifting and becoming not accurate over time.

I read there is a way to calculate the yaw using a combination of accelerometer and magnetometer that will keep yaw accurate with a compensation for the constant drift, however, I haven’t yet found a working formula.

Any idea?

UPDATE

I could compensate the yaw drift by using the following formula:

Create initial reference point (fixed world frame reference) using magnetometer:

m_w = (m_x,m_y,m_z)

Take current magnetomer point (also fixed world frame reference) using magnetometer:

n_w = (n_x,n_y,n_z)

Convert projected reading to angles

a = atan2(m_z,m_x)
b = atan2(n_z,n_x)

Yaw drift can be calculated as follows

y_d = (a-b) * 180 / PI

Now deduct result from current accelerometer yaw.

WALLA!