Hi Everyone,

I'm currently using Madgwick's popular filter for my own AHRS project. After some experimentation with sensors and adjusting the beta gain, I've noticed some unpredictable behavior of the filter. For example, euler angles computed from the quaternion changed unexpectedly to 20 degrees off the reference angle (in a static scenario!).

After setting the magnetometer input to (0,0,0) the problem stayed the same. Then I removed the gyroscope inputs (0,0,0) and the problem stayed the same. My conclusion was that it must have been related to the accelerometer inputs.

After experimenting with real sensors I moved to artificial ACC input data and set up a test bed for Madgwick's algorithm (MadgwickTests on GitHub). I've figured out that in Madgwick's algorithm the fast inverse square root leads to huge instabilities when noisy measurements are applied. As an example, consider the pitch and roll angles shown in the following image:

The input for the filter was: gyro = (0,0,0), acc = (2.5, 2.5, 9.15)  +- 0.1 uniform noise (see test.c for details). As you can see, the pitch and roll angles (red/green, in degrees) of the original implementation show unacceptable variations.

Remarkably more stable and accurate pitch/roll angles (blue/magenta) were achieved by exchanging the inverse square root implementation. I used  the following code from Accurate and Fast InvSqrt for computing the inverse square root of float x:

unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
float tmp = *(float*)&i;
float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);

Please see the GitHub commit in which I've added some code for switching between the original (0), the proposed (1) and the reference (2) inverse square root implementations.
I hope that my investigations are helpful to improve the accuracy of Madgwick's C filter implementation.

Cheers, Tobias

Views: 43244

Reply to This

Replies to This Discussion

Hi Jeroen, I've tested the code finally - looks good! However, I have to adjust the Euler angle outputs as follows to get comparable results:

   pitch *= -1.0;
   yaw *= -1.0;
   if (roll < 0)
      roll += M_PI;
   else
      roll -= M_PI;

Cheers,

Tobias

Hello everyone! First of all I'd like to say THANK YOU VERY VERY MUCH to Jeroen van de Mortel for his formulae fix!

I had a problem with madgwick filter: when I gave my sensor more than 90 degrees rotation over roll or pitch axis (i.e. sensor z axis starts to point downwards) - resulting quaternion have become really shaky and constantly flipping +-180 degrees (q0 and q3 were changing signs constantly).

Also setting bigger and bigger values of beta parameter was making quaternion more and more stable (which is sort of the contrary of what is expected).

With described fix to gradient decent step flipping have stopped! That's great! I'm actually interested how does original code work on all those youtube videos I've seen.. hmm. Maybe drones just don't very much roll or pitch more than 30 degrees.

Anyway, now I have two more interesting problems.

Without magnetometer data - everything seems fine; I'm actually okay with not using magnetometer. But I'm still curious.

However, if I use magnetometer and roll or pitch angle approach 90 degrees - unwanted (but smooth) yaw rotation on 180 starts to happen. When I rotate sensor upside down over one axis I also get yaw shift of 180 degrees.
I'm using LSM9DS0 sensor; magnetometer z axis is inverse of accel and gyro. If I inverse magno z measurements on filter's input - this strange yaw rotation still occurs but in the other direction.

It's not supposed to be like that, is it?

Hi everyone,

I am applying Madgwick AHRS filter in Android in order to get the phone orientation (I used the source code in the published report). However, the output result seems to be not correct. For me it seems like there is a 90 degree rotation around the z axis (I just consider the yaw angle). I think the cause of this problem is the difference of used coordinate system. 

The coordinate system described in Android phone is as follow (as attached picture). Assume that a device is held in its default orientation, the X axis is horizontal and points to the right, the Y axis is vertical and points up, and the Z axis points toward the outside of the screen face. 

Android also provide function getOrientation()  that represent device motion or device position relative to the earth. However, the output is too unstable. That the reason, I want to use Madgwick filter. But, I could not find any information about the coordinate system used in this filter. Could you please tell me the coordinate system (sensor frame) using in Madgwick and the Earth output coordinate from converting quaternion to Euler angles as formula  mentioned in the report.

Sorry for this basis question. I appreciate for any input

Hi Tobias,

It seems that you have lots of experience in using Madgwick filter. Thank you for your sharing in this post. 

