[Robots Home] | [Robots, Servo Info] | [A Robot Project] | [Robot Program]

[Robots Sensor] | [My First Robot] | [Other Robots] | [Simulators]| [Robot Control]

Kalman Filtering

Combining data from multiple sensors

Suppose you have two sensors, possibly different types, that
measure the same value. e.g. range to closest obstacle.

Kalman filtering of the measured values produces a better estimate
of the correct value than either sensor alone. We assume the sensors
are of different types and thus have different random error
properties. The sensors must first be analyzed to determine the
statistical standard deviation of the random error. We will actually
compute the standard deviation squared that is the variance.

Call the sensors sensor_1 and sensor_2.

Make N measurements with sensor_1 and record the values. x_1,...,x_N
without moving the sensor or target. Compute

    mean_1     = sum(x_1,...,x_N) / N
    variance_1 = sum( (x_1 - mean_1)^2,...,(x_N - mean_1)^2) / N

These are the mean value and variance for sensor_1.

Repeat for sensor_2. The N may be different for sensor_2. Choose N
large enough to get a good sample. 10 is probably too small, 100 may
be too large.

The information that must be stored for use during operation is
variance_1 and variance_2.

Now in operation, at some time sensor_1 gives a value z_1 and
sensor_2 gives a value z_2. Compute the best estimated value z
and the variance of z

    K(t)        = variance_1 / (variance_1 + variance_2)
    z(t)        = z_1 + K(t) * (z_2 - z_1)
    variance(t) = variance_1 - K(t) * variance_1

In general, when another measurement, z_1, is made by sensor_1

    K(t+1)        = variance(t) / (variance(t) - variance_2)
    z(t+1)        = z(t) + K(t+1) * (z_1 - z(t) )
    variance(t+1) = variance(t) - K(t+1) * variance(t) 

Or, symmetrically, when another measurement, z_2, is made by sensor_2

    K(t+1)        = variance(t) / (variance(t) - variance_1)
    z(t+1)        = z(t) + K(t+1) * (z_2 - z(t) )
    variance(t+1) = variance(t) - K(t+1) * variance(t)

Any number of sensors making the same measurement may be
combined by repeated application of the three computations.

For the Kalman filter to be optimal, the errors for both sensors
must be white noise, no correlation from measurement to measurement
and no cross correlation. Both sensors must be linear and implicitly
calibrated to give the same mean measurement at some distance.

Reference: "Stochastic models, estimation, and control" by Maybeck
Acedemic Press, 1979, pp 12-13

Last updated 4/20/05