kalman.c

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

	*speed = k->Xk[0];

	/* check against length constraints */

	if ((*length < DEFAULT_LENGTH_MIN) || (*length > DEFAULT_LENGTH_MAX))
	  return FALSE;

	return TRUE;
	break;
 
      default:
	/* this statement should not be reached, k->state has only three
	   legal values */
	break;
   }
}

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

  kalman_speed

  estimate speed and length using Kalman filter.
 
  This code is a representation in C of the matrix algebra contained
  in the paper "A Statistical Algorithm for Estimating Speed from
  Single Loop Volume and Occupancy Measurements" by D. J. Dailey.
  A PostScript version of the paper is provided with this code.

  Parameters: k (kalman_t *):  kalman struct for loop being estimated

  Returns:  (double): length estimate from filter.
                      This will be 0.0 if vol/occ is null on this
		      or previous iteration

  Speed estimate will be placed in k->Xk[0].

  Note: this function based closely on Matlab code with as little
        retyping as possible to avoid introducing errors.

*******************************************************************/
 
static double kalman_speed(kalman_t *k)
{
    /* local copies of values from kalman struct, they are explained
       in kalman.h */

    double a;
    double b;
    double l;
    double sigma_no;
    double sigma_s;
    const double fac = KALMAN_NORMALIZATION;

    /* variables taken from Matlab code which implements the Kalman filter
       algebraically.  Read the paper for further explanation */

    double x[2];
    double cs2, s12, s13, s14, s22, s23, s24;
    double H[2][2];   /* observation function H */
    double R[2][2];   /* observation correlation matrix */
    double Q[2][2];   /* system correlation matrix */

    double hpx[2];

    double d, d2;
    double s2, s3, s32, cs22;
    double alpha, beta;
    double zk[2];

    double sigma_o2, sigma_s2;
    double cp, dp, ep, fp;
    double g, h, i, j;
    double det_hph;
    double p, q, r,s;

    /* optimization variables, calcluate commonly-used squares once  */
    double alpha_squared, beta_squared, sigma_s_squared;

    /* added to make code more readable at end of function*/
    double Xk_squared, Xk_cubed;

    /* length estimate, will be return value */
    double lbar;

    a        = k->a;
    b        = k->b;
    l        = k->l;               /* length, from kalman struct */
    sigma_no = k->sigma_no;
    sigma_s  = k->sigma_s;

    sigma_s_squared = sigma_s * sigma_s; /* optimization, sigma_s **2
					    is used a lot */

    /* Observation function H */

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

    cs2 = sigma_s_squared;

    s12 = x[0] * x[0];
    s13 = s12 * x[0];
    s14 = s12 * s12;

    s22 = x[1] * x[1];
    s23 = s22 * x[1];
    s24 = s22 * s22;

    H[0][0]= -(3*(cs2+s12)/s14)*l/fac;
    H[0][1]=0.0;
    H[1][0]=0.0;
    H[1][1]= -(3*(cs2+s22)/s24)*l/fac;

    /* Linearized measurement variables */

    alpha = H[0][0];
    alpha_squared = alpha * alpha;
    
    beta  = H[1][1];
    beta_squared = beta * beta;

    /* Observation Correlation Matrix */

    R[0][0] = R[1][1] = sigma_no * sigma_no;
    R[0][1] = R[1][0] = 0.0;

    /* System Correlation Matrix */

    Q[0][0] = Q[1][1] = sigma_s_squared;
    Q[1][0] = Q[0][1] = 0.0;

    /* Initialize the measurement and observer variance and
       observation equation */
    
    sigma_o2 = R[1][1];
    sigma_s2 = sigma_s_squared;

    /* handle null data */

    if ((k->vol[0] == 0.0) || (k->vol[1] == 0.0) ||
	(k->occ[0] == 0.0) || (k->occ[0] == 0.0))
    {
	cp = k->Pk0[0][0];
	dp = k->Pk0[1][0];
	ep = k->Pk0[0][1];
	fp = k->Pk0[1][1];

	g = a*(a*cp + b*dp) + b*(a*ep + b*fp) + sigma_s2;
	h = a*cp + b*ep;
	i = a*cp + b*dp;
	j = cp + sigma_s2;

	det_hph = (g*alpha_squared + sigma_o2)
	  *(j*beta_squared + sigma_o2)
	    - h*i*alpha_squared * beta_squared;

	p = (1/det_hph) *(alpha*(g*(j*beta_squared + sigma_o2)
				 - h*i*beta_squared));

	q = (1/det_hph) *(beta*h*sigma_o2);

	r = (1/det_hph) *alpha*i*sigma_o2;

	s = (1/det_hph) *beta*(j*(g*alpha_squared+sigma_o2)
			       - h*i*alpha_squared);

	/* fill in the output parameters */

	k->Pk[0][0] = g-p*g*alpha-q*i*beta;
	k->Pk[1][0] = h-h*p*alpha-q*j*beta;
	k->Pk[0][1] = i-r*g*alpha-s*i*beta;
	k->Pk[1][1] = j-h*r*alpha-s*j*beta;

	k->Xk[0] = a*(k->Xk0[0]) + b*(k->Xk0[1]);
	k->Xk[1] = k->Xk0[1];

	return 0.0;
    }
    
    /* Measurement Function h */

    s2 = x[0] * x[0];
    s3 = s2 * x[0];

    cs2 = sigma_s_squared;
    d=s3/(s2+cs2);

    s22= x[1] * x[1];
    s32= s22 * x[1];

    cs22 = sigma_s_squared;
    d2=s32/(s22+cs22);
 
    hpx[0] = (l/fac)/d;
    hpx[1] = (l/fac)/d2;
    
    zk[0] = (k->occ[0]) / (k->vol[0]);
    zk[1] = (k->occ[1]) / (k->vol[1]);
    
    zk[0] = zk[0] - hpx[0] + alpha*x[0];
    zk[1] = zk[1] - hpx[1] + beta*x[1];

    /* calculate the Kalman filter result */

    /* note that Pk[1][0] is Pk(1,2) in Matlab: C is zero-based while
       Matlab is one-based, C is row-major while Matlab is column-major */

    cp = k->Pk0[0][0];
    dp = k->Pk0[1][0];
    ep = k->Pk0[0][1];
    fp = k->Pk0[1][1];

    g = a*(a*cp + b*dp) + b*(a*ep + b*fp) + sigma_s2;
    h = a*cp + b*ep;
    i = a*cp + b*dp;
    j = cp + sigma_s2;

    det_hph = (g*alpha_squared + sigma_o2)
      *(j*beta_squared + sigma_o2)
      - h*i*alpha_squared * beta_squared;

    p = (1/det_hph) *(alpha*(g*(j*beta_squared + sigma_o2)
			     - h*i*beta_squared));

    q = (1/det_hph) *(beta*h*sigma_o2);

    r = (1/det_hph) *alpha*i*sigma_o2;

    s = (1/det_hph) *beta*(j*(g*alpha_squared+sigma_o2)
			   - h*i*alpha_squared);

    /* fill in the output parameters */

    k->Pk[0][0] = g-p*g*alpha-q*i*beta;
    k->Pk[1][0] = h-h*p*alpha-q*j*beta;
    k->Pk[0][1] = i-r*g*alpha-s*i*beta;
    k->Pk[1][1] = j-h*r*alpha-s*j*beta;

    k->Xk[0] = a*(k->Xk0[0]) + b*(k->Xk0[1]) +
      p*(zk[0] - alpha*(a*(k->Xk0[0]) + b*(k->Xk0[1])))
	+ q*(zk[1] - beta*(k->Xk0[0]));

    k->Xk[1] = k->Xk0[0] +
      r*(zk[0] -alpha*(a*(k->Xk0[0]) + b*(k->Xk0[1])))
	+ s*(zk[1] - beta*(k->Xk0[0]));

    Xk_squared = k->Xk[0] * k->Xk[0];
    Xk_cubed = Xk_squared * k->Xk[0];

    /* estimate length, return it */

    lbar = (fac*(k->occ[0])/k->vol[0])
      * Xk_cubed / (sigma_s2 + Xk_squared);

    return lbar;
}


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

  accessor functions

  get_ functions return silently if k is null
  set_ functions return 0.0 if k is null

  set_ functions also call kalman_restart since the old
  filter state is no longer valid once these fields are reset.

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

