- Code: Select all
#define normalize(pitch, roll) \
if (pitch > 100.0/180.0*3.14159) \
{ \
pitch = pitch - 3.14159/2.0; \
roll = roll + 3.14159; \
} \
if (pitch < -100.0/180.0*3.14159) \
{ \
pitch = pitch + 3.14159/2.0; \
roll = roll + 3.14159; \
} \
if (roll > 3.14159) \
roll = roll - 3.14159; \
if (roll < -3.14159) \
roll = roll + 3.14159; \
Generally, I think that u-speed component is more significant for roll estimation by accelerometers while pitch is good enough without w-speed. But both of roll and pitch angles estimated by KF are disturbed after agressive maneuvers during take-off operation (high angle amplitudes) in my tests. I think to make variable degree of belief for accelerometer angle for different flight stages (take-off, altitude stable flight, landing). Pitch always more reliable than roll after KF with my flight data.
And I don't understand what is phisical meaning for matrix R whith diagonal elements [25, 25, 30]. How it can be correlated with noise RMS of sensors? When I added for 5 points to each value KF becomes unstable.