Currently, I am applying Madgwick filter in Android. But the result is not correct. I think the problem is the difference in coordinate system.What is the using coordinate system in sensor frame? What is the output world coordinate system? Could you please share some information? I want to apply Madgwick filter in Android. But I do not know the original coordinate system which Madgwick used to feed the input of update function from Android.

I am appreciate for your reply.

Hi Tim,

I am so confused about the coordinate system apply in Madgiwck filter. What is the output world coordinate system of the filter? Is it ENU (X point to East, Y points to North and Z points up). What should I feed into AHRSupdate() function if I apply Madgwick filter in Android mobile which has coordinate system as picture. 

I am appreciate if you can make me clear this point. Looking forward to your reply. 

Hi, 

I am new here and want to use Madgwick library for my drone. I use CORTEX M4, and as IMU have a STEVAL-MKI124V1.

I am not sure :

In which units I have to put a data to do function MadgwickAHRSupdateIMU? (Accel, Gyro, Magnet).

Do I have to callibrate all the sensors? (I am sure that with callibration I will have the best results, but for start, is it necessary?)

Also MY accelerometer is 16 bit I set +-2g - so I do this with RAW data from ACC:

 ACCE.X=(float)((AKCE.X/65536)*4);

My Gyro is 16 bits and I set 2000 dps - so I do this:

GYRO.X=(float)(GYRO.X/(65536))*2000;

I am doing something wrong, because those euler angles I am obtaining (I am not moving with sensors!!):

-->in attachment

Can you help me please?

Thank you very much.

Attachments:

You mention there has been some change to the code----yet two years later, it seems the source code has not been updated. In the versions of this C-code you claim to have found, was there any change to the Gyroscope Heading code? Your reference and samples discuss the Quaternion aspects of his code and not the Heading results...

Jeroen van de Mortel said:

These lines where changed in the code, sebastian emailed me that the code on the website was old and soon will be replaced by a new version.

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
_8bx = 2.0f * _4bx;
_8bz = 2.0f * _4bz;

// Gradient decent algorithm corrective step
s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax)    +   _2q1*(2.0f*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) +   _2q0*(2.0f*(q0q1 + q2q3) - ay) +   -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);             
s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax)    +     _2q3*(2.0f*(q0q1 + q2q3) - ay)   +   (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) +   _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);

The code was indeed never updated by sebastian, I found there was an optimization error in de code which is opensource when using the compass version. There was only an error in the quaternion part.

Thomas Solley said:

You mention there has been some change to the code----yet two years later, it seems the source code has not been updated. In the versions of this C-code you claim to have found, was there any change to the Gyroscope Heading code? Your reference and samples discuss the Quaternion aspects of his code and not the Heading results...

Jeroen van de Mortel said:

These lines where changed in the code, sebastian emailed me that the code on the website was old and soon will be replaced by a new version.

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
_8bx = 2.0f * _4bx;
_8bz = 2.0f * _4bz;

// Gradient decent algorithm corrective step
s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax)    +   _2q1*(2.0f*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) +   _2q0*(2.0f*(q0q1 + q2q3) - ay) +   -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);             
s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax)    +     _2q3*(2.0f*(q0q1 + q2q3) - ay)   +   (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) +   _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);

Let me see if I have this correctly.

If we want to use SM's C code for 6DOF (Mahony algorithm as I understand), the existing code is just fine.

If we want to use his C code for 9DOF (his algorithm as I understand), the code has two known bugs:

  1. the two quoted in this reply (magnetometer and quaternion calcs)

  2. the one much earlier about invsqrt.

Am I correct on both counts?

Are there any other issues?


Jeroen van de Mortel said:

The code was indeed never updated by sebastian, I found there was an optimization error in de code which is opensource when using the compass version. There was only an error in the quaternion part.

Thomas Solley said:

You mention there has been some change to the code----yet two years later, it seems the source code has not been updated. In the versions of this C-code you claim to have found, was there any change to the Gyroscope Heading code? Your reference and samples discuss the Quaternion aspects of his code and not the Heading results...

Jeroen van de Mortel said:

These lines where changed in the code, sebastian emailed me that the code on the website was old and soon will be replaced by a new version.

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
_8bx = 2.0f * _4bx;
_8bz = 2.0f * _4bz;

