dynamics.c++

卡尔曼滤波类

// dynamics.cpp		implementation of 1-D Dynamics Class

//                      (c) Copyright 1995, Everett F. Carter Jr.
//                      Permission is granted by the author to use
//			this software for any application provided this
//			copyright notice is preserved.

static const char rcsid[] = "@(#)dynamics.c++	1.4 12:37:59 6/3/96   EFC";

#include <string.h>
#include <dynamics.hpp>

// #define DEBUG
// #define DEBUG2

#ifdef DEBUG
#include <taygeta/traceback.hpp>
#endif
#ifdef PROFILE
#include <taygeta/profile.hpp>
#endif

char* Dynamics::name = NULL;
int   Dynamics::namelen = 0;
int   Dynamics::instances = 0;

Dynamics::Dynamics(const int n_in,const float ndt,const char* nam)
			 : n(n_in), dt(ndt), t(0.0), es_os(NULL)
{
#ifdef DEBUG
	TraceBack tb( "Dynamics::Dynamics(...)" );
#endif
#ifdef PROFILE
	Profile pr( "Dynamics::Dynamics(...)" );
#endif
	 u.resize(n_in);

	if ( instances++ == 0 || name == NULL )
	{
        	if ( nam != NULL )
			set_name( nam );
        	else
                	namelen = 0;
	}

	Dynamics::reset();

#ifdef DEBUG2
	cerr << "\nname is: " << name << "\tDynamics::instances = " << instances << endl;
#endif
        
}

Dynamics::Dynamics() : n(0), dt(0.0), t(0.0), u(1), es_os(NULL)
{
#ifdef DEBUG
	TraceBack tb( "Dynamics::Dynamics()" );
	tb << "on entry instances = " << instances << endl;
#endif

	if ( instances++ == 0 )
	{
		name = NULL;
		namelen = 0;
	}
#ifdef DEBUG2
	cerr << "Dynamics::instances = " << instances << "\tname: " << name << endl;
#endif

	set_flag( -1 );
#ifdef DEBUG2
	cerr << "Dynamics::Dynamics() set flag to " << status() << endl;
	cerr << "\tn = " << n << " u is " << u.size() << endl;
#endif
#ifdef DEBUG
	tb << "set flag to " << status() << endl;
	tb << "n = " << n << " u is " << u.size() << endl;
#endif


}

Dynamics::Dynamics(const Dynamics& dy)
{
#ifdef DEBUG
	TraceBack tb( "Dynamics::Dynamics(Dynamics&)" );
#endif
	n = dy.n;
	t = dy.t;
	dt = dy.dt;

	u.resize(n);

	u = dy.u;

	if ( instances++ == 0 )
	{
		name = NULL;
		namelen = 0;
	}

	es_os = dy.es_os;

#ifdef DEBUG2
	cerr << "Dynamics::instances = " << instances << "\tname: " << name << endl;
#endif

	set_flag( dy.err_flag );

#ifdef DEBUG2
	cerr << "Dynamics::Dynamics(&) set flag to " << status() << endl;
	cerr << "\tu is " << u.n << endl;
#endif

}

Dynamics::~Dynamics()
{
#ifdef DEBUG
	TraceBack tb( "Dynamics::~Dynamics()" );
#endif
#ifdef DEBUG2
	cerr << "Dynamics::instances = " << instances << " at dtor" << endl;
#endif

          	 if ( --instances == 0 && name )
		 {
#if defined( __ZTC__ ) && __ZTC__ <= 0x301
			delete [namelen+1]name;
#else
                        delete []name;
#endif
			name = NULL;
			namelen = 0;
		 }

}

Dynamics& Dynamics::operator=(const Dynamics& dy)
{
#ifdef DEBUG
	TraceBack tb( "Dynamics::operator=(Dynamics)" );
#endif
	n = dy.n;
	t = dy.t;
	dt = dy.dt;

	u.resize(n);

	u = dy.u;

	set_flag( dy.err_flag );

	if ( dy.es_os )
		es_os = dy.es_os;

	return *this;
}

void Dynamics::set_name(const char* newname)
{
#ifdef DEBUG
	TraceBack tb( "Dynamics::set_name()" );
#endif
	if ( name )
		delete []name;

	if ( newname && strlen( newname ) > 1 )
	{
        	namelen = strlen( newname ) + 1;
        	name = new char[ namelen + 1 ];
        	strcpy( name, newname );
#ifdef DEBUG
	if ( es_os )
		*es_os << this << "  Dynamics::set_name(), set to " << name << '\n';
#endif
	}
	else
	{
		namelen = 0;
		name = NULL;
	}

}

void Dynamics::init(const int n_in,const float ndt,const char* nam)
{
#ifdef DEBUG
	TraceBack tb( "Dynamics::init(int,float,char*)" );
#endif
#ifdef PROFILE
	Profile pr( "Dynamics::init()" );
#endif
	 n = n_in;
         dt = ndt;
         t = 0.0;

#ifdef DEBUG

	tb << "about to resize to n = " << n << " u is currently: " << u.size() << endl;
	// tb << "u.rows = " << u.rows << '\n';
#endif         
	 u.resize(n);

#ifdef DEBUG
	tb << "resized" << endl;
#endif

	 if ( nam && name == NULL )
		     set_name( nam );


	Dynamics::reset();

#ifdef DEBUG
	tb << "about to clear the error flag" << endl;
	tb << "the flag is " << status() << endl;
#endif
         reset_flag();
        
}

ostream& Dynamics::parameter_settings(ostream& os)
{
	if ( name != NULL )
		os << method() << "\n  ";
	os << " dimension: " << dimension() << "   delta t: " << delta_t();

	return os;

}

float Dynamics::time(const float tt)
{
	float old_t = t;

	t = tt;

	return old_t;
}

int Dynamics::dimension()
{
	return n;
}

float Dynamics::delta_t()
{
	return dt;
}

int Dynamics::dimension(const int d)
{
	int old_d = n;
#ifdef DEBUG
	if ( es_os )
		*es_os << this << "  Dynamics::dimension(int)\told dimension = "
			<< old_d << "   new dimension = " << d << '\n';
#endif
	if ( d != n )
		u.resize( d );
	n = d;


	return old_d;
}

float Dynamics::delta_t(const float h)
{
	float old_dt = dt;

#ifdef DEBUG
	if ( es_os )
		*es_os << this << "  Dynamics::delta_t(float)\told delta_t = "
		       << old_dt << "   new delta_t = " << h << '\n';
#endif

	dt = h;


	return old_dt;
}

void Dynamics::input_data(const BasicArray& v)
{
	for (int i = 0; i < n; i++)
		u[i] =v[i];

}


void Dynamics::input_data(const float* v)
{
	for(int i = 0; i < n; i++)
		u[i] = v[i];

}

void Dynamics::input_data(const double* v)
{
	for(int i = 0; i < n; i++)
		u[i] = v[i];
}

void Dynamics::output_data(BasicArray& v)
{
	for (int i = 0; i < n; i++)
		v[i] =u[i];

}


void Dynamics::output_data(float* v)
{
	for(int i = 0; i < n; i++)
		v[i] = u[i];

}

void Dynamics::output_data(double* v)
{
	for(int i = 0; i < n; i++)
		v[i] = u[i];
}

void Dynamics::attach(ostream& os)
{
	 es_os = &os;

#ifdef DEBUG_2
	cerr << "Dynamics::attach(ostream&), setting es_os = " << es_os << endl;
#endif
}
	
ostream& operator<<(ostream& os, const Dynamics& model)
{
	os << model.u;

	return os;
}