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