// Gradient decent algorithm corrective step
s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax)    +   _2q1*(2.0f*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) +   _2q0*(2.0f*(q0q1 + q2q3) - ay) +   -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);             
s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax)    +     _2q3*(2.0f*(q0q1 + q2q3) - ay)   +   (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) +   _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);

Indeed the 6DOF C code is correct, but the C code of the 9DOF not. I found there was a difference between the matlab code an the optimized C code of the 9DOF.

I don't know anything about the invsqrt.

Keith Laidlaw said:

Let me see if I have this correctly.

If we want to use SM's C code for 6DOF (Mahony algorithm as I understand), the existing code is just fine.

If we want to use his C code for 9DOF (his algorithm as I understand), the code has two known bugs:

  1. the two quoted in this reply (magnetometer and quaternion calcs)

  2. the one much earlier about invsqrt.

Am I correct on both counts?

Are there any other issues?


Jeroen van de Mortel said:

The code was indeed never updated by sebastian, I found there was an optimization error in de code which is opensource when using the compass version. There was only an error in the quaternion part.

Thomas Solley said:

You mention there has been some change to the code----yet two years later, it seems the source code has not been updated. In the versions of this C-code you claim to have found, was there any change to the Gyroscope Heading code? Your reference and samples discuss the Quaternion aspects of his code and not the Heading results...

Jeroen van de Mortel said:

These lines where changed in the code, sebastian emailed me that the code on the website was old and soon will be replaced by a new version.

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
_8bx = 2.0f * _4bx;
_8bz = 2.0f * _4bz;

// Gradient decent algorithm corrective step
s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax)    +   _2q1*(2.0f*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) +   _2q0*(2.0f*(q0q1 + q2q3) - ay) +   -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);             
s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax)    +     _2q3*(2.0f*(q0q1 + q2q3) - ay)   +   (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) +   _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);

Hi Jeroen van de Mortel:

I'm trying to implement the Magdwick filter on on my android glasses; however, I see a VERY large overshoot in the output when the glasses are subject to external acceleration... Here's what I have done so far:

i) Ensured all three axes are aligned

ii) Corrected the equations for s0 s1 s2 s3 based on the error you had found in the C-code

iii) Changed the inverse square root to a regular math function

Here's my code from Android. If there are ANY pointers you can give me, I would really really appreciate it. I was about to subtract the static error from the sensor data (which I didn't in the code I posted)...

package erau.efrc.getorientation;

import android.app.Activity;
import android.content.Intent;
import android.os.Bundle;
import android.view.View;
import android.view.WindowManager;
import android.widget.Button;
import android.widget.TextView;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

public class DisplayAttitude extends Activity implements SensorEventListener{

// Display-Object
TextView textAngles = null;
TextView betaValue = null;
Button goBack = null;
Button increaseBeta = null;
Button decreaseBeta = null;

// Sensor Manager
private SensorManager mSensorManager = null;

// Sensors
Sensor gravity = null;
Sensor magnetometer = null;
Sensor gyroscope = null;

private final float NS2S = 1.0f / 1000000000.0f; // Nano-seconds to seconds
private float timestamp = 0.0f;
private float dT = 0.0f;

// Initial beta value
private final float initBeta = 2.5f;
private final float finalBeta = 0.01f; // Found the filter gain emperically,
// needs to be optimized for vehicle accelerations
private final float initDuration = 15f; // Amount of time (seconds) initBeta must be used
private boolean initComplete = false;
private float beta = initBeta;
private final float betaIncrement = 0.01f;

// Save timestamp when the program starts up
private float initTime;

// Sensor-Measurements
float[] mGravity = new float[3];
float[] mGeomagnetic = new float[3];
float[] mGyro = new float[3];
float[] gyroBias = new float[3];
private int gyroBiasCount = 0;
private final int gyroBiasEst = 500;

// Orientation
float q0 = 1f, q1 = 0f, q2 = 0f, q3 = 0f;

// Define SENSOR_DELAY
private int SENSOR_DELAY = SensorManager.SENSOR_DELAY_FASTEST;

private void getSensors() {
gravity = mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
magnetometer = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
gyroscope = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
}

private void registerSensorListeners() {
mSensorManager.registerListener(this, gravity, SENSOR_DELAY);
mSensorManager.registerListener(this, magnetometer, SENSOR_DELAY);
mSensorManager.registerListener(this, gyroscope, SENSOR_DELAY);
}

@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.display_attitude);

