bayesflt.hpp

Bayesian Filtering Classe C++source

class Linear_correlated_observe_model : public Linrz_correlated_observe_model
/* Linear observation model, correlated observation noise
    zp(k) = Hx(k) * x(k|k-1)
    Enforces linear model invariant. Careful when deriving to to change this invariant!
 */
{
public:
	Linear_correlated_observe_model (std::size_t x_size, std::size_t z_size) :
		Linrz_correlated_observe_model(x_size, z_size), hx(z_size)
	{}
	const FM::Vec& h(const FM::Vec& x) const
	{	// Provide a linear implementation of functional h assumes model is already Linrz for Hx
		hx.assign (FM::prod(Hx,x));
		return hx;
	}
private:
	mutable FM::Vec hx;
};

class Linear_uncorrelated_observe_model : public Linrz_uncorrelated_observe_model
/* Linear observation model, uncorrelated observation noise
    zp(k) = Hx(k) * x(k|k-1)
    Enforces linear model invariant. Careful when deriving to to change this invariant!
 */
{
public:
	Linear_uncorrelated_observe_model (std::size_t x_size, std::size_t z_size) :
		Linrz_uncorrelated_observe_model(x_size, z_size), hx(z_size)
	{}
	const FM::Vec& h(const FM::Vec& x) const
	{	// Provide a linear implementation of functional h assumes model is already Linrz for Hx
		hx.assign (FM::prod(Hx,x));
		return hx;
	}
private:
	mutable FM::Vec hx;
};


/*
 * Bayesian Filter
 *
 * A Bayesian Filter uses Bayes rule to fuse the state probabilities
 * of a prior and a likelhood function
 */
class Bayes_filter_base : public Bayes_base
{
	// Empty
};

/*
 * Likelihood Filter - Abstract filtering property
 * Represents only the Bayesian Likelihood of a state observation
 */
class Likelihood_filter : virtual public Bayes_filter_base
{
public:
	/* Virtual functions for filter algorithm */

	virtual void observe (Likelihood_observe_model& h, const FM::Vec& z) = 0;
	/* Observation state posterior using likelihood model h at z
	*/
};

/*
 * Functional Filter - Abstract filtering property
 * Represents only filter predict by a simple functional
 * (non-stochastic) model
 * 
 * A similar functional observe is not generally useful. The inverse of h is needed for observe!
 */
class Functional_filter : virtual public Bayes_filter_base
{
public:
	/* Virtual functions for filter algorithm */

	virtual void predict (Functional_predict_model& f) = 0;
	/* Predict state with functional no noise model
	    Requires x(k|k), X(k|k) or internal equivilent
	    Predicts x(k+1|k), X(k+1|k), using predict model
	*/
};

/*
 * State Filter - Abstract filtering property
 * Represents only filter state and an update on that state
 */
class State_filter : virtual public Bayes_filter_base
{
public:
	State_filter (std::size_t x_size);
	/* Set constant sizes, state must not be empty (must be >=1)
	    Exceptions:
	     bayes_filter_exception is x_size < 1
	 */

	FM::Vec x;			// expected state

	/* Virtual functions for filter algorithm */

	virtual void update () = 0;
	/* Update filters state
	    Updates x(k|k)
	*/
};


/*
 * Kalman State Filter - Abstract filtering property
 * Linear filter representation for 1st (mean) and 2nd (covariance) moments of a distribution
 *
 * Probability distributions are represted by state vector (x) and a covariance matix.(X)
 *
 * State (x) sizes is assumed to remain constant.
 * The state and state covariance are public so they can be directly manipulated.
 *  init: Should be called if x or X are altered
 *  update: Guarantees that any internal changes made filter are reflected in x,X.
 *  This allows considerable flexibility so filter implemtations can use different numerical representations
 *
 * Derived filters supply definititions for the abstract functions and determine the algorithm used
 * to implement the filter.
 */

class Kalman_state_filter : public State_filter
{
public:
	FM::SymMatrix X;	// state covariance

	Kalman_state_filter (std::size_t x_size);
	/* Initialise filter and set constant sizes
	 */

	/* Virtual functions for filter algorithm */

	virtual void init () = 0;
	/* Initialise from current state and state covariance
	    Requires x(k|k), X(k|k)
	*/
	void init_kalman (const FM::Vec& x, const FM::SymMatrix& X);
	/* Initialise from a state and state covariance
	    Parameters that reference the instance's x and X members are valid
	*/
	virtual void update () = 0;
	/* Update filters state and state covariance 
	    Updates x(k|k), X(k|k)
	*/
						
	// Minimum allowable reciprocal condition number for PD Matrix factorisations
	Numerical_rcond rclimit;
};


/*
 * Information State Filter - Abstract filtering property
 * Linear filter information space representation for 1st (mean) and 2nd (covariance) moments of a distribution
 *   Y = inv(X)   Information
 *   y = Y*x      Information state
 */

class Information_state_filter : virtual public Bayes_filter_base
{
public:
	Information_state_filter (std::size_t x_size);
	FM::Vec y;				// Information state
	FM::SymMatrix Y;		// Information

	virtual void init_yY () = 0;
	/* Initialise from a information state and information
	    Requires y(k|k), Y(k|k)
	    Parameters that reference the instance's y and Y members are valid
	*/
	void init_information (const FM::Vec& y, const FM::SymMatrix& Y);
	/* Initialise from a information state and information
	    Parameters that reference the instance's y and Y members are valid
	*/

	virtual void update_yY () = 0;
	/* Update filters information state and information
	    Updates y(k|k), Y(k|k)
	*/
};


