kalman.c

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

static char *rcsid = "$Id: kalman.c,v 1.12 1996/12/17 22:14:51 kint Exp $";

/**
 ** kalman.c
 **
 ** Implements a kalman filter to estimate speed from volume and occupancy
 ** values obtained from single-loop sensors.  This code is based on the 
 ** paper "A Statistical Algorithm for Estimating Speed from Single Loop
 ** Volume and Occupancy Measurements" by D. J. Dailey.
 **
 ** public functions:
 **
 **   kalman_filter
 **
 **     Handles the state machine and calls kalman_speed to perform
 **     the actual calculations.  Checks the returned length against
 **     constraints to determine if the speed estimate is deemed robust.
 **     Restarts filter for a sensor if length goes negative.
 **
 **   kalman_initialize
 **
 **     Sets a kalman struct to an initial state.
 **     This would be in the constructor in C++.
 **
 **   accessor functions
 **
 **      kalman_get_a
 **      kalman_set_a
 **      kalman_get_b
 **      kalman_set_b
 **      kalman_get_l
 **      kalman_set_l
 **      kalman_get_sigma_no
 **      kalman_set_sigma_no
 **      kalman_get_sigma_s
 **      kalman_set_sigma_s
 **      kalman_get_initial_speed
 **      kalman_set_initial_speed
 **
 ** private functions:
 **
 **   These are not intended to be called by application functions and
 **   are declared static to this file.  They would be private in C++.
 **
 **   kalman_speed
 **
 **     performs the actual calculations.  This is translated from Dan
 **     Dailey's original Matlab code with minimal modifications.  This
 **     updates the kalman struct, performs calculations, and returns
 **     a computed length value which may be used to check the usability
 **     of the speed estimate.
 **
 **   kalman_load
 **
 **     loads new volume and occupancy, shifts old values
 **     within state matrices.
 **
 **   kalman_ok
 **
 **     Utility function which performs sanity check on volume and occupancy.
 **     Volume must be > 0, occupancy must be in range 0 < occupancy <= 100.
 **     Returns TRUE if data passes these tests, FALSE otherwise.
 **
 **   kalman_restart
 **
 **     Sets kalman state variables (Xk, Xk0, Pk, Pk0, vol, occ) to
 **     0 or 1, sets state to ks_initial.  Called by kalman_initialize().
 **
 ** Note: the structure of kalman_speed() is dictated largely by the need to
 ** keep the C implementation in line with the Matlab prototype.
 **/

#include "kalman_p.h"
#include "kalman.h"



/*******************************************************************

  kalman_initialize

  initializes kalman struct, should be first thing called after
  struct is allocated.  This would be the constructor in C++.

  initializes a, b, l, sigma_s, sigma_no to DEFAULT_ values (see kalman.h)

  initializes initial_speed to DEFAULT_SPEED, this value will be loaded into
  Xk and Xk0 whenever the filter is restarted.

  calls kalman_restart() to set vol, occ, Xk, Xk0, Pk, Pk0

  Parameter: k (kalman_t *): kalman struct to initialize

  Note:  silently returns if k is null

  Note2: set_ accessors (kalman_set_a, etc) should be called AFTER
         this function

*******************************************************************/

void kalman_initialize(kalman_t *k)
{
    if (!k)
      return;

    kalman_set_a(k, DEFAULT_A);
    kalman_set_b(k, DEFAULT_B);
    kalman_set_l(k, DEFAULT_L);
    kalman_set_sigma_s(k, DEFAULT_SIGMA_S);
    kalman_set_sigma_no(k, DEFAULT_SIGMA_NO);

    kalman_set_initial_speed(k, DEFAULT_SPEED);

    kalman_restart(k);
}


/*******************************************************************

  kalman_restarts Kalman filter by resetting parameters

  state set to ks_initial
  vol and occ set to 1.0
  Xk and Xk0 set to initial_speed
  Pk and Pk0 set to identity matrix of sigma_s ** 2

  Parameter: k (kalman_t *): kalman struct being restarted

  Note: silently returns if k is null

*******************************************************************/


static void kalman_restart(kalman_t *k)
{
    double sigma_s_squared;

    if (!k)
      return;

    k->state = ks_initial;

    k->Xk0[0] = k->Xk0[1] = k->initial_speed;
    k->Xk[0] = k->Xk[1] = k->initial_speed;

    k->vol[0] = k->vol[1] = 1.0;
    k->occ[0] = k->occ[1] = 1.0;

    sigma_s_squared = k->sigma_s * k->sigma_s;

    k->Pk0[0][0] = k->Pk0[1][1] = sigma_s_squared;
    k->Pk0[0][1] = k->Pk0[1][0] = 0.0;

    k->Pk[0][0] = k->Pk[1][1] = sigma_s_squared;
    k->Pk[0][1] = k->Pk[1][0] = 0.0;
}

/*******************************************************************

  kalman_ok

  tests volume and occupancy against constraints:

  volume > 0
  occupancy > 0 and occupancy < 100.0

  Parameters:  vol (int):    volume in vehicles/hour
               occ (double): occupancy as percent (0-100)

  Returns: TRUE if tests passed, FALSE otherwise

*******************************************************************/
    
