Gluonpilot

Yes the roll angle precision is similar, but less crucial for the moment.

The output frequency is currently 50Hz. A simpler algorithm that I will use for quadrocopters has 100Hz output, but I can easily change that to 200Hz or more.

I will post the papers and matlab code in a few day, I've been too busy flying this weekend

Thanks! It will be very interest. I have my own very noisy data from onboard IMU and it is interest to me to compare results. I havn't understood about sensors acquisition rate. Am I right that 50 Hz is output algorithm frequency, so this output angle update rate. But what are accelerometer and gyro output frequencies on algorithm input? What is bandwidth for IMU sensors with applied smoothering filters?

Hi Tom, in the comments section, there is a dt square in the P(0,0) equation, however, in the code, there isn't any dt square. do you remember why? thanks..i'm trying to understand kalman filter

Code: Select all

/*
 * The predict function. Updates 2 variables:
 * our model-state x and the 2x2 matrix P
 *     
 * x = [ angle, bias ]'
 *
 *   = F x + B u
 *
 *   = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
 *
 *   => angle = angle + dt (dotAngle - bias)
 *      bias  = bias
 *
 *
 * P = F P transpose(F) + Q
 *
 *   = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
 *
 *  P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + [b]dt²[/b] * P(1,1) + Q(0,0)
 *  P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
 *  P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
 *  P(1,1) = P(1,1) + Q(1,1)
 *
 *
 */
void ars_predict(struct Gyro1DKalman *filterdata, const float dotAngle, const float dt)
{
   filterdata->x_angle += dt * (dotAngle - filterdata->x_bias);

   filterdata->P_00 +=  - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;
   filterdata->P_01 +=  - dt * filterdata->P_11;
   filterdata->P_10 +=  - dt * filterdata->P_11;
   filterdata->P_11 +=  + filterdata->Q_gyro * dt;
}


Oh yes,
Shame on us, engineers! dt is very small (0.02 or smaller), which means that dt² is even a lot smaller: 0.0004 -> close to zero. To save on processing power, these things are omitted most of the time (it won't affect the end result anyway). Yes, that's one of the reasons why mathematicians and engineers don't always get along

ah i see...thanks for your explanation Tom ! without your implementations, many of us would be so lost out there.

sorry for double posting, how did you compensate for yaw drift in your 6dof implementation? i wouldn't think that you pligged it into kalman filter since there is no accelerometer data to correct the bias.

No yaw drift correction. You could do this with the heading from the GPS, but for now it's not really needed.
I selected the ADXRS613 gyro for yaw because it is very stable (as you can see in the video)

hi tom, for kalman filter, what does it mean by

On the downside:

* Special care needs to be taken because this algorithm uses euler angles, which are not gimbal-lock free

are strapdown systems gimbal lock-free?

What do you mean by 'strapdown systems'?

Euler angles can't handle attitude well when pitch ~ 90.

how do you determine the measurement noise covariance R or the process noise covariance Q experimentally? Is there any steps for doing that? is there any way to estimate them?