**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_{1},
and the gyroscope bias that causes the drift as x_{2}.
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.

Future plans of the segbot include replacing the current tilt sensor
and gyro with cheaper and smaller components. These parts include
the ADXL350 Accelerometer and ADXRS300 Gyro from Analog Devices.
These parts are a fraction of the cost of the current sensors with
similar performance and accuracy.

We designed a new board with both the accelerometer and gyro onboard.
The accelerometer's 37mV/G response was too low for our purpose,
so we amplified the signal with an instrumental amp then filtered
it through an RC filter to smooth the signal.