Idea behind complementary filter is to take slow moving signals from accelerometer and fast moving signals from a gyroscope and combine them.Accelerometer gives a good indicator of orientation in static conditions. Gyroscope gives a good indicator of tilt in dynamic conditions. So the idea is to pass the accelerometer signals through a low-pass filter and the gyroscope signals through a high-pass filter and combine them to give the final rate. The key-point here is that the frequency response of the low-pass and high-pass filters add up to 1 at all frequencies. This means that at any given time the complete signal is subject to either low pass or high pass. Equation for low-pass filter: y[n]=(1-alpha).x[n]+alpha.y[n-1] //use this for angles obtained from accelerometers x[n] is the pitch/roll/yaw that you get from the accelerometer y[n] is the filtered final pitch/roll/yaw which you must feed into the next phase of your program Equation for high-pass filter: y[n]=(1-alpha)y[n-1]+(1-alpha)(x[n]-x[n-1]) //use this for angles obtained from gyroscopes x[n] is the pitch/roll/yaw that you get from the gyroscope y[n] is the filtered final pitch/roll/yaw which you must feed into the next phase of your program n is the current sample indicator. alpha is related to time-constant. it defines the boundary where the accelerometer readings stop and the gyroscope readings take over and vice-versa. It controls how much you want the output to depend on the current value or a new value that arrives. Both the alpha's have to be the same. alpha is usually > 0.5 using the definitions above. How to choose alpha?alpha=(tau)/(tau+dt) where tau is the desired time constant (how fast you want the readings to respond) and dt = 1/fs where fs is your sampling frequency. This equation is derived from filter/control theory will put a link to this as soon as I get it. A quick and dirty way of implementing a complementary filter: angle = (1-alpha)*(angle + gyro * dt) + (alpha)*(acc) First reading is the angle as obtained from gyroscope integration. Second reading is the one from accelerometer. In the case that gyro = 0, angle will converge to that given by accelerometer. If alpha is really small the output angle will not believe the reading from the accelerometer so readily and will believe the gyroscope as and when it happens. For some good links:Check out the attachment filter.pdf for an excellent introduction to complementary filter. |

Filters >