kalman_demo.c

来自「This package implements a Kalman filter 」· C语言 代码 · 共 121 行

static char *rcsid = "$Id: kalman_demo.c,v 1.1 1996/12/13 22:23:29 kint Exp $";

/* kalman_demo.c:  demonstrates Kalman filter using canned data
 *
 * Kalman functions used:
 *
 *   kalman_initialize: initializes Kalman filter for a sensor
 *
 *   kalman_filter: runs Kalman filter for a sensor
 *
 * Rick Kint
 * ITS Project, U of Washington
 */

#include <stdio.h>

/* included for optarg defs */
//#include <unistd.h>

#include "kalman.h"

#define LINELEN  80   

/* function declarations */

void usage();

/* default values */

char *default_data_file_name = "kalman.dat";

    
int main(int argc, char **argv)
{
    char         *data_file_name;  /* name of data file */
    kalman_t     k;                /* kalman state struct */
    FILE         *data_file;       /* data file */
    char         line[LINELEN];    /* line read from data file */
    int          volume,           /* volume read from file */
                 iteration;        /* current filter iteration */
    double       occupancy,        /* occupancy read from file */
                 speed,            /* speed calculated by filter */
                 length;           /* length calculated by filter */
    int          valid;            /* flag: is Kalman estimate usable? */
    extern char  *optarg;          /* for getopt: arg of option */
    int          option;           /* char returned by getopt */
    
    /* set defaults */

    data_file_name = default_data_file_name;

    /* parse CL args, if any */

    while ((option = getopt(argc, argv, "f:")) != EOF)
    {
	switch(option)
	{
	  case 'f':
	    data_file_name = optarg;
	    break;

	  default:
	    usage();
	    exit(1);
	}
    }

    /* open data file, exit on failure */

    data_file = fopen(data_file_name, "r");
    if (!data_file)
    {
	fprintf(stderr, "could not open data file %s", data_file_name);
	perror("");
	exit(1);
    }

    /* initializations */

    kalman_initialize(&k);

    iteration = 0;

    /* read through data file, printing results in format:
     *   iteration: volume occupancy speed length valid-string
     */

    while (fgets(line, LINELEN, data_file))
    {
	sscanf(line, "%d %lf", &volume, &occupancy);
	valid = kalman_filter(&k, volume, occupancy, &speed, &length);

	if (valid)
		printf("%3d: vol = %5d veh/hr, occ = %5.1f%%, speed = %6.2f mph, length = %6.2f ft\n",
		       iteration, volume, occupancy, speed, length);
	else
		printf("%3d: no speed estimate available\n", iteration);

	iteration++;
    }

    fclose(data_file);
}


/*
 * usage
 *
 * print usage message
 */

void usage()
{
    fprintf(stderr, "Usage: kalman_demo [-f data_file] [-v]\n");
    fprintf(stderr, "\t data_file = file containing sample data (default is %s\n",default_data_file_name);
    fprintf(stderr, "\t-v = verbose\n");
}