void kalman_set_a(kalman_t *k, double a)
{
    if (k) {
      k->a = a;
      kalman_restart(k);
  }
}

void kalman_set_b(kalman_t *k, double b)
{
    if (k) {
      k->b = b;
      kalman_restart(k);
  }
}

void kalman_set_l(kalman_t *k, double l)
{
    if (k) {
      k->l = l;
      kalman_restart(k);
  }
}

void kalman_set_sigma_no(kalman_t *k, double sigma_no)
{
    if (k) {
      k->sigma_no = sigma_no;
      kalman_restart(k);
  }
}

void kalman_set_sigma_s(kalman_t *k, double sigma_s)
{
    if (k)  {
      k->sigma_s = sigma_s;
      kalman_restart(k);
  }
}


void kalman_set_initial_speed(kalman_t *k, double initial_speed)
{
    if (k) {
      k->initial_speed = initial_speed;
      kalman_restart(k);
  }
}



double kalman_get_a(kalman_t *k)
{
    if (k)
      return k->a;
    else
      return 0.0;
}

double kalman_get_b(kalman_t *k)
{
    if (k)
      return k->b;
    else
      return 0.0;
}

double kalman_get_l(kalman_t *k)
{
    if (k)
      return k->l;
    else
      return 0.0;
}


double kalman_get_sigma_no(kalman_t *k)
{
    if (k)
      return k->sigma_no;
    else
      return 0.0;
}

double kalman_get_sigma_s(kalman_t *k)
{
    if (k)
      return k->sigma_s;
    else
      return 0.0;
}

double kalman_get_initial_speed(kalman_t *k)
{
    if (k)
      return k->initial_speed;
    else
      return 0.0;
}