kalman.c++

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

// kalman.cpp		implementation of the kalman filter class

static const char rcsid[] = "@(#)kalman.c++	1.3 16:44:57 10/6/92   EFC";

#include "kalman.hpp"

#include <lumatrix.hpp>

Kalman::Kalman(const int n_in,const int m_in, Dynamics& ph) : n(n_in),
			 m(m_in), k(gain), p(covariance), phi(ph)
{
	x.resize( n );
	z.resize( m );

	gain.resize(n, m);
	covariance.resize(n, n);
	q.resize(n, n);
	r.resize(m,m);
	h.resize(m, n);

	// scratch space
	xs.resize( n );
	ht.resize(n, m);
	pht.resize(n, m);
	denom.resize(m, m);
	innovation.resize( m );
	I.resize(n, n);

	covariance.diag( 1.0 );
	I.diag( 1.0 );

}

void Kalman::step_x(const int steps)
{
	phi <<  x;

	for (int it = 0; it < steps; it++)
	{
		++phi;
	}

	x << phi;
}

void Kalman::step_p(const int steps)
{
	int i, j;

	for (int it = 0; it < steps; it++)
	{
		for (i = 0; i < n; i++)		// step by columns of p
		{
			for (j = 0; j < n; j++)
				xs[j] = covariance(j,i);
			phi << xs;
			xs << ++phi;
			for (j = 0; j < n; j++)
				covariance(j,i) = xs[j];
		}

		for (i = 0; i < n; i++)	     // step by rows of p
		{
			for (j = 0; j < n; j++)
				xs[j] = covariance(i,j);
			phi << xs;
			xs << ++phi;
			for (j = 0; j < n; j++)
				covariance(i,j) = xs[j];
		}

		// now add system noise
		covariance = covariance + q;

	}

}

void Kalman::set_gain()
{
	pht = covariance * ht;

	denom = h * pht + r;

	denom = invm( denom );

	gain = pht * denom;
		
}

void Kalman::update_x()
{
	innovation = z - h * x;

	x = x + gain * innovation;

}

void Kalman::update_p()
{
	covariance = (I - gain * h) * covariance;
}


// get the filtered value
Vector& operator<<(Vector& v,const Kalman& filter)
{
	v = filter.x;

	return v;
}

// incorporate observations
Kalman& operator<<(Kalman& filter,const Vector& v)
{
	filter.z = v;

	filter.update_x();
	filter.update_p();

	return filter;

}

// do time step
Kalman& Kalman::operator++()
{

	step_x();

	step_p();

	set_gain();


	return *this;
}