itrFlt.hpp

Go to the documentation of this file.

#ifndef _BAYES_FILTER_ITERATED_COVARIANCE
#define _BAYES_FILTER_ITERATED_COVARIANCE
 
/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */
 
/*
 * Iterated Covariance Filter Scheme.
 *  A non-linear Covariance (Kalman) filter with relinearisation and iteration
 *
 * The observe algorithm uses the iterated non-linear formulation 
 * from Bar-Shalom and Fortmann p.119 (full scheme)
 * Discontinuous observe models require that state is normalised with
 * respect to the observation.
 *
 * The filter is operated by performing a
 *  predict, observe
 * cycle defined by the base class
 */
#include "bayesFlt.hpp"
 
/* Filter namespace */
namespace Bayesian_filter
{
 
class Iterated_covariance_scheme;
 
class Iterated_observe_model : virtual public Observe_model_base
/* Linrz observation model which can be iterated
    Hx can be relinearised
 */
{
protected: // model is not sufficient, it is used to build observe model's
    Iterated_observe_model ()
    {}
    Iterated_observe_model () {…}
public:
    virtual void relinearise (const FM::Vec& x) = 0;
    // Relinearised about state x
};
class Iterated_observe_model : virtual public Observe_model_base {…};
 
 
class Iterated_terminator : public Bayes_base
/*
 * Termination condition for filter Iteration
 *  Used by iterated observe to parameterise termination condition
 *  If iteration continues, the terminator must also relinearise the model about the filters new state
 *
 * Defaults to immediately terminating the iteration
 *
 * A more useful terminator can built by derivation.
 * For example terminator constructed with a reference to the filter and model can
 * detect convergence of x and/or X
 */
{
public:
    virtual bool term_or_relinearize (const Iterated_covariance_scheme& f)
    {
        return true;
    }
    virtual bool term_or_relinearize (const Iterated_covariance_scheme& f) {…}
};
class Iterated_terminator : public Bayes_base {…};
 
class Counted_iterated_terminator : public Iterated_terminator
/* Termination condition with a simple fixed number of iterations
 */
{
public:
    Counted_iterated_terminator (Iterated_observe_model& model, unsigned iterations) :
        m(model), i(iterations)
    {}
    Counted_iterated_terminator (Iterated_observe_model& model, unsigned iterations) : {…}
    bool term_or_relinearize (const Iterated_covariance_scheme& f);
    Iterated_observe_model& m;
    unsigned i;
};
class Counted_iterated_terminator : public Iterated_terminator {…};
 
 
 
class Iterated_covariance_scheme : public Linrz_kalman_filter
{
public:
    Iterated_covariance_scheme (std::size_t x_size, std::size_t z_initialsize = 0);
    /* Initialised filter requires an addition iteration limit for the
       observe algorithm */
    Iterated_covariance_scheme& operator= (const Iterated_covariance_scheme&);
    // Optimise copy assignment to only copy filter state
 
    void init ();
    void update ();
    Float predict (Linrz_predict_model& f);
 
    Float observe (Linrz_uncorrelated_observe_model& h, Iterated_terminator& term, const FM::Vec& z);
    Float observe (Linrz_correlated_observe_model& h, Iterated_terminator& term, const FM::Vec& z);
    // Observe with iteration
    Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z)
    {   // Observe with default termination
        Iterated_terminator term;
        return observe (h, term, z);
    }
    Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z) {…}
    Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z)
    {   // Observe with default termination
        Iterated_terminator term;
        return observe (h, term, z);
    }
    Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z) {…}
 
public:                     // Exposed Numerical Results
    FM::SymMatrix S, SI;        // Innovation Covariance and Inverse
 
protected:                  // Permanently allocated temps
    FM::RowMatrix tempX;
 
protected:                  // allow fast operation if z_size remains constant
    std::size_t last_z_size;
    void observe_size (std::size_t z_size);
                            // Permanently allocated temps
    FM::Vec s;
    FM::Matrix HxT;
};
class Iterated_covariance_scheme : public Linrz_kalman_filter {…};
 
 
}//namespace
#endif