slamjam / imumargalgorithm30042010sohm Goto Github PK

I try to put your IMU code into my project, the acceleration sensor is 
mma7260，the gyroscope is enc03。using the cortex-m3 series mcu sampling the 
voltage and calculate the three-axis acceleration and angular velocity.And then 
calls the function MadgwickAHRSupdateIMU(gx,gy,gz,ax,ay,az),Set the sample 
frequency 200.f and betaDef 0.1f,then computed the euler angles directly from 
quaternion data using the equations (7) (8) (9) mentioned in your excellent 
report.But the result is very extremly unstable,there are great error even 
though the platform is stationary,that is bad than simple integral.
could you reply my questions and help me solving the problem。
very thankful

This code:

hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + 
q0q2));

hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - 
q0q1));

bx = sqrt(hx * hx + hy * hy);

bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - 
q2q2));


Needs to be changed to this:


hx = mx * (q0q0 + q1q1 - q2q2 - q3q3) + my * 2 * (q1q2 - q0q3) + mz * 2 * (q0q2 
+ q1q3);

hy = mx * 2 * (q1q2 + q0q3) + my * (q0q0 - q1q1 + q2q2 - q3q3) + mz * 2 * (q2q3 
- q0q1);

bz = mx * 2 * ( q1q3 - q0q2) + my * 2 * (q0q1 + q2q3) + mz * (q0q0 - q1q1 - 
q2q2 + q3q3); 

bx = sqrt(hx * hx + hy * hy);

    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
You must use temporary variables for q0-q3 here, must not make the assignments 
directly, as q0-3 are used in the other equations.

What steps will reproduce the problem?
1. in IMUupdate set input for gyro always = 0 (that mean use only accelerometer)
2. run the IMUupdate fast ( 500hz (halfT of 0,001 or 0,002) ) 

What is the expected output? What do you see instead?
the expected output should move only pitch and roll. instead it also move very 
much on yaw.

Solved commenting out integral correction (or setting Ki = 0)

No issue!
Only thanks for your code.

I have used it to integrate Wiimote with a 3D OpenSceneGraph Terrain,
and it is very fine. Now i'm going to integrate an external magnetometer, 
to eliminate the yaw drift, and then i'll develop a real wii extension for the 
magnetomer.

It' very funny to fly with the wiimote!

Thanks

Hola, veo que entiendes castellano, espero que me puedas leer y entender.

Excelente trabajo el que has hecho, te felicito.

Te cuento mi experiencia y algunas dudas que tengo:

Tengo un WiiMotionPlus inside, estoy trabajando con las librerías de 
Wiiyourself! y he logrado hacer un juego con el mando. El juego lo hice 
controlando solo dos ángulos de movimiento pitch y roll que lo brinda la 
librería wiiyourself, pero nunca pude calcular yaw. Lo que hice para yaw es 
tomar los valores del giroscopio e ir incrementando o decrementando el angulo 
de acuerdo a la aceleración angular de yaw, pero los resultados son 
espantosos. He visto tu trabajo y me pareció muy bueno, así que decidí 
implementar tus algoritmos en mi juego. Pero no se si lo estoy haciendo bien, 
porque los valores del quaternion que da como resultado el calculo, no me 
parece que estén bien. Lo que hago es directamente tomar los datos del 
dispositivo y pasarlos al algoritmo:
MadgwickAHRSupdateIMU(remote->MotionPlus.Speed.Pitch, 
remote->MotionPlus.Speed.Roll, remote->MotionPlus.Speed.Yaw, 
remote->Acceleration.X, remote->Acceleration.Y, remote->Acceleration.Z);

Pero no entiendo que mas debo hacer, o si esto que estoy haciendo esta bien. 
Ademas, ¿como paso del quaternion a ángulos? ¿Que interpretación 
geométrica tiene el quaternion resultante? ¿que algoritmo usas para hacer un 
smoth en los movimientos, de tal forma que son fluidos y no torpes?

Ayudame por favor, 
Muchas gracias.
Saludos.

Hello congratulatios for this work. It is fantastic.

I am implementing your algorithm MARG filter in Matlab, but it dosen`t work, 
and I undertand why.

I attach my implementing in Matlab and my log test. 

Can you help my?

thanks

Dear sir,
What steps will reproduce the problem?
I recently used your MARG to test my sensor bought in Phidgets.(1056)
The drift in gyro is quite serious, even if I let the sensor stays still at the 
beginning. I tried to set the zeta at different value, but it didn't help much. 
What procedure can I take to make it works

My gyro just dun seem to have a constant drift, so is there other filter can do 
the job? i.e. kalman? attached picture is my gyro performance in fixed 
position(not moving it).
Just ignore the Magnetic field title in the pic. Coz I dun hv time to change 
it, it's gyro!

http://forum.sparkfun.com/viewtopic.php?f=14&t=22738&start=30
i read one of your posts in sparkfun, could I hv the kalman basics u posted 
before coz the link is dead.

Thanks a lot

Dear Sirs,

I have read your great report, and I have noticed that you have mentioned and 
also there are lot of references regarding motion of a human body. I would like 
to build a 9dof imu to analyse human running gait, I have results with 3 axis 
accelerometer, but I am not satisfied. I would like to ask you for your opinion 
if this technique that you are describing is the right way to analyse human 
moving. 
I was thinking about to buy ready made boards sold by Sparkfun,..., which 
integrates accelerometer, gyro and compass, all connected to i2c, but I'm 
worried about i2c timing- sensor data would not be captured at the same moment 
sychronously because of nature of serial communication. I have read that 
performance rapidly gains with sampling frequency and then remains almost equal 
for sampling rate of 500hz or 50hz, so at lower frequency also different 
timestamps of measured data would give less error (my way of thinking). What 
would be the adequate frequency for recording a running human ?

Anyway, thank you for great explanation in the paper.