covflt.cpp

Bayesian Filtering Classe C++source

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

/*
 * Covariance Filter.
 */
#include "covFlt.hpp"
#include "matSup.hpp"

/* Filter namespace */
namespace Bayesian_filter
{
	using namespace Bayesian_filter_matrix;


Covariance_scheme::Covariance_scheme (std::size_t x_size, std::size_t z_initialsize) :
	Kalman_state_filter(x_size),
	S(Empty), SI(Empty), W(Empty),
	tempX(x_size,x_size)
/*
 * Initialise filter and set the size of things we know about
 */
{
	last_z_size = 0;	// Leave z_size dependants Empty if z_initialsize==0
	observe_size (z_initialsize);
}

Covariance_scheme& Covariance_scheme::operator= (const Covariance_scheme& a)
/* Optimise copy assignment to only copy filter state
 * Precond: matrix size conformance
 */
{
	Kalman_state_filter::operator=(a);
	return *this;
}


void Covariance_scheme::init ()
{
						// Postconditions
	if (!isPSD (X))
		error (Numeric_exception("Initial X not PSD"));
}

void Covariance_scheme::update ()
{
	// Nothing to do, implicit in observation
}

Bayes_base::Float
 Covariance_scheme::predict (Linrz_predict_model& f)
/* Standard Linrz prediction
 */
{
	x = f.f(x);		// Extended Kalman state predict is f(x) directly
						// Predict state covariance
	noalias(X) = prod_SPD(f.Fx,X, tempX);
	noalias(X) += prod_SPD(f.G, f.q, tempX);

	return 1;
}

Bayes_base::Float
 Covariance_scheme::predict (Gaussian_predict_model& f)
/* Specialised 'stationary' predict, only addative noise
 */
{
						// Predict state covariance, simply add in noise
	noalias(X) += prod_SPD(f.G, f.q, tempX);
  
	return 1;
}


void Covariance_scheme::observe_size (std::size_t z_size)
/*
 * Optimised dynamic observe sizing
 */
{
	if (z_size != last_z_size) {
		last_z_size = z_size;

		S.resize(z_size,z_size, false);
		SI.resize(z_size,z_size, false);
		W.resize(x.size(),z_size, false);
	}
}

Bayes_base::Float
 Covariance_scheme::observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s)
/* correlated innovation observe
 */
{
						// Size consistency, z to model
	if (s.size() != h.Z.size1())
		error (Logic_exception("observation and model size inconsistent"));
	observe_size (s.size());// Dynamic sizing

						// Innovation covariance
	Matrix temp_XZ (prod(X, trans(h.Hx)));
	noalias(S) = prod(h.Hx, temp_XZ) + h.Z;

						// Inverse innovation covariance
	Float rcond = UdUinversePD (SI, S);
	rclimit.check_PD(rcond, "S not PD in observe");

						// Kalman gain, X*Hx'*SI
	noalias(W) = prod(temp_XZ, SI);

						// State update
	noalias(x) += prod(W, s);
	noalias(X) -= prod_SPD(W, S, temp_XZ);

	return rcond;
}


Bayes_base::Float
 Covariance_scheme::observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s)
/* uncorrelated innovation observe
 */
{
						// Size consistency, z to model
	if (s.size() != h.Zv.size())
		error (Logic_exception("observation and model size inconsistent"));
	observe_size (s.size());// Dynamic sizing

						// Innovation covariance
	Matrix temp_XZ (prod(X, trans(h.Hx)));
	noalias(S) = prod(h.Hx, temp_XZ);
	for (std::size_t i = 0; i < h.Zv.size(); ++i)
		S(i,i) += Float(h.Zv[i]);	// ISSUE mixed type proxy assignment

						// Inverse innovation covariance
	Float rcond = UdUinversePD (SI, S);
	rclimit.check_PD(rcond, "S not PD in observe");

						// Kalman gain, X*Hx'*SI
	noalias(W) = prod(temp_XZ, SI);

						// State update
	noalias(x) += prod(W, s);
	noalias(X) -= prod_SPD(W, S, temp_XZ);

	return rcond;
}

}//namespace