#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;		// Modelprotected: // 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)	{}};