Before reading this tutorial you should have covered Drivers 1: A new device driver and Drivers 2: A more complex device driver.

Introduction

This tutorial will explain the function and applications of virtual sensors, then demonstrate how to build a Kalman filter virtual sensor, using the OpenCV external library.

A virtual sensor is a device that doesn't correspond to a physical device. In DevBot a device is simply a collection of properties with associated functions. So a virtual sensor is a collection of properties with associated functions that is not associated with a physical device.

Virtual sensors provide prepackaged functionality. They can be pulled into programs as click together components and used with ease. For example a Kalman filter virtual sensor could be used to smooth multiple sensor readings.

The Kalman filter

The Kalman filter takes as input a stream of noisy measurements of a dynamic system. It uses these measurements to estimate the state of the system. For example, given a stream of measured locations for a mobile vehicle, the kalman filter could predict more accurate location information as well as velocity and acceleration (This would be a 2nd-order Kalman filter, as acceleration is the second derivative of position).

For the purposes of this tutorial the Kalman filter algorithm can be described as a two-step process. Firstly there is a predict step that makes use of the learned system dynamics to anticipate the next state of the system. Secondly, an update step occurs that takes a new observed measurement and uses it to correct the system dynamics and system state.

All kinds of sensor readouts from a mobile vehicle, such as range information from IR sensors and odometry information from wheel encoders, are providing streams of noisy measurements of dynamic systems, therefore Kalman filters could be used to mitigate the noise in each of these cases. Sensors could be coupled with Kalman filter virtual sensors to improve the quality of the sensor readings.

For more information on Kalman filters see wikipedia.

Utilizing OpenCV - the open source computer vision library

OpenCV is a an open source computer vision library initially developed by Intel. It is widely-used in the computer vision community and provides useful implementations of many common algorithms, including the Kalman filter. This external library is easily wrapped up in a DevBot driver as you will see in the forthcoming example. By wrapping the OpenCV functionality in the driver, any program using this driver would not require any OpenCV data types or functions.

For more information on OpenCV, see their wiki.

An example driver

In constructing a Kalman filter DevBot driver there were many decisions to be made.

Should the driver have a polling function?
How many dimensions should the Kalman Filter driver support?
Should the driver implement a 0th order, 1st order or nth order Kalman Filter?

It was clear the driver needed a poll function to cope with a regular input stream of measurements and output stream of predictions.

It was also desirable for the Kalman filter driver to be flexible in regards to the number of dimensions in the model and the order of the filter. The dimensions were assumed to be independent of each other as in most practical applications of the filter, so the driver was made 1-dimensional leaving multi-dimensional data to be modelled with multiple Kalman filter drivers.

To cope with different orders of Kalman filter, e.g. 0th order (position information only), 1st order (position and velocity information), the size of the state vector was left dynamic and the sizes of all related matrices also left dynamic. In this way the order of the Kalman filter could be set in the configuration file, or programmatically.

Private data

The polling function which calls the OpenCV Kalman filter update functions needs access to several OpenCV data structures, as well as the Measurement, Prediction and PollPeriod properties, therefore these were all included in the private data structure.

typedef struct {
    BotProperty *Measurement; /* stores current measurement */
    BotProperty *Prediction;  /* stores current prediction */
    BotProperty *Period;      /* stores the polling period */
    /* The following fields are in OpenCV formats */
    CvMat *StateCv;           /* stores the current state of the system */
    CvMat *MeasurementCv;     /* stores the current measurement */
    CvKalman *KalmanCv;       /* stores the Kalman filter data structure */
} KalmanData;

Polling function

The polling function carries out the main Kalman filter processing, calling the relevant functions in OpenCV. The first part of the function deals with constructing a transition matrix that holds the information about the dynamics of the system over time, for example relating changes in position to changes in velocity. This matrix is necessarily dependent on the period between the current poll_device call and the last one, as it is this time change which determines how much the predicted state will be modified.