this.getWindow().setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN,
WindowManager.LayoutParams.FLAG_FULLSCREEN);

/* Initialize everything */
textAngles = (TextView) findViewById(R.id.textAngles);
goBack = (Button) findViewById(R.id.bGoBack);
betaValue = (TextView) findViewById(R.id.betaValue);
increaseBeta = (Button) findViewById(R.id.bIncreaseBeta);
decreaseBeta = (Button) findViewById(R.id.bDecreaseBeta);

increaseBeta.setOnClickListener(new View.OnClickListener() {
@Override
public void onClick(View v) {
beta += betaIncrement;
betaValue.setText(getBeta());
}
});

decreaseBeta.setOnClickListener(new View.OnClickListener() {
@Override
public void onClick(View view) {
beta -= betaIncrement;
betaValue.setText(getBeta());
}
});

mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);

getSensors();

registerSensorListeners();
}

public void AttitudeToMainClick(View view) {
if(view.getId() == R.id.bGoBack) {
finish();
}
}

@Override
protected void onResume() {
super.onResume();
registerSensorListeners();
}

@Override
protected void onRestart() {
super.onRestart();
registerSensorListeners();
}

@Override
protected void onPause() {
mSensorManager.unregisterListener(this);
super.onPause();
}

@Override
protected void onStop() {
mSensorManager.unregisterListener(this);
super.onStop();
}

@Override
protected void onDestroy() {
mSensorManager.unregisterListener(this);
super.onDestroy();
}

public void onAccuracyChanged(Sensor sensor, int accuracy) {
}

public void onSensorChanged(SensorEvent event) {

if(initTime == 0.0f) {
initTime = event.timestamp;
betaValue.setText(getBeta());
}

// Change beta value if initBetaDuration is reached
if(initComplete == false && (event.timestamp - initTime) * NS2S >= initDuration) {
beta = finalBeta;
betaValue.setText(getBeta());
initComplete = true;
}

/* Remap the coordinate system so that X-Axis is along line-of-sight,
Y-Axis is to the right and Z-Axis is down */
float[] remappedValues = new float[3];
boolean remapResult = remapVector(event.values, remappedValues, SensorManager.AXIS_MINUS_Z,
SensorManager.AXIS_X, SensorManager.AXIS_MINUS_Y);

if(remapResult == true) {

if (event.sensor.getType() == Sensor.TYPE_GRAVITY) {
mGravity = remappedValues;
}

if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
mGeomagnetic = remappedValues;
}

if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
if(timestamp != 0.0f) {
dT = ( event.timestamp - timestamp ) * NS2S;
mGyro = remappedValues;
}

timestamp = event.timestamp;
}

//boolean success = MadgwickAHRSupdate(mGyro[0], mGyro[1], mGyro[2],
// mGravity[0], mGravity[1], mGravity[2],
// mGeomagnetic[0], mGeomagnetic[1], mGeomagnetic[2]);


boolean success = MadgwickAHRSupdateIMU(mGyro[0], mGyro[1], mGyro[2],
mGravity[0], mGravity[1], mGravity[2]);

if(success) {

// Formulas are obtained from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Eu...
double heading = Math.toDegrees((Math.atan2(2f * (q0 * q3 + q1 * q2), 1f - 2f * (q1 * q1 + q3 * q3))));
double pitch = Math.toDegrees(Math.asin(2f * (q0*q2 - q3*q1)));
double roll = Math.toDegrees(Math.atan2(2f * (q0*q1 + q2*q3), 1f - 2f * (q1*q1 + q2*q2)));

// Ensure heading is between 0 and 360 degrees
heading = 180 + heading;
// Ensure pitch is positive up
pitch = -pitch;
// Ensure roll is between -90 and +90
roll = roll span>0 ? (180+roll) : (roll-180);

// Display orientation
System.out.println("Roll: " + String.format("%.1f", roll)
+ " Pitch: " + String.format("%.1f",pitch)
+ " Yaw: " + String.format("%.1f", heading)
+ " " + getBeta() + "\n");
}
}
}

private String getBeta() {

return "Beta: " + String.format("%.3f", beta);
}

