Sensors: General Description of the tilt sensor and gyroscope

The vertical angle of the segbot was determined by using two sensors: a tilt sensor and a gyroscope.  Specifically, the tilt sensor was a Crossbow CXTLA01, which is single axis and capable of measuring ±20°, while the rate gyro was a Silicon VSG CRS02 capable of measuring ±150°/s.  Either device could be used to measure the angle, however, each of the two sensors has shortcomings where the other excels. 

Tilt sensor data sheet: (http://www.xbow.com/Products/Product_pdf_files/Tilt_pdf/6020-0014-01_B_CXTLA.pdf )

Gyroscope data sheet: (http://www.willow.co.uk/Crs02.pdf)

The tilt sensor measures an angle relative to the force of gravity.  It has excellent steady state characteristics, however, it develops significant phase lag with increasing frequency that becomes unacceptable at moderate frequencies.  The slow response of the tilt sensor is very problematic for our system because of the need for accurate dynamic as well as static measurements.

The gyroscope, on the other hand, has fantastic dynamic characteristics. A gyroscope measures angular rate and can be used to find an angle by integrating.  A problem with all gyroscopes is that there is a bias near DC that upon integrating causes an angular measurement to drift.  Left alone, this is a problem that would render an angular measurement unusable.

Our research came up with two solutions to this problem in the form of filters that take advantage of the strong points of each sensor.  The first method we tried was using a complementary filter, and the second method was to use a Kalman filter.

Complementary Filter

Recall that we like the low frequency characteristics of the tilt sensor but dislike the response at high frequencies; however, we like the high frequency characteristics of the gyroscope but dislike its steady-state performance.  The complementary filter takes the best of both worlds; it low passes the tilt sensor reading and high passes the gyroscope.  The gains on the low and high pass filters are related to a chosen frequency/time constant so that adding the two filtered signals is equated to unity.  Expressed in equation form:

           

where H_i and H_g represent the transfer functions of the tilt sensor and gyroscope, respectively, while G_i and G_g are the low pass filter to the tilt sensor and high pass filter to the gyroscope, respectively.  As outlined this paper, (http://www.geology.smu.edu/~dpa-www/robo/balance/inertial.pdf)

this is the equation that relates the sensors dynamics to the filters.  We made the assumption that the gyroscope angular measurement was obtained from an ideal integrator, which reduced the second term on the left hand side of the above equation to simply the Gyroscope high pass filter, which took the form

where t is the time constant corresponding to corner frequency of where it is desired that the sensors switch dominance of the final signal. 

We performed a series of tests to identify the tilt sensor’s transfer function.  The test setup included a motor that excited the tilt sensor with a sinusoidal input.  An encoder measured the angle which was compared to the tilt sensor for a number frequencies in which the steady state amplitude ratio and phase was used to create a Bode plot and further find a representative transfer function.  For our experiment the corner frequency was found to be at about 3.5Hz and was modeled as a single pole.  The data sheet for the tilt sensor mentioned a 6Hz bandwidth.  As in the paper we modeled the tilt sensor as single pole:

The only unknown now is the filter applied to the tilt sensor, which can be solved for to yield

                       

The expanded equation for the complementary filter then becomes

                       

Notice that the chosen corner frequency does not change the equation.  We actually tuned the value of t in the filters until we were satisfied with the response; we ended up using about 0.8s.

Finally, the filter transfer functions were discretized and then converted to C source code.

Kalman Filter

Although the complementary filter worked very well for the segbot, there was a prospect of better performance in a discrete Kalman filter, so we gave it a go.

The general purpose the Kalman filter used in our context was to combine the redundant signals of the tilt sensor and the integrated gyro reading to form one cleaner more acurate signal.  It basically works by predicting the state based on the previous state and then finding gains related to the amount of confidence that is held in each sensor relative to one another as well as the amount of confidence held in the state estimate.  The state estimate is updated based on the measurement error of the previous state further weighted by the gains just found.  The entire procedure repeats for the next time step to create an excellent dynamic filter.

An excellent source for the Kalman filter is http://www.cs.unc.edu/~welch/kalman/ ,  which includes links to many Kalman filter related items as well as this paper http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf  that was very useful in actually implementing the Kalman filter.

I will follow with an explanation of how we implemented the Kalman filter in our system specifically.

From the paper linked above by Welch and Bishop, 2004, the discrete Kalman filter can be reduced to time update and measurement update equations which cyclically update by using the most recently available information from other set, specifically:

Time update equations:

Measurement update equations:

where the noiseless state space representation of the discrete system is

                       

                       

P is the error covariance matrix defined by the expected value of the error, Q is the process noise covariance, R the measurement error covariance, and K is the Kalman gain matrix.

We defined a two-dimensional state with tilt angle as x₁, and the gyroscope bias that causes the drift as x₂.  It follows

and                  

The error covariance matrix, P, will converge, thus for the purpose of initializing it to implement it in code we can choose an initial 2x2 P matrix, we decided on the identity matrix I.  But we use a generic P_k-1 matrix and a Q matrix with Q_a and Q_g as diagonal entries in order to solve for the time update equation. Q_a and Q_g are constants that will be tuned later, the ratio of the two represent how much we will rely on one sensor compared to the other.  The equation for our system become:

                       

which reduces to

                       

where the entries P_e are chosen for notational convenience.  The Kalman gains for our system become

                       

The state update with the value of the angle we will use for the control becomes

                       

The updated covariance matrix becomes

                       

Tilt Sensor/Gyroscope Filter Conclusion

Both the complementary and the Kalman filters on the segbot achieved satisfactory results.  However, the Kalman filter seemed to have more flexibility and an equivalent if not a better response.  It was simple to adjust the constants R, Q_a, and Q_g in order to weight the estimations and relative influence of the sensors.  The tilt sensor and the gyroscope used were both relatively expense and this consideration was also looked into by designing a board that uses an accelerometer and a cheaper gyroscope.  Although a version of the cheaper sensor was compiled it has yet to be tested.  Filtering under the conditions of much cheaper sensors may prove one filter more superior to the other.