static bool poll_device(BotDevice *device)
{

...

    /* Update delta_t */
    period = bot_property_get_uint(kalman->Period);
    delta_t = (double)period;

    /* Scale delta_t to seconds */
    delta_t /= (1000*1000);

    /* Constuct the new transition matrix with the new delta_t value */
    for (i = 0; i < KalmanCv->DP; i++) {
        for (j = 0; j < KalmanCv->DP; j++) {
            if (i > j)
                KalmanCv->transition_matrix->data.fl[i * KalmanCv->DP + j] = 0;
            else if (i == j) {
                KalmanCv->transition_matrix->data.fl[i * KalmanCv->DP + j] = 1;
            } else {
                KalmanCv->transition_matrix->data.fl[i * KalmanCv->DP + j] =
                    pow(delta_t, j - i) / factorial(j - i);
            }
        }
    }

The following code snippet shows the predict step of the Kalman filter and the corresponding Prediction property update.

    /* Update prediction. For an nth-order Kalman filter, the prediction has
     * n dimensions. Here we return just the 1st dimension to match the format
     * of the measurement
     */
    prediction = cvKalmanPredict(KalmanCv, 0);
    bot_property_set_real(kalman->Prediction, prediction->data.fl[0]);

Finally comes the update step of the Kalman filter which depends on whether a new measurement is available. To determine if the current value of the Measurement property has been recently updated, the time of the last update is extracted using bot_property_get_timed() and then subtracted from the current time. This gives the age of the measurement in microseconds which is then compared to the value of the Period property to determine whether the measurement was updated since the last call to poll_device().

If the measurement is recent, then it is used to update the state of the Kalman filter using cvKalmanCorrect(). Otherwise the pre-correction predicted state is copied into the post-correction predicted state, which effectively uses the blind prediction of cvKalmanPredict() as the next best guess at the state of the system.

    /* Get the time the measurement was last updated */
    meas = bot_property_get_timestamp(kalman->Measurement);
    measurement = bot_property_get_real(kalman->Measurement);
    kalman->MeasurementCv->data.fl[0] = measurement;

    /* Get the current time */
    gettimeofday(&now_tv, NULL);
    now = tv_to_usec(&now_tv);

    /* Compute the difference between the last update, and now */
    diff = now - meas;

    /* Check if measurement is new, if so update and adjust filter state */
    if (diff < period)
        cvKalmanCorrect(KalmanCv, kalman->MeasurementCv);
    else {
        /* If measurement not used, go with prediction */
        for (i = 0; i < KalmanCv->DP; i++)
            KalmanCv->state_post->data.fl[i] = KalmanCv->state_pre->data.fl[i];
    }

    return TRUE;
}

Initialization function

The initialization function first calls bot_device_property_lookup() on the properties of the device which will be used in poll_device() and stores these properties in the private data. Most drivers will follow this pattern, since bot_device_property_lookup() is relatively slow and poll_device() is performance critical.

