To understand complementary filter before we should know nature of the sensors signals. We know the signals from accelerometer (its slow or fast movind based on Hardware you used and same way the signals from a gyroscope. But the complementary filter design here 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.

  • Combine orientation estimated from Accelerometer readings with that estimated from the Gyroscope readings

Racc – current readings from accelerometer
Rgyro – obtained from Rest(n-1) and current gyroscope readings

 

Measuring Angles: Two sensors, No reliability! Gyros Drift Approximate discrete integration Accelerometers Precise only if...

This means that at any given time the complete signal is subject to either low pass or highpass.
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?

α  = Γ / (Γ + dt)

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  from

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.

Gyroscope Drift problem [7], [6]

Due to non-ideal offset compensation of the gyroscope “gyro_x_scalled” is never equal to zero and with time “angle_x_gyro” will drift away. To solve this problem, angle is also calculated using the accelerometer and then combined with angle calculated by gyroscope. As the MPU6050 chip is placed horizontally to the ground, the z axes acceleration measures 1g (i.e. 9.81) as shown in Figure 3. We can use this acceleration vector and its projection on y and x axes to calculate the angle of x and y.

Figure 3: Projection of z-axis acceleration on x-axis tilt

And the angle calculated using accelerometer are given as

\theta_x^{accel} = \tan^{-1}\frac{accel x scalled}{\sqrt{accel y scalled^2+accel z scalled^2}}
\theta_y^{accel} = \tan^{-1}\frac{accel y scalled}{\sqrt{accel x scalled^2+accel z scalled^2}}

The problem with angle calculated using accelerometer is that they are noisy and any application which have vibrations can affect the accuracy of the calculated angle. Moreover, if the MPU6050 is moved along any axes it register acceleration and messes up the angle calculation. So to get better results, the angle calculated using gyroscope and accelerometer are combined using a simple filter.

\theta_x = Filter_gain*\theta_x^{gyro} + (1-Filter_gain)*\theta_x^{accel}

and use the filtered angle “\theta_x” in the integration of the gyroscope instead of “\theta_x^{gyro} ” to avoid the drift problem. So the equation for the gyroscope angle is changed to

\theta_x^{gyro} (t_n)  = gyro_x_scalled * T  + \theta_x(t_{n-1})

Figure 4 shows the implementation of the given equations in Arduino Sketch

Figure 4: Arduino Sketch calculating angles

Advertisements