private boolean remapVector(float[] inVector, float[] outVector, int X, int Y, int Z) {
final int VECTOR_SIZE = 3;

// Return false if any duplicates exist
if(X == Y || Y == Z || X == Z) {
return false;
}

// Return false if the vectors are not 3-dimensional
if(inVector.length != VECTOR_SIZE || outVector.length != VECTOR_SIZE) {
return false;
}

switch (X){
case SensorManager.AXIS_X:
outVector[0] = inVector[0];
break;

case SensorManager.AXIS_MINUS_X:
outVector[0] = -inVector[0];
break;

case SensorManager.AXIS_Y:
outVector[0] = inVector[1];
break;

case SensorManager.AXIS_MINUS_Y:
outVector[0] = -inVector[1];
break;

case SensorManager.AXIS_Z:
outVector[0] = inVector[2];
break;

case SensorManager.AXIS_MINUS_Z:
outVector[0] = -inVector[2];
break;

/* Return false if an invalid axis parameter is passed */
default:
return false;
}

switch (Y) {
case SensorManager.AXIS_X:
outVector[1] = inVector[0];
break;

case SensorManager.AXIS_MINUS_X:
outVector[1] = -inVector[0];
break;

case SensorManager.AXIS_Y:
outVector[1] = inVector[1];
break;

case SensorManager.AXIS_MINUS_Y:
outVector[1] = -inVector[1];
break;

case SensorManager.AXIS_Z:
outVector[1] = inVector[2];
break;

case SensorManager.AXIS_MINUS_Z:
outVector[1] = -inVector[2];
break;

/* Return false if an invalid axis parameter is passed */
default:
return false;
}

switch (Z) {
case SensorManager.AXIS_X:
outVector[2] = inVector[0];
break;

case SensorManager.AXIS_MINUS_X:
outVector[2] = -inVector[0];
break;

case SensorManager.AXIS_Y:
outVector[2] = inVector[1];
break;

case SensorManager.AXIS_MINUS_Y:
outVector[2] = -inVector[1];
break;

case SensorManager.AXIS_Z:
outVector[2] = inVector[2];
break;

case SensorManager.AXIS_MINUS_Z:
outVector[2] = -inVector[2];
break;

/* Return false if an invalid axis parameter is passed */
default:
return false;
}

return true;
}

private boolean MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _8bx, _8bz, _2q0, _2q1, _2q2,
_2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
return MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
}

// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement
recipNorm = 1f / (float) Math.sqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

// Normalise magnetometer measurement
recipNorm = 1f / (float) Math.sqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;

// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = (float) Math.sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
_8bz = 2.0f * _4bz;
_8bx = 2.0f * _2bx;

// Gradient decent algorithm corrective step
s0 = -_2q2 * (2 * (q1q3 - q0q2) - ax) + _2q1 * (2 * (q0q1 + q2q3) - ay) -_4bz * q2 * (_4bx * (0.5f - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (-_4bx * q3 + _4bz * q1) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + _4bx * q2 * (_4bx * (q0q2 + q1q3) + _4bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2 * (q1q3 - q0q2) - ax) + _2q0 * (2 * (q0q1 + q2q3) - ay) -4 * q1 * (2 * (0.5f - q1q1 - q2q2) - az) + _4bz * q3 * (_4bx * (0.5f - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (_4bx * q2 + _4bz * q0) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + (_4bx * q3 - _8bz * q1) * (_4bx * (q0q2 + q1q3) + _4bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0*(2*(q1q3 - q0q2) - ax) + _2q3*(2*(q0q1 + q2q3) - ay) + (-4*q2)*(2*(0.5f - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5f - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5f - q1q1 - q2q2) - mz);
s3 = _2q1*(2*(q1q3 - q0q2) - ax) + _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5f - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5f - q1q1 - q2q2) - mz);
recipNorm = 1f / (float) Math.sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;

// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}

// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * dT;
q1 += qDot2 * dT;
q2 += qDot3 * dT;
q3 += qDot4 * dT;

// Normalise quaternion
recipNorm = 1f / (float) Math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;

return true;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

private boolean MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement
recipNorm = 1f / (float) Math.sqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;

// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = 1f / (float) Math.sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;

// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}

// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * dT;
q1 += qDot2 * dT;
q2 += qDot3 * dT;
q3 += qDot4 * dT;

// Normalise quaternion
recipNorm = 1f / (float) Math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;

return true;
}
}

Hello Paven Chinta

Have you calibrated your magnetometer?

Reply to Discussion

RSS

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service