static bool initialize_device(BotDevice * device)
{

...

    tmp = bot_device_lookup_property(device, "MP");
    tmp_MP = bot_property_get_uint(tmp);

The Kalman filter OpenCV structure is then created as follows, using the matrix dimensions already retrieved from the relevant properties.

    CvKalman *KalmanCv = cvCreateKalman(tmp_DP, tmp_MP, 0);

A slightly more complex property initialization is seen when getting the State property, which is an array of numbers (represented in configuration files as a comma delimited list). This array is retrieved with the following.

    /* Get initial state info and load into property array and OpenCV vector */
    tmp = bot_device_lookup_property(device, "InitialState");

    /* read in variable readings array */
    readings = malloc(sizeof(BOT_TYPE_REAL_TYPE) *
                    bot_property_get_length(tmp));
    bot_property_get_real_array(tmp, readings);

The resulting array is then used to initialize the OpenCV state matrix.

    /* Now update the State property and OpenCV vector */
    for (i = 0; i < KalmanCv->DP; i++)
        kalman->StateCv->data.fl[i] = readings[i];

Shutdown function

The shutdown function simply frees the memory allocated to OpenCV data structures in the initialization function, then frees the private data structure.

static void shutdown_device(BotDevice *device)
{
    /* Free OpenCV data structures */
    KalmanData *kalman = bot_device_get_data(device);
    cvReleaseKalman(&(kalman->KalmanCv));
    cvReleaseMat(&kalman->StateCv);
    cvReleaseMat(&kalman->MeasurementCv);
    /* Free private data */
    free(kalman);
}

Bringing it all together

Bring all of the above together yields kalman_opencv.c

/******************************************************************************
 *
 * Implements a driver for an nth-order one-dimensional Kalman Filter
 *
 * To deal with multi-dimensional observation data, just use multiple
 * Kalman Filters, one for each dimension. This assumes the changes in
 * each of the dimensions is independent of the changes in any other
 * dimension.
 *
 ******************************************************************************/

#include <devbot/core/type.h>
#include <devbot/core/device.h>
#include <devbot/core/driver.h>
#include <devbot/core/property.h>

#include <assert.h>
#include <stdlib.h>
#include <stdio.h>

#undef EXIT               /* Required because of EXIT redefinition in OpenCV */
#include <cv.h>           /* OpenCV header file */
#include <math.h>

/* private data struct */
typedef struct {
    BotProperty *Measurement; /* stores current measurement */
    BotProperty *Prediction;  /* stores current prediction */
    BotProperty *Period;      /* stores the polling period */
    /* The following fields are in OpenCV formats */
    CvMat *StateCv;           /* stores the current state of the system */
    CvMat *MeasurementCv;     /* stores the current measurement */
    CvKalman *KalmanCv;       /* stores the Kalman filter data structure */
} KalmanData;

static const BotPropertyProto driver_properties[] = {
    {
        .name = "MP",
        .description = "The number of measurement vector dimensions",
        .type_name = BOT_TYPE_UINT_NAME,
        .flags = BOT_NO_DEFAULT,
    },
    {
        .name = "DP",
        .description = "The number of state vector dimensions",
        .type_name = BOT_TYPE_UINT_NAME,
        .flags = BOT_NO_DEFAULT,
    },
    {
        .name = "InitialState",
        .description = "The intial state vector (x, dx/dt, d2x/dt2)",
        .type_name = BOT_TYPE_REAL_NAME,
        .length = 3,
        .flags = BOT_NO_DEFAULT,
    },
    {
        .name = "Measurement",
        .description = "The measurement vector",
        .type_name = BOT_TYPE_REAL_NAME,
    },
    {
        .name = "Prediction",
        .description = "The predicted measurement vector",
        .type_name = BOT_TYPE_REAL_NAME,
    },
    {
        .name = "ProcessNoise",
        .description = "The process noise for the process noise covariance "
                       "matrix which is assumed to be the identity matrix "
                       "multiplied by this value",
        .type_name = BOT_TYPE_REAL_NAME,
        .flags = BOT_NO_DEFAULT,
    },
    {
        .name = "MeasurementNoise",
        .description = "The measurement noise for the measurement noise "
                       "covariance matrix which is assumed to be the identity "
                       "matrix mulitplied by this value",
        .type_name = BOT_TYPE_REAL_NAME,
        .flags = BOT_NO_DEFAULT,
    },
    {
        .name = "PrioriError",
        .description = "The priori error estimate for the priori error "
                       "estimate noise covariance matrix which is assumed to "
                       "be the identity matrix mulitplied by this value",
        .type_name = BOT_TYPE_REAL_NAME,
        .flags = BOT_NO_DEFAULT,
    },
    END_PROPERTY_PROTO
};

static int factorial(int num)
{
    int i, tmp = 1;
    for (i = 2; i <= num; i++)
        tmp *= i;
    if (num < 0)
        return -1;
    return tmp;
}

static bool poll_device(BotDevice *device)
{
    KalmanData *kalman = bot_device_get_data(device);
    uint64_t period, diff, now, meas;
    double measurement, delta_t;
    const CvMat *prediction;
    struct timeval now_tv;
    int i, j;

    /* Obtain convenient pointer to Kalman filter struct */
    CvKalman *KalmanCv = kalman->KalmanCv;

    /* Update delta_t */
    period = bot_property_get_uint(kalman->Period);
    delta_t = (double)period;

    /* Scale delta_t to seconds */
    delta_t /= (1000*1000);

    /* Constuct the new transition matrix with the new delta_t value */
    for (i = 0; i < KalmanCv->DP; i++) {
        for (j = 0; j < KalmanCv->DP; j++) {
            if (i > j)
                KalmanCv->transition_matrix->data.fl[i * KalmanCv->DP + j] = 0;
            else if (i == j) {
                KalmanCv->transition_matrix->data.fl[i * KalmanCv->DP + j] = 1;
            } else {
                KalmanCv->transition_matrix->data.fl[i * KalmanCv->DP + j] =
                    pow(delta_t, j - i) / factorial(j - i);
            }
        }
    }

    /* Update prediction. For an nth-order Kalman filter, the prediction has
     * n dimensions. Here we return just the 1st dimension to match the format
     * of the measurement
     */
    prediction = cvKalmanPredict(KalmanCv, 0);
    bot_property_set_real(kalman->Prediction, prediction->data.fl[0]);

    /* Get the time the measurement was last updated */
    meas = bot_property_get_timestamp(kalman->Measurement);
    measurement = bot_property_get_real(kalman->Measurement);
    kalman->MeasurementCv->data.fl[0] = measurement;

    /* Get the current time */
    gettimeofday(&now_tv, NULL);
    now = tv_to_usec(&now_tv);

    /* Compute the difference between the last update, and now */
    diff = now - meas;

    /* Check if measurement is new, if so update and adjust filter state */
    if (diff < period)
        cvKalmanCorrect(KalmanCv, kalman->MeasurementCv);
    else {
        /* If measurement not used, go with prediction */
        for (i = 0; i < KalmanCv->DP; i++)
            KalmanCv->state_post->data.fl[i] = KalmanCv->state_pre->data.fl[i];
    }

    return TRUE;
}

static bool initialize_device(BotDevice * device)
{
    double process_noise, measurement_noise, priori_error;
    KalmanData *kalman = malloc(sizeof(KalmanData));
    uint32_t tmp_MP, tmp_DP;
    double *readings;
    BotProperty *tmp;
    int i;

    /* Get measurement and state vector dimensions to construct struct */
    tmp = bot_device_lookup_property(device, "MP");
    tmp_MP = bot_property_get_uint(tmp);
    tmp = bot_device_lookup_property(device, "DP");
    tmp_DP = bot_property_get_uint(tmp);

    /* Construct OpenCV Kalman filter struct */
    CvKalman *KalmanCv = cvCreateKalman(tmp_DP, tmp_MP, 0);

    tmp = bot_device_lookup_property(device, "ProcessNoise");
    process_noise = bot_property_get_real(tmp);
    tmp = bot_device_lookup_property(device, "MeasurementNoise");
    measurement_noise = bot_property_get_real(tmp);
    tmp = bot_device_lookup_property(device, "PrioriError");
    priori_error = bot_property_get_real(tmp);

    /* Get handles for properties to store in private data to speed up
     * poll_device
     */
    kalman->Measurement = bot_device_lookup_property(device, "Measurement");
    kalman->Prediction = bot_device_lookup_property(device, "Prediction");
    kalman->Period = bot_device_lookup_property(device, "PollPeriod");

    /* Set-up private data OpenCV matrices */
    kalman->StateCv = cvCreateMat(KalmanCv->DP, 1, CV_32FC1);
    kalman->MeasurementCv = cvCreateMat(KalmanCv->MP, 1, CV_32FC1);

    /* Init OpenCV matrices to zero */
    cvZero(kalman->MeasurementCv);

    /* Get initial state info and load into property array and OpenCV vector */
    tmp = bot_device_lookup_property(device, "InitialState");

    /* read in variable readings array */
    readings = malloc(sizeof(BOT_TYPE_REAL_TYPE) *
                    bot_property_get_length(tmp));
    bot_property_get_real_array(tmp, readings);

    /* FIXME: Hack for "InitialState" length */
    assert(KalmanCv->DP == 3);

    /* Now update the State property and OpenCV vector */
    for (i = 0; i < KalmanCv->DP; i++)
        kalman->StateCv->data.fl[i] = readings[i];
    free(readings);

    /* Set-up OpenCV Kalman filter struct */

    /* The following initialization of the measurement matrix assumes the 
     * required measurement value is the first element in the state vector
     */
    cvSetIdentity(KalmanCv->measurement_matrix, cvRealScalar(1));
    cvSetIdentity(KalmanCv->process_noise_cov, cvRealScalar(process_noise));
    cvSetIdentity(KalmanCv->measurement_noise_cov,
                  cvRealScalar(measurement_noise));
    cvSetIdentity(KalmanCv->error_cov_post, cvRealScalar(priori_error));

    /* Assign Kalman filter to the struct */
    kalman->KalmanCv = KalmanCv;
    /* Pass along the private data */
    bot_device_set_data(device, kalman);
    return TRUE;
}

static void shutdown_device(BotDevice *device)
{
    /* Free OpenCV data structures */
    KalmanData *kalman = bot_device_get_data(device);
    cvReleaseKalman(&(kalman->KalmanCv));
    cvReleaseMat(&kalman->StateCv);
    cvReleaseMat(&kalman->MeasurementCv);
    /* Free private data */
    free(kalman);
}

static const BotDriver boilerplate = {
    .name               = "kalman_opencv",
    .description        = "OpenCV based Kalman filter driver",
    .version            = "1.0",
    .initialize_device  = initialize_device,
    .shutdown_device    = shutdown_device,
    .poll_device        = poll_device,
    .driver_properties  = driver_properties
};

BOT_DRIVER_BOILERPLATE(boilerplate);

Previous tutorial

Next tutorial

Drivers 3: Virtual Sensors - Kalman filter

Contents