/*
 * Linearizable filter models - Abstract filtering property
 *  Linrz == A linear, or gradient Linearized filter
 *
 * Predict uses a Linrz_predict_model that maintains a Jacobian matrix Fx and addative noise
 * NOTE: Functional (non-stochastic) predict is NOT possible as predict requires Fx.
 *
 * Observe uses a Linrz_observe_model and a variable size observation (z)
 * There are two variants for correlated and uncorrelated observation noise
 * Derived filters supply the init,predict,observe,update functions and determine
 * the algorithm used to implement the filter.
 */

class Linrz_filter : virtual public Bayes_filter_base
{ 
public:
	/* Virtual functions for filter algorithm */

	virtual Float predict (Linrz_predict_model& f) = 0;
	/* Predict state using a Linrz model
	    Requires x(k|k), X(k|k) or internal equivilent
	    Returns: Reciprocal condition number of primary matrix used in predict computation (1. if none)
	*/

	virtual Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z) = 0;
	virtual Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z) = 0;
	/* Observation z(k) and with (Un)correlated observation noise model
	    Requires x(k|k), X(k|k) or internal equivilent
	    Returns: Reciprocal condition number of primary matrix used in observe computation (1. if none)
	*/
};


/*
 * Linearizable Kalman Filter
 *  Kalman state representation and linearizable models
 *
 * Common abstration for many linear filters
 *  Has a virtual base to represent the common state
 */
class Linrz_kalman_filter : public Linrz_filter, virtual public Kalman_state_filter
{
protected:
	Linrz_kalman_filter() : Kalman_state_filter(0) // define a default constructor
	{}
};


/*
 * Extended Kalman Filter
 *  Kalman state representation and linearizable models
 *
 * Observe is implemented using an innovation computed from the non-linear part of the
 * obseve model and linear part of the Linrz_observe_model
 *
 * Common abstration for many linear filters
 *  Has a virtual base to represent the common state
 */
class Extended_kalman_filter : public Linrz_kalman_filter
{
protected:
	Extended_kalman_filter() : Kalman_state_filter(0) // define a default constructor
	{}
public:
	virtual Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z);
	virtual Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z);
	/* Observation z(k) and with (Un)correlated observation noise model
	    Requires x(k|k), X(k|k) or internal equivilent
	    Returns: Reciprocal condition number of primary matrix used in observe computation (1. if none)
	    Default implementation simple computes innovation for observe_innovation
	*/

	virtual Float observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s) = 0;
	virtual Float observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s) = 0;
	/* Observation innovation s(k) and with (Un)correlated observation noise model
	    Requires x(k|k), X(k|k) or internal equivilent
	    Returns: Reciprocal condition number of primary matrix used in observe computation (1. if none)
	*/
};


/*
 * Sample State Filter - Abstract filtering property
 *
 * Probability distributions are represted by a finite sampling
 *
 * State (x_size) size and its sampling (s_size) are assumed to remain constant.
 * The state sampling public so they can be directly manipulated.
 *  init: Should be used if they to be altered
 *  update: Guarantees that any internal changes made filter are reflected in sampling S.
 */

class Sample_state_filter : virtual public Bayes_filter_base
{
public:
	FM::ColMatrix S;		// state sampleing (x_size,s_size)

	Sample_state_filter (std::size_t x_size, std::size_t s_size);
	/* Initialise filter and set constant sizes for
	    x_size of the state vector
	    s_size sample size
	    Exceptions:
	     bayes_filter_exception is s_size < 1
	*/
	~Sample_state_filter() = 0;	// ISSUE Provide unambigues distructor incase S is not distructable

	/* Virtual functions for filter algorithm */

	virtual void init_S () = 0;
	/* Initialise from current sampleing
	*/

	void init_sample (const FM::ColMatrix& initS);
	/* Initialise from a sampling
	 */

	virtual Float update_resample () = 0;
	/* Resampling update
	    Returns lcond, Smallest normalised likelihood weight, represents conditioning of resampling solution
	            lcond == 1. if no resampling performed
	            This should by multipled by the number of samples to get the Likelihood function conditioning
	 */

	std::size_t unique_samples () const;
	/* Count number of unique (unequal value) samples in S
	    Implementation requires std::sort on sample column references
	*/
};


/*
 * Sample Filter: Bayes filter using
 *
 * Probability distributions are represted by a finite sampling
 *
 * The filter is operated by performing a
 * 	predict, observe
 * cycle derived from the bayes_filter. observe Likelihoods are merged into a single combined weight.
 *   update: MUST be used to complete a explict resampling of the particles using merged weights
 *
 * Derived filters supply definititions for the abstract functions and determine the algorithm used
 * to implement the filter.
 */

class Sample_filter : public Likelihood_filter, public Functional_filter, virtual public Sample_state_filter
{
public:
	Sample_filter (std::size_t x_size, std::size_t s_size);
	/* Initialise filter and set constant sizes for
	    x_size of the state vector
	    s_size sample size
	    Exceptions:
	     bayes_filter_exception is s_size < 1
	*/

	/* Virtual functions for filter algorithm */

	virtual void predict (Functional_predict_model& f);
	/* Predict state posterior with functional no noise model
	*/

	virtual void predict (Sampled_predict_model& f) = 0;
	/* Predict state posterior with sampled noise model
	*/

	virtual void observe (Likelihood_observe_model& h, const FM::Vec& z) = 0;
	/* Observation state posterior using likelihood model h at z
	*/

	virtual void observe_likelihood (const FM::Vec& lw) = 0;
	/* Observation fusion directly from likelihood weights
	    lw may be smaller then the state sampling. Weights for additional particles are assumed to be 1
	*/
};


}//namespace
#endif