Gluonpilot

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.

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?

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?

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.

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?

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.

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?

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

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?)

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°?