Hi, sorry for the late reply.. our Forum wasn’t sending us notifications of new posts.
Do you know C? it would be better to run the code in C, as you will be able to get better timed loops.
Looking at your video I can see that the loop isn’t constant, this would affect the values for the gyro.
This is one of the problems with Python. Where as C, you will be able to run perfectly time loops.
Look at applying low pass, high pass or a median filter to your raw data before calculating the angles.
I have code for a low pass filter and median below. It is being used for compass, but you can try and test it with gyro and accel values..
It is also ideal to print out your pre filtered values and then post filtered values and plot them in a graph. (i use excel) and you should be able to see the noise reduction.
BTW: are you willing to share your Python code for the Kalman filter? I would love to add it to our GIT repo.
Low Pass
float kLowPassFilterFactor = 0.1;
float kHighPassFilterFactor = 0.1;
*mag_raw = *mag_raw * kLowPassFilterFactor + oldXMagRawValue*(1 - kLowPassFilterFactor);
*(mag_raw+1) = *(mag_raw+1) * kLowPassFilterFactor + oldYMagRawValue*(1 - kLowPassFilterFactor);
*(mag_raw+2) = *(mag_raw+2) * kLowPassFilterFactor + oldZMagRawValue*(1 - kLowPassFilterFactor);
oldXMagRawValue = *mag_raw;
oldYMagRawValue = *(mag_raw+1);
oldZMagRawValue = *(mag_raw+2);
Median
#define FILTERED_LP 1
#define FILTERED_M 2
#define MEDIANTABLESIZE 3
for (f = MEDIANTABLESIZE-1; f > 0 ; f--){
medianTable1.x[f] = medianTable1.x[f-1];
medianTable1.y[f] = medianTable1.y[f-1];
medianTable1.z[f] = medianTable1.z[f-1];
}
medianTable1.x[0] = *mag_raw;
medianTable1.y[0] = *(mag_raw+1);
medianTable1.z[0] = *(mag_raw+2);
medianTable2 = medianTable1;
qsort(medianTable2.x, MEDIANTABLESIZE, sizeof(int), cmpfunc);
qsort(medianTable2.y, MEDIANTABLESIZE, sizeof(int), cmpfunc);
qsort(medianTable2.z, MEDIANTABLESIZE, sizeof(int), cmpfunc);
*mag_raw = medianTable2.x[(int)MEDIANTABLESIZE/2];
*(mag_raw+1) = medianTable2.y[(int)MEDIANTABLESIZE/2];
*(mag_raw+2) = medianTable2.z[(int)MEDIANTABLESIZE/2];Mark --OzzMaker.com --
