Kalman Filtering IMU Data

[Goldfinch]

Here is the if statement with 100 degree of pitch condition in ahrs_kalman_2x3.c

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.

[Tom]

Oh yes, this normalization makes sure the pitch is in [-90, +90] and roll [-180, +180]. I choose 100° instead of 90° for pitch for a reason I can't remember. I guess it was oscillating too fast between +90° and -90° in same cases.

In my flight data, roll is always good. Pitch can sometimes be a bit off. Pitch is also more important because -5° is a lot different than +5°. This is most visible during take-off. I need to do some more experiments to make sure the plane is also perfectly pitch stable on take-off. I guess tuning some parameters (relying more in gyroscopes than the accelerometers) will be good enough. I have something in ming like "when |G| > 1.3 -> reply more in the gyroscopes".

In practice, R/Q is what matters. You can divide R by 100 if you also divide Q by 100. They define how much you reply on the gyroscope (bigger R) and how much on the accelerometers. Those values are try-and-error in Matlab. I tried to calculate them, but then you only get some theoretical limit values that seem to be useless in practice.

[Goldfinch]

I kept in mind difference between roll estimated with accelerometer data accounting centripetal acceleration and roll angle directly integrated from angle rates in small time interval when angle drift is small enough. I always get great disturbanses in roll from accelerometer, but accelerometer pitch angle is better approximated to integrated pitch. I can illustrate it's behavior with figures in next time.

[Tom]

I'd need more information/code to better understand your issues

[TCMontague]

Hey Tom,
In your code, you create an array of sin values that then is used in your fast_sin:

Code: Select all

double fast_sin(double x)
{
   // -180..180 -> -60..60
   int i = (int)(x/(2.0/(180.0/3.14159))) + (180/2);
   return sin_lookup[i];
}   

I am using this on a ducted fan, and have found that the rounding to 2 degrees is causing serious problems in the settling value. It seems that indexing to every 2 degrees with no interpolation causes values to settle to 2 degree increments, more often not zero.

I know that sin(x) is an expensive function to run, but I would suggest that you implement some interpolation between values in your fast_sin function. I understand that 2 degrees may not be so bad for applications like yours, but it caused my team some troubles that had to be fixed.

[Tom]

Hmm I never thought it would cause problems... but you may be right. For my application the precision is not so critical, except when the value is around 0.

Can you share us some more information on your project, it looks interesting

[khushu]

Hi Tom,

Very interesting discussion indeed.. I just happened to find this through google...
I'm a absolutely new to kalman and IMU theory, i thought I'll join in.

This paper I thought gave me a bit more insights as I'm still trying to understand
the details of how this stuff works.

http://www.ias.ac.in/sadhana/Pdf2004Apr/Pe1161.pdf

trying to make a simulink model and try to visualize it completely...

Can you suggest me some links that help me visualize the quaternion better, and how these equations were derived, I could not visualize it well. Any help would be good.

[magellan]

Hi Tom,

Very interesting discussion indeed.. I just happened to find this through google...
I'm a absolutely new to kalman and IMU theory, i thought I'll join in.

This paper I thought gave me a bit more insights as I'm still trying to understand
the details of how this stuff works.

http://www.ias.ac.in/sadhana/Pdf2004Apr/Pe1161.pdf

trying to make a simulink model and try to visualize it completely...

Can you suggest me some links that help me visualize the quaternion better, and how these equations were derived, I could not visualize it well. Any help would be good.

Khushu,

Try the Mahoney papers, very interesting read as well:
http://gentlenav.googlecode.com/files/MahonyPapers.zip

[Tom]

I usually do the math-modeling using euler angles, and when it looks OK then I try to do the same using quaternions. I have no idea how they would look like when visualizing them

[khushu]

Thank you guys, those papers are of real help...
Would post my finding on kalman and quat...