kalman.h

This package implements a Kalman filter as described in the paper "A Statistical Algorithm for Esti

/* $Id: kalman.h,v 1.8 1996/12/17 22:12:15 kint Exp $ */

#ifndef _KALMAN_H_
#define _KALMAN_H_

/**
 ** kalman.h
 **
 ** Data structures, defines, and public function prototypes for a
 ** Kalman filter which estimates traffic speed from volume and occupancy
 ** values obtained from single-loop sensors.  This filter is described 
 ** paper "A Statistical Algorithm for Estimating Speed from Single Loop
 ** Volume and Occupancy Measurements" by D. J. Dailey.
 **
 ** Enumerated Type
 **
 **   kalman_state
 **
 **     describes the state of the Kalman filter for a given sensor:
 **       ks_initial:  no data has been loaded
 **       ks_first:    first data sample has been loaded
 **       ks_running:  filter is running
 **
 **     The filter requires two consecutive valid data points to start.
 **     Loading the first data point moves the filter from ks_initial
 **     to ks_first.  If the next sample contains valid data, the filter
 **     moves from ks_first to ks_running, otherwise it goes back to
 **     ks_initial.  Once the filter is running, it remains in that state
 **     until the returned length goes negative (which moves the filter
 **     to ks_initial).
 **      
 ** Data Structures
 **
 **   kalman_t
 **
 **     contains all state and parameters for filter.
 **/

/* Enumerated Type
 *
 * possible filter states:
 * ks_initial: no data loaded, filter not running
 * ks_first:   one data point loaded, one more needed
 * ks_running: filter is running
 */

enum kalman_states { ks_initial, ks_first, ks_running };

/* Data Structure
 *
 * kalman_t contains state and parameters for each sensor
 */

typedef struct
{
    /* fields changed after each iteration */

    double Pk[2][2];   /* updated error covariance */
    double Xk[2];      /* updated speed estimates */

    double occ[2];     /* occupancy, 0-100%: 0 is current, 1 is prev */
    double vol[2];     /* volume, veh/hr: 0 is current, 1 is prev */

    double Pk0[2][2];  /* previous error covariance */
    double Xk0[2];     /* previous speed estimates */

    /* filter state */

    enum kalman_states state;

    /* fields typically not changed, but which are in this struct
       because they may differ for each sensor.  These are Kalman
       filter parameters which are derived experimentally */

    double a;          		/* ar[2] coefficients */
    double b;                   /*  " */
    double sigma_s;             /* variability of speed */
    double sigma_no;            /* std dev of occ/vol ratio */

    /* mean length of vehicles at sensor.  Derived experimentally */

    double l;	                /* mean length */

    /* initial speed estimate, used when filter is started.  The default
       is the speed limit */

    double initial_speed;       /* initial speed estimate */
} kalman_t;

/* Defines
 *
 * default parameters
 */

#define DEFAULT_L 22.0			/* mean length */
#define DEFAULT_SIGMA_S 14		/* variability of speed */
#define DEFAULT_SIGMA_NO  0.00078	/* std dev of occ/vol ratio */
#define DEFAULT_A 0.7837		/* ar[2] coefficients */
#define DEFAULT_B 0.2104		/*  experimentally derived using
					    least-squares forward/backward */
#define DEFAULT_SPEED  60.0             /* initial value for speed estimate,
					   default is speed limit */

/* length thresholds, if a length is not between them
 * then the corresponding speed estimate is deemed unusable.
 * Cf page 10, para 2 of the paper.
 */

#define DEFAULT_LENGTH_MIN  15.0
#define DEFAULT_LENGTH_MAX  40.0

/* normalization constant used in kalman_speed().
 * this converts between different units as follows:
 *
 * fac = 5280ft/mile * 1hr/3600 seconds * 180 samples/hour
 *       * 1/100(occ percent) * 20 seconds/sample
 *
 * This works out to 5280*180*20 / (3600*100), or 5280/100.
 * This assumes that the samples occur every 20 seconds, this will need
 * to be changed for different sample periods.
 */

#define KALMAN_NORMALIZATION    52.8

/*
 * TRUE and FALSE, only if not already defined
 */

#ifndef TRUE
#define TRUE  1
#endif

#ifndef FALSE
#define FALSE 0
#endif


/* Public Function Prototypes */

/* wrapper around actual filter, this is function to call from app.
 * It expects volume in vehicles/hr and occupancy as percent (0-100).
 * It returns validity flag, speed estimate in MPH and length estimate
 * in feet
 */

int kalman_filter(kalman_t *k, int vol, double occ, double *speed,
		  double *length);

/* initializes filter parameters, called after allocation */
void kalman_initialize(kalman_t *k);

/* accessor functions.  These should be called after kalman_initialize() */

void kalman_set_a(kalman_t *k, double a);
void kalman_set_b(kalman_t *k, double b);
void kalman_set_l(kalman_t *k, double l);
void kalman_set_sigma_no(kalman_t *k, double sigma_no);
void kalman_set_sigma_s(kalman_t *k, double sigma_s);
void kalman_set_initial_speed(kalman_t *k, double initial_speed);

double kalman_get_a(kalman_t *k);
double kalman_get_b(kalman_t *k);
double kalman_get_l(kalman_t *k);
double kalman_get_sigma_no(kalman_t *k);
double kalman_get_sigma_s(kalman_t *k);
double kalman_get_initial_speed(kalman_t *k);

#endif  /* _KALMAN_H_ */