Print PDF
Favorite
Email
Step 10A very flattering filter.
To combine the two sensor readings together in a way that produces the best angle estimate, a special technique called a complementary filter is used. (No, not a complimentary filter, and definitely not a Kalman filter.) The filter is really just a single line of code:
angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN;
Okay, so it's a long line. You can also do it with an op-amp or two.
This filter does exactly what is necessary in this scenario: it favors the gyroscope reading for short time durations and the accelerometer average reading for long time durations. Let's break it down:
A is the factor that determines the cutoff time for trusting the gyro and filtering in the accelerometer. It's always between 0 and 1, usually close to 1. In this case, A is defined to be 0.962. This means that at every time step, 96.2% of the new angle measurement comes from the old angle measurement plus the integrated gyro measurement. The remaining 3.8% comes from the accelerometer. This slowly averages in the accelerometer over many time steps.
DT is the time in seconds between program loops, the time step. Here it is defined to be 0.020 and is set by delay(20) at the end of the loop. The code in the loop itself take much less than 20ms, so the delay dominates the time step.
rate is the gyro reading, converted to degrees per second.
accel_raw*A_GAIN is the accelerometer reading, converted to degrees. It is very important that these two be in the same unit base before adding together. (You can't add apples to oranges.)
The time constant of the filter is the duration of time at which the gyroscope reading starts to be filtered out heavily and the accelerometer reading starts to be averaged in heavily. It's actually a continuous process, but the time constant is a single measure of where the balance begins to shift. The time constant is:
tau = DT*(A)/(1-A) = 0.5s
So, for this filter, the gyro is trusted for about 0.5 seconds and the accelerometer start to average in significantly after that. This value can be tweaked by changing A.
angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN;
Okay, so it's a long line. You can also do it with an op-amp or two.
This filter does exactly what is necessary in this scenario: it favors the gyroscope reading for short time durations and the accelerometer average reading for long time durations. Let's break it down:
A is the factor that determines the cutoff time for trusting the gyro and filtering in the accelerometer. It's always between 0 and 1, usually close to 1. In this case, A is defined to be 0.962. This means that at every time step, 96.2% of the new angle measurement comes from the old angle measurement plus the integrated gyro measurement. The remaining 3.8% comes from the accelerometer. This slowly averages in the accelerometer over many time steps.
DT is the time in seconds between program loops, the time step. Here it is defined to be 0.020 and is set by delay(20) at the end of the loop. The code in the loop itself take much less than 20ms, so the delay dominates the time step.
rate is the gyro reading, converted to degrees per second.
accel_raw*A_GAIN is the accelerometer reading, converted to degrees. It is very important that these two be in the same unit base before adding together. (You can't add apples to oranges.)
The time constant of the filter is the duration of time at which the gyroscope reading starts to be filtered out heavily and the accelerometer reading starts to be averaged in heavily. It's actually a continuous process, but the time constant is a single measure of where the balance begins to shift. The time constant is:
tau = DT*(A)/(1-A) = 0.5s
So, for this filter, the gyro is trusted for about 0.5 seconds and the accelerometer start to average in significantly after that. This value can be tweaked by changing A.
| « Previous Step | Download PDFView All Steps | Next Step » |
 |
Add Comment
|
