bayesflt.hpp

Bayesian Filtering Classe C++source

#ifndef _BAYES_FILTER
#define _BAYES_FILTER

/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id: bayesFlt.hpp 562 2006-04-05 20:46:23 +0200 (Wed, 05 Apr 2006) mistevens $
 */

/*
 * Bayesian filtering reresented as Dual heirarchy of:
 *  Prediction and Observation models
 *  Filtering Schemes
 */
 
// Common headers required for declerations
#include "bayesException.hpp"	// exception types
#include "matSupSub.hpp"			// matrix support subsystem

/* Filter namespace */
namespace Bayesian_filter
{
	// Allow use of a short name for matrix namespace
	namespace FM = Bayesian_filter_matrix;


/*
 * Abstraction support classes, at the base of the heirarchy
 */

class Bayes_base {
/*
 * A very abstract Polymorphic base representation!
 * Interface provides: type, internal error handing, and destruction
 */
public:
	typedef Bayesian_filter_matrix::Float Float;
	// Type used thoughout as a number representation for state etc

	virtual ~Bayes_base() = 0;
	// Polymorphic

	static void error (const Numeric_exception& a);
	static void error (const Logic_exception& a);
	// Report a filter, throw a Filter_exception
	//  No exception saftey rules are specified, assume the object is invalid
	// May have side effects for debuging
};


class Numerical_rcond
/*
 * Numerical comparison of reciprocal condition numbers
 *  Required for all linear algebra in models and filters
 *  Implements minimum allowable reciprocal condition number for PD Matrix factorisations
 */
{
public:
	Numerical_rcond()
	{	limit_PD = limit_PD_init;
	}
	void set_limit_PD(Bayes_base::Float nl)
	{	limit_PD = nl;
	}
	inline void check_PSD (Bayes_base::Float rcond, const char* error_description) const
	/* Checks a the reciprocal condition number
	 * Generates a Bayes_filter_exception if value represents a NON PSD matrix
	 * Inverting condition provides a test for IEC 559 NaN values
	 */
	{	if (!(rcond >= 0))
			Bayes_base::error (Numeric_exception (error_description));
	}

	inline void check_PD (Bayes_base::Float rcond, const char* error_description) const
	/* Checks a reciprocal condition number
	 * Generates a Bayes_filter_exception if value represents a NON PD matrix
	 * I.e. rcond is bellow given conditioning limit
	 * Inverting condition provides a test for IEC 559 NaN values
	 */
	{	if (!(rcond >= limit_PD))
			Bayes_base::error (Numeric_exception (error_description));
	}
private:
	Bayes_base::Float limit_PD;		
	const static Bayes_base::Float limit_PD_init;	// Initial common value for limit_PD
};


/*
 * Abstract Prediction models
 *  Predict models are used to parameterise predict functions of filters
 */
class Predict_model_base : public Bayes_base
{
	// Empty
};


class Sampled_predict_model : virtual public Predict_model_base
/* Sampled stochastic predict model
    x*(k) = fw(x(k-1), w(k))
   fw should generate samples from the stochastic variable w(k)
   This fundamental model is used instead of the predict likelihood function L(x*|x)
   Since drawing samples from an arbitary L is non-trivial (see MCMC theory)
   the burden is place on the model to generate these samples.
   Defines an Interface without data members
 */
{
public:
	virtual const FM::Vec& fw(const FM::Vec& x) const = 0;
	// Note: Reference return value as a speed optimisation, MUST be copied by caller.
};

class Functional_predict_model :virtual public Predict_model_base
/* Functional (non-stochastic) predict model f
    x*(k) = fx(x(k-1))
   This fundamental model is used instead of the predict likelihood function L(x*|x)
   Since L is a delta function which isn't much use for numerical systems.
   Defines an Interface without data members
 */
{
public:
	virtual const FM::Vec& fx(const FM::Vec& x) const = 0;
	// Functional model
	// Note: Reference return value as a speed optimisation, MUST be copied by caller.
	
	const FM::Vec& operator()(const FM::Vec& x) const
	{	// Operator form of functional model
		return fx(x);
	}
};

class Gaussian_predict_model : virtual public Predict_model_base
/* Gaussian noise predict model
   This fundamental noise model for linear/linearised filtering
    x(k|k-1) = x(k-1|k-1)) + G(k)w(k)
    G(k)w(k)
    q(k) = state noise covariance, q(k) is covariance of w(k)
    G(k) = state noise coupling
*/
{
public:
	Gaussian_predict_model (std::size_t x_size, std::size_t q_size);

	FM::Vec q;		// Noise variance (always dense, use coupling to represent sparseness)
	FM::Matrix G;		// Noise Coupling
	
	Numerical_rcond rclimit;
	// Reciprocal condition number limit of linear components when factorised or inverted
};

class Addative_predict_model : virtual public Predict_model_base
/* Addative Gaussian noise predict model
   This fundamental model for non-linear filtering with addative noise
    x(k|k-1) = f(x(k-1|k-1)) + G(k)w(k)
    q(k) = state noise covariance, q(k) is covariance of w(k)
    G(k) = state noise coupling
   ISSUE Should be privately derived from Gaussian_predict_model but access control in GCC is broken
*/
{
public:
	Addative_predict_model (std::size_t x_size, std::size_t q_size);

	virtual const FM::Vec& f(const FM::Vec& x) const = 0;
	// Functional part of addative model
	// Note: Reference return value as a speed optimisation, MUST be copied by caller.

	FM::Vec q;		// Noise variance (always dense, use coupling to represent sparseness)
	FM::Matrix G;		// Noise Coupling

	Numerical_rcond rclimit;
	// Reciprocal condition number limit of linear components when factorised or inverted
};

class Linrz_predict_model : public Addative_predict_model
/* Linrz predict model
   This fundamental model for linear/linearised filtering
    x(k|k-1) = f(x(k-1|k-1)
    Fx(x(k-1|k-1) = Jacobian of of functional part fx with respect to state x
 */
{
public:
	Linrz_predict_model (std::size_t x_size, std::size_t q_size);
	FM::Matrix Fx;		// Model
};

class Linear_predict_model : public Linrz_predict_model
/* Linear predict model
   Enforces linearity on f
    x(k|k-1) = Fx(k-1|k-1) * x(k-1|k-1)
 */
{
public:
	Linear_predict_model (std::size_t x_size, std::size_t q_size);
	const FM::Vec& f(const FM::Vec& x) const
	{	// Provide the linear implementation of functional f
		xp.assign (FM::prod(Fx,x));
		return xp;
	}
private:
	mutable FM::Vec xp;
};

class Linear_invertable_predict_model : public Linear_predict_model
/* Linear invertable predict model
   Fx has an inverse
    x(k-1|k-1) = inv.Fx(k-1|k-1) * x(k|k-1)
 */
{
public:
	Linear_invertable_predict_model (std::size_t x_size, std::size_t q_size);
	struct inverse_model {
		inverse_model (std::size_t x_size);
		FM::ColMatrix Fx;	// Model inverse (ColMatrix as usually transposed)
	} inv;
};



/*
 * Abstract Observation models
 *  Observe models are used to parameterise the observe functions of filters
 */
class Observe_model_base : public Bayes_base
{
	// Empty
};

class Observe_function : public Bayes_base
// Function object for predict of observations
{
public:
	virtual const FM::Vec& h(const FM::Vec& x) const = 0;
	// Note: Reference return value as a speed optimisation, MUST be copied by caller.
};


class Likelihood_observe_model : virtual public Observe_model_base
/* Likelihood observe model L(z |x)
 *  The most fundamental Bayesian definition of an observation
 * Defines an Interface without data members
 */
{
public:
	Likelihood_observe_model(std::size_t z_size) : z(z_size)
	{}
	virtual Float L(const FM::Vec& x) const = 0;
	// Likelihood L(z | x)

	virtual void Lz (const FM::Vec& zz)
	// Set the observation zz about which to evaluate the Likelihood function
	{
		z = zz;
	}
protected:
	FM::Vec z;			// z set by Lz
};

class Functional_observe_model : virtual public Observe_model_base, public Observe_function
/* Functional (non-stochastic) observe model h
 *  zp(k) = hx(x(k|k-1))
 * This is a seperate fundamental model and not derived from likelihood because:
 *  L is a delta function which isn't much use for numerical systems
 * Defines an Interface without data members
 */
{
public:
	Functional_observe_model(std::size_t /*z_size*/)
	{}
	const FM::Vec& operator()(const FM::Vec& x) const
	{	return h(x);
	}

};

class Parametised_observe_model : virtual public Observe_model_base, public Observe_function
/* Observation model parametised with a fixed z size
 *  Includes the functional part of a noise model
 *  Model is assume to have linear and non-linear components
 *  Linear components need to be checked for conditioning
 *  Non-linear components may be discontinous and need normalisation
 */
{
public:
	Parametised_observe_model(std::size_t /*z_size*/)
	{}
	virtual const FM::Vec& h(const FM::Vec& x) const = 0;
	// Functional part of addative model
	virtual void normalise (FM::Vec& /*z_denorm*/, const FM::Vec& /*z_from*/) const
	// Discontinous h. Normalise one observation (z_denorm) from another
	{}	//  Default normalistion, z_denorm unchanged
	
	Numerical_rcond rclimit;
	// Reciprocal condition number limit of linear components when factorised or inverted
};

class Uncorrelated_addative_observe_model : public Parametised_observe_model
/* Observation model, uncorrelated addative observation noise
	Z(k) = I * Zv(k) observe noise variance vector Zv
 */
{
public:
	Uncorrelated_addative_observe_model (std::size_t z_size) :
		Parametised_observe_model(z_size), Zv(z_size)
	{}
	FM::Vec Zv;			// Noise Variance
};

class Correlated_addative_observe_model : public Parametised_observe_model
/* Observation model, correlated addative observation noise
    Z(k) = observe noise covariance
 */
{
public:
	Correlated_addative_observe_model (std::size_t z_size) :
		Parametised_observe_model(z_size), Z(z_size,z_size)
	{}
	FM::SymMatrix Z;	// Noise Covariance (not necessarly dense)
};

class Jacobian_observe_model : virtual public Observe_model_base
/* Linrz observation model Hx, h about state x (fixed size)
    Hx(x(k|k-1) = Jacobian of h with respect to state x
	Normalisation consistency Hx: Assume normalise will be from h(x(k|k-1)) so result is consistent with Hx
 */
{
public:
	FM::Matrix Hx;		// Model
protected: // Jacobian model is not sufficient, it is used to build Linrz observe model's
	Jacobian_observe_model (std::size_t x_size, std::size_t z_size) :
		Hx(z_size, x_size)
	{}
};

class Linrz_correlated_observe_model : public Correlated_addative_observe_model, public Jacobian_observe_model
/* Linrz observation model Hx, h with repespect to state x (fixed size)
    correlated observation noise
    zp(k) = h(x(k-1|k-1)
    Hx(x(k|k-1) = Jacobian of f with respect to state x
    Z(k) = observe noise covariance
 */
{
public:
	Linrz_correlated_observe_model (std::size_t x_size, std::size_t z_size) :
		Correlated_addative_observe_model(z_size), Jacobian_observe_model(x_size, z_size)
	{}
};

class Linrz_uncorrelated_observe_model : public Uncorrelated_addative_observe_model, public Jacobian_observe_model
/* Linrz observation model Hx, h with repespect to state x (fixed size)
    uncorrelated observation noise
    zp(k) = h(x(k-1|k-1)
    Hx(x(k|k-1) = Jacobian of f with respect to state x
    Zv(k) = observe noise covariance
 */
{
public:
	Linrz_uncorrelated_observe_model (std::size_t x_size, std::size_t z_size) :
		Uncorrelated_addative_observe_model(z_size), Jacobian_observe_model(x_size, z_size)
	{}
};