Kalman Filtering IMU Data

Here is the place for all your dsPic or autopilot questions.

Moderator: lukasz

Re: Kalman Filtering IMU Data

Postby Tom » Wed Apr 28, 2010 7:17 am

Those constants are very important for the well functioning of you filter. Therefor I prefer to model my algorithm in Matlab (or any other software on your PC), and use real-life logged data from the sensors during a flight. Then you can start playing around with the parameters until you are satisfied with the performance.
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Kalman Filtering IMU Data

Postby pizza » Thu Apr 29, 2010 2:29 am

ah i do not have matlab, would love to monitor with other softwares, but i don't exactly know how to monitor the performance of kalman filter. do we tune R and Q, then compare the kalman angles with accelerometer angles? but we can't really trust the acclerometer all the time, so how do we judge the filter's performance?

is R = uncertainty in accelerometer measurement = covariance of measurement noise = variance of measurement noise?
R = E[(X-E[X])(X-E[X])T]
but we can't possibly measure E[X]
is R approximately the rms noise of accelerometer stated in datasheet?
does R change?
pizza
 
Posts: 39
Joined: Wed Jan 27, 2010 2:33 am

Re: Kalman Filtering IMU Data

Postby Bucho » Tue May 04, 2010 5:37 am

Tom, I looked at your firmware; do I understand it correctly, that you describe the attitude with a quaternion, and run te kalman filter with euler angles?
User avatar
Bucho
 
Posts: 5
Joined: Fri Mar 26, 2010 1:33 pm

Re: Kalman Filtering IMU Data

Postby Tom » Tue May 04, 2010 11:47 am

No,

Please take a look at the latest version:
http://code.google.com/p/gluonpilot/sou ... rtos_pilot

There are 2 versions of the attitude filter:
- ahrs.h (general definition)
- ahrs_simple_quaternion.c => implementation of ahrs.c using quaternions and a simple PID bias estimator. Much like the DCM method. Will probably be used for quadrocopters
- ahrs_kalman_2x3.c => kalman based: 2 states (pitch and roll: euler angles) and 3 measurements (3 accelerations). This one is is used for stabilizing UAV's and works very well.
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Kalman Filtering IMU Data

Postby Bucho » Sun May 09, 2010 5:01 am

Ok, I looked a bit deeper in the code. Why did you use PID-estimation vor the quaternion version and kalman filtering for the euler angle version? What are the advantages / disadvantages of both versions?
User avatar
Bucho
 
Posts: 5
Joined: Fri Mar 26, 2010 1:33 pm

Re: Kalman Filtering IMU Data

Postby Tom » Sun May 09, 2010 6:00 pm

The problem is that a quaternion has 4 variables. Put this in a matrix and it soon becomes a very large one (4x4 or bigger). For kalman filtering you need to do a matrix invertion, which is very CPU intensive.
The kalman filtered approach only calculates pitch and roll, so it's state only has 2 variables. That's why the euler angles (pitch and roll) versions uses the kalman filter (which is still better than PID), while the quaternion version does not.
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Kalman Filtering IMU Data

Postby Goldfinch » Thu Aug 05, 2010 1:11 pm

Tom, I loked into Your Kalman implementation in ahrs_kalman_2x3.c I have doubts with accelerometer model with centripetal accelerations. I think that in this line of code
Code: Select all
tmp1[2] = sensor_data.acc_z - ((sensor_data.p*w - sensor_data.q*u)/G  - cos_pitch*cos_roll);
"sensor_data.p*w" is unnecessary member. I'm I right or not? Could You please share papers and docs You followed in Your Kalman filter code?
Goldfinch
 
Posts: 12
Joined: Mon Mar 15, 2010 11:22 am
Location: Russia

Re: Kalman Filtering IMU Data

Postby Tom » Thu Aug 05, 2010 2:16 pm

The paper is attached here: viewtopic.php?f=4&t=75&p=397&hilit=paper#p397


sensor_data.p*w is not necessary (because it will only have minor influence), but it comes directly from the dynamics equations. See also http://www.gluonpilot.com/wiki/Matlab_a ... 2x3_Kalman
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Kalman Filtering IMU Data

Postby Goldfinch » Fri Aug 06, 2010 5:30 am

Tom, thanks! In Beard's paper there is
Code: Select all
az = (p*v-q*u)/G - cos_pitch*cos_roll
But side velocity component v=0. Your matrix dh-dx differs from Beard's. I tried to replay Your code in Simulink with my flight data, but can't get same results as You posted. Maybe I made some mistakes, so I will examine it. Do You trust this method or maybe want to improve it to make it more accurate?

Could You please explain what is the normalize process for pitch_rad and roll_rad in Your code? Why there is limitation of 100 degrees for pitch exactly? And what the reason of choosing the condition for KF reset (pitch = -1 and roll = -1 rad)? How often it occurs?)
Goldfinch
 
Posts: 12
Joined: Mon Mar 15, 2010 11:22 am
Location: Russia

Re: Kalman Filtering IMU Data

Postby Tom » Fri Aug 06, 2010 9:24 am

Well,
The module Beard uses has an airspeed sensor. I don't have this. But I have a very accurate pressure sensor for height. This is the one I use for the vertical velocity (this vertical velocity has only minor influence on the end result).
This method is quite accurate, I've made a lot of flights and matlab simulations with it. However, any change to improve it will be taken :-) I'm planning to add an other loop (very slow one), to adapt for gyro bias drift. The current algorithm can handle bias drift, but sometimes the calibration was made months ago or at a total different temperature. In this case the bias is a bit too different.

Normalization only takes care we use always the same domain. Eg pitch 0°, roll 180° instead of roll 0° and pitch 180° (which is the same attitude if you don' t take yaw into account).

(int)pitch == -1 is only used when pitch = NaN. I haven't found a good build-in function for NaN to do it otherwise. Normally never occurs, but you never know.

Where do you see the pitch limitation to 100°?
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

PreviousNext

Return to Firmware

Who is online

Users browsing this forum: No registered users and 4 guests

cron