static int kalman_ok(int vol, double occ)
{
    if ((vol > 0) && ((occ > 0.0) && (occ < 100.0)))
      return TRUE;

    return FALSE;
}

/*******************************************************************

  kalman_load

  shifts values for next iteration of filter:
   previous vol, occ go into vol[1] and occ[1]
   Xk0 gets values of Xk (old Xk becomes input to next iteration)
   Pk0 gets values of Pk (old Pk becomes input to next iteration)

  loads new values for vol, occ

  Parameters: k (kalman_t *): kalman struct being loaded
              vol (int):      volume in vehicles/hour
	      occ (double):   occupancy as percentage (0-100)

  Note: silently returns if k is null

*******************************************************************/

static void kalman_load(kalman_t *k, int vol, double occ)
{
    if (!k)
      return;

    /* previous vol, occ values go into vol[1], occ[1] */

    k->vol[1] = k->vol[0];	
    k->occ[1] = k->occ[0];

    /* load new volume and occupancy */

    k->vol[0] = (double) vol;
    k->occ[0] = occ;

    /* Xk (result of previous iteration) becomes Xk0 for input to
       next iteration */

    k->Xk0[0] = k->Xk[0];
    k->Xk0[1] = k->Xk[1];

    /* Pk (result of previous iteration) becomes Pk0 for input to
       next iteration */

    k->Pk0[0][0] = k->Pk[0][0];
    k->Pk0[0][1] = k->Pk[0][1];
    k->Pk0[1][0] = k->Pk[1][0];
    k->Pk0[1][1] = k->Pk[1][1];
}

/*******************************************************************

  kalman_filter

  front end to actual filter function kalman_speed()

  Checks current filter state, performs appropriate actions based on
  this state.  The filter must be loaded with two consecutive good
  observations before it is usable (state ks_running).  Once in
  ks_running, the filter is restarted if the length goes negative.

  If state is ks_running, checks the input values and loads either the
  values or zeroes.

  Parameters: k (kalman_t *):  kalman struct being calculated
              vol (int):       volume in vehicles/hour
	      occ (double):    occupancy as percentage (0-100)
	      speed (double):  output parameter, contains speed estimate
	                       from Kalman filter.  A value of -1.0 means
			       the speed was not calculated, or that
			       the filter was restarted because the
			       length went negative.
	      length (double): output parameter, contains length estimate
	                       from Kalman filter.  A value of 0.0 means
			       that length was not calculated.  If this
			       goes negative, the filter is restarted.

  Returns: TRUE if speed is usable, FALSE if it is not usable.
           The speed may be unusable because:
	      (a) the filter is not yet initialized
	      (b) the filter was restarted
	      (c) the length estimate is not within constraints
	      (d) the filter received null data on this or the previous
	          iteration

  Output values indicate the following conditions:

        RETURN  SPEED   LENGTH
          T       N       N     Speed usable
          F       N      > 0    Length not between constraints (15<l<40),
                                speed not usable
          F       N       0     Null/bad input data received on this
                                or previous iteration, speed not usable
          F      -1       0     Filter not yet started (waiting for
                                two consecutive good data points), or
				kalman_t pointer k was null
          F      -1      < 0    Length went negative, filter restarted


  Note: if k is null, returns FALSE, sets speed to -1.0 and length to 0.0

*******************************************************************/

int kalman_filter(kalman_t *k, int vol, double occ, double *speed,
		  double *length)
{
    /* check for null kalman_t pointer */

    if (!k)
    {
	*speed = -1.0;
	*length = 0.0;
	return FALSE;
    }

    /* perform appropriate actions based on filter state */

    switch (k->state)
    {
	/* ks_initial means no data has been loaded
	   if state == ks_initial
	      if the data is good
	         load the data
		 set state to ks_first
	      return unset/unusable */
		
      case ks_initial:
	if (kalman_ok(vol, occ))
	{
	    kalman_load(k, vol, occ);
	    k->state = ks_first;
	}
	*speed = -1.0;
	*length = 0.0;
	return FALSE;
	break;

	/* ks_first means the first data point has been loaded
	   if state == ks_first
	      if the data is good
	         load the data
		 run the filter
		 set state to ks_running
	      else restart filter (set state to ks_initial)
              return unset/unusable  */

      case ks_first:
	if (kalman_ok(vol, occ))
	{
	    kalman_load(k, vol, occ);
	    k->state = ks_running;
 	    *length = kalman_speed(k);
	}
	else
	  kalman_restart(k);

	*speed = -1.0;
	*length = 0.0;
	return FALSE;
	break;

      case ks_running:
	/* if the data is good, load it, otherwise load zeroes.
	   call the filter
	   if lbar below 0
	      restart filter (set state to ks_initial)
	      set speed to -1.0 (not calculated)
	      leave length alone so calling function can use it
	      return false
	   else if lbar is within constraints
	      leave speed, length alone
	      return TRUE
	   else (lbar not within constraints)
	      leave speed, length alone
	      return FALSE */

	/* if data good, load it, else load zeroes */

	if (kalman_ok(vol, occ))
	  kalman_load(k, vol, occ);
	else
	  kalman_load(k, 0, 0.0);
 
	*length = kalman_speed(k);

	if (*length < 0.0)
	{
	    kalman_restart(k);
	    *speed = -1.0;
	    return FALSE;
	}