For some reason the version in the paper diverges when I use it. I want to use the paper version because it should be more accurate and it estimates gyro biases. Below are the results from the common version and the white paper version:
I attached a zip file with the MATLAB data/scripts to reproduce those results. I have tried changing which axes are which and varying the tuning parameters, but it always seems to diverge. Has anyone successfully used both versions of the filter?
The sensors are an ADIS16355 for accel/gyro and LSM303 for mag.
Hi
I m trying to implement the same code for my arduino.
I succesfully connected my Arduino Pro Mini and my AltIMU10DOF (from Pololu) and I get the raw values of the accelerometers,, gyros and magnetormeters without any problem. I get clear readings and I can play with the option throught I2C to get what I need. That s ok.
The big problem cames by implementing the algorithm itself. Since I get raw values from the sensors I really don t knot how to feed them into the C code that MARG gave us to our disposal here.
Now...what I don t understand is at the beginning of the code
// Math library required for ‘sqrt’
#include <math.h>
// System constants
#define deltat 0.001f
#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f)
#define beta sqrt(3.0f / 4.0f) * gyroMeasError
// sampling period in seconds (shown as 1 ms)
// gyroscope measurement error in rad/s (shown as 5 deg/s)
// compute beta
// Global system variables
float a_x, a_y, a_z;
// accelerometer measurements
float w_x, w_y, w_z;
// gyroscope measurements in rad/s
float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f; // estimated orientation quaternion elements with initial conditions
void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z)
{
the gyro must be converted in rad/s and that ok. But what about accelerometers and magnetometers? Are they easly taken and passed to the function "filterUpdate" or should I at least remove some offset from the raw readings?
Of course I can read all the sensor for le t say 2 seconds, save their results and then calculate the mean value. If I leave my sensor board level with the ground I get 1 G pointing towards the grounds. That s fine...but my question is: should I calcutate this vector or should I I feed all values to the function?
I try looking into FreeIMU library, and some other code in internet but I cannot understand this problem.
Since you realize the same code for matlab and then for arduino, maybe could you please help me with some hints?
It sounds like you have the right idea. The filter doesn't need the accelerations or magnetometer measurements in any specific units because it uses them to compute a unit vector's direction. As long as the three components have the same scale factor, the actual value of that scale factor doesn't matter. Depending on the quality of your sensors and what you're trying to achieve, you may not need to remove an offset from the accels/mags, but you'll get the best results if you calibrate them to find where zero is for each axis.
I tried right now to implement your code but I found two variables that I never saw: b_x and b_z
What are those variable standing for?
Furhtermore. Do you initialze the quaternion (at the starting point of your program) with a predefined value? (like the g gravity vektor [0,0,-1] or just with the default values:
q.w = 1.0;
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
?!?
Thank in advance for your answer. I ll let you know that the progress of my work.
Reagrds
b_z and b_x represent the filter's estimate of the direction of Earth's magnetic flux. I initialize b_x to zero and b_z to 1 and the quaternion to 1,0,0,0.
Furhtermore. Do you initialze the quaternion (at the starting point of your program) with a predefined value? (like the g gravity vektor [0,0,-1] or just with the default values:
Code:
q.w = 1.0;
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
That is a bad initialisation choice for a quaternion. The x,y,z components represent a scaled unit vector ( multiplied by the sine of the rotation ), but 0,0,0 cannot be scaled to a unit vector by any means.
Those are the pitch, roll and yaw (degree) that I get running the code. I copied the code entirly (I downloaded the Matlab version) and made some minor changes in reading and feeding sensors datas to the filter, but not inthe filter. Did you get something like that you too? What could it be?!?
Interestingly there are more version of the code to convert quaternion to degree. I tried 3 of them but at the end I used the one I foudn in the code of wilywampa:
Waht I don t unterstand is the major changes like -95° and 20 ms later it goes up to 165. These oscillation with this amplitude are not normal.. Somethign is wrong, but uploading a "standard" software AHRS (with DCM) in the same Arduino board with the same sensor gives right angle measures.
Did you have the same strange behavoiur?
Regards
michinyon:
That is a bad initialisation choice for a quaternion. The x,y,z components represent a scaled unit vector ( multiplied by the sine of the rotation ), but 0,0,0 cannot be scaled to a unit vector by any means.
This is wrong, and the reason why it's wrong is right in front of you: "multiplied by the sine of the rotation." When the rotation is zero, the sine is zero, and you get 1,0,0,0. Plug in zero roll, pitch, and yaw into any Euler angle to quaternion calculator and see what you get.
wilhem: That data doesn't look right. Try printing out the inputs to the filter instead of the outputs, and make sure the inputs are continuous and otherwise look as expected.
Hi
any update on this topic?
I am using its C library and facing the same issue . when my board is till then also all the 3 values i.e Heading, Pitch and Roll randomly displays all the angles at same state.I tried finding the solution but didn't get much help on it.
I coudlnt manage this problem, so I tried to implement the board with the "traditional" DCM Algorithm snd it works perfectly. So I can say, that is not a problem of my board or of the HW.
How he coudnlt run this program with his code is not clear to me.
