kaltest.c++

面向对象的卡尔曼滤波器源码

/* kaltest	test  Kalman filter with 1-D 1 direction wave equation


	du/dt + c du/dx = 0

	can use float* or Vector for the field values

*/
static const char rcsid[] = "@(#)kaltest.c++	1.1 14:43:22 7/2/93   EFC";

#define PROGRAM	"kaltest"
#define REV_LEV "V1.1"

#include <stdlib.h>
#include <iostream.h>
#include <math.h>

#ifndef NO_TIMING
#include <cputime.hpp>
#endif

#include <vector.hpp>

#ifndef INCLUDE_VECTOR
#define INCLUDE_VECTOR
#endif

#include <matrix.hpp>

#include <standio.hpp>

#include "laxwen1.hpp"

#include "kalman.hpp"

#include <getargs.h>


#define DEBUG


#define max(a,b)	(((a) > (b) ) ? (a) : (b) )
#define min(a,b)	(((a) < (b) ) ? (a) : (b) )

#ifndef PI
#define PI	3.141592654
#endif

int steps = 200;
int n = 32;
double dt = 0.01;
double dx = 0.02;		// now set by 1 / n
double wnum = 1.0;
double c = 1.0;
int obs_rate = 1;
int menu = 0;
int verbose = 0;

ARG	Argtab[] =
{
	{ 'c', REAL,	(int *)&c,	"c wave speed"		},
	{ 'n', INTEGER, &n,		"number of x points"	},
	{ 'h', REAL,	(int *)&dt,	"h (dt)"	        },
	{ 'o', INTEGER, &obs_rate,	"observation rate"	},
	{ 't', INTEGER, &steps,		"number of time steps"	},
	{ 'v', BOOLEAN, &verbose,	"verbose execution"	},
	{ 'w', REAL,	(int *)&wnum,	"initial wave number"   },
	{ '-', BOOLEAN, &menu,		"display this list"	}
};

#define TABSIZE     ( sizeof(Argtab) / sizeof(ARG) )

// entries in oindex are the observation locations
int oindex[] = { 16, 5, 22 };
#define m	( sizeof( oindex ) / sizeof( oindex[0] ) )

void analytic(float *u, int nx, float dx, float t);

// the derivative function, for use by the Lax-Wendroff Method
float p(Vector& phi, int j)
{
	float u = phi[j];

	return c * u;
}


int main(int argc, char **argv)
{
	int i, it, nskip;
	float *ue;
	float t;
	
	argc = getargs( argc, argv, Argtab, TABSIZE);

	if (menu)
	{
		menu = 0;
		cerr << PROGRAM << '\t' << REV_LEV << '\n';
		cerr << "usage: " << PROGRAM << " [options] [outfile]\n";
		pr_usage( Argtab, TABSIZE);
		return 1;
	}
	
        Stdout fout(&argc, argv);

        if ( !fout )
        {
              cerr << PROGRAM << " unable to open output file " << argv[1] << endl;
              return 1;
        }



	dx = 1.0 / n;


	Vector u(n);		// size of system vector is n
	Vector z(m);		// size of observation vector is m

	ue = new float[n];


	LaxWendroff1 model(p, dx, dt, n );	// wave equation dynamics

	// define a Kalman filter for wave equation dynamics
	// n system points, m observation points
	Kalman filter( n, m, model );

	// set up IC

	for (i = 0; i < n; i++)
	{
		u[i] = 0.0;
	}

	// tell Kalman filter about system noise, observation noise
	//	and where the observations are
	Matrix q(n, n);
	q = 0.0;
	for (i = 0; i < n; i++)
		q(i,i) = 1.0e-3;

	Matrix r(m, m);
	r = 0.0;
	for (i = 0; i < m; i++)
		r(i,i) = 0.05;

	Matrix h(m, n);
	h = 0.0;
	for (i = 0; i < m; i++)
		h(i, oindex[i] ) = 1.0;


	filter.sys_noise( q );		// tell filter system noise level
	filter.obs_noise( r );		// tell filter observation noise level
	filter.obs_sys( h );		// tell filter what parts are observed

	filter.initial( u );		// set initial state for filter

	
	if ( --obs_rate < 0 )
		obs_rate = 0;
	
	if ( verbose )
	{
		float ns;
		cerr << PROGRAM << '\t' << REV_LEV << '\t';

		cerr << model.method() << '\n';

		ns = c / dt;
		cerr << "c(k) = " << c << '\t';
		cerr << "steps to cross domain: " << ns << '\n';
		cerr << "grids points in domain: " << n << '\t';
		cerr << "dx: " << dx << '\t';
		cerr << "dt: " << dt << '\n';

	}

#ifndef NO_TIMING
	CPUTime timer;
#endif

	for (it = 0, nskip = 0, t = 0.0; it <= steps; it++, t += dt)
	{
		analytic(ue, n, dx, t);

		if (nskip == 0)
		{


			// get observation(s) from the analytic data
			// simulating an observation system, adding random noise
			// would be even more realistic.
			for (i = 0; i < m; i++)
				z[i] = ue[ oindex[i] ];

			// give the filter the observations for the next time step
			filter << z;


			nskip = obs_rate;
		}
		else
			nskip--;
		

			// return the filtered result in u, ( u << filter )
			u << filter;

			// output filter value, exact value, and covariance diagonal
			for (i = 0; i < n; i++)
				fout << u[i] << '\t' << ue[i] << '\t'
				      << filter.p[i][i] << '\n';
			fout << '\n';


		// take a time step
		++filter;

	}

	return 0;


}


void analytic(float *u, int nx, float dx, float t)
{
	int i;
	float k;

	k = 2.0 * PI * wnum;
	for (i = 0; i < nx; i++)
		u[i] = sin( k * (i * dx - c * t ) );

}