kalmanslam.cpp

Bayesian Filtering Classe C++source

/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2004 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id: kalmanSLAM.cpp 598 2006-10-04 19:44:40 +0000 (Wed, 04 Oct 2006) mistevens $
 */

/*
 * SLAM : Simultaneous Locatization and Mapping
 *  Kalman filter representing representation of SLAM
 */

		// Bayes++ Bayesian filtering schemes
#include "BayesFilter/bayesFlt.hpp"
		// Bayes++ SLAM
#include "SLAM.hpp"
#include "kalmanSLAM.hpp"
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

namespace SLAM_filter
{

template <class Base>
inline void zero(FM::ublas::matrix_range<Base> A)
// Zero a matrix_range
{	// Note A cannot be a reference
	typedef typename Base::value_type Base_value_type;
	FM::noalias(A) = FM::ublas::scalar_matrix<Base_value_type>(A.size1(),A.size2(), Base_value_type());
}

Kalman_SLAM::Kalman_SLAM( Kalman_filter_generator& filter_generator ) :
	SLAM(),
	fgenerator(filter_generator),
	loc(0), full(0)
{
	nL = 0;
	nM = 0;
}

Kalman_SLAM::~Kalman_SLAM()
{
	fgenerator.dispose (loc);
	fgenerator.dispose (full);
}

void Kalman_SLAM::init_kalman (const FM::Vec& x, const FM::SymMatrix& X)
{
	// TODO maintain map states
	nL = x.size();
	nM = 0;
	if (loc) fgenerator.dispose (loc);
	if (full) fgenerator.dispose (full);
		// generate a location filter for prediction
	loc = fgenerator.generate(nL);
		// generate full filter
	full = fgenerator.generate(nL);
		// initialise location states
	full->x.sub_range(0,nL) = x;
	full->X.sub_matrix(0,nL,0,nL) = X;
	full->init();
}

void Kalman_SLAM::predict( BF::Linrz_predict_model& lpred )
{
		// extract location part of full
	loc->x = full->x.sub_range(0,nL);
	loc->X = full->X.sub_matrix(0,nL,0,nL);
		// predict location, independant of map
	loc->init();
	loc->predict (lpred);
	loc->update();
		// return location to full
	full->x.sub_range(0,nL) = loc->x;
	full->X.sub_matrix(0,nL,0,nL) = loc->X;
	full->init();
}

void Kalman_SLAM::observe( unsigned feature, const Feature_observe& fom, const FM::Vec& z )
{
	// Assume features added sequenatialy
	if (feature >= nM) {
		error (BF::Logic_exception("Observe non existing feature"));
		return;
	}
	// TODO Implement nonlinear form
	// Create a augmented sparse observe model for full states
	BF::Linear_uncorrelated_observe_model fullm(full->x.size(), 1);
	fullm.Hx.clear();
	fullm.Hx.sub_matrix(0,nL, 0,nL) = fom.Hx.sub_matrix(0,nL, 0,nL);
	fullm.Hx(0,nL+feature) = fom.Hx(0,nL);
	fullm.Zv = fom.Zv;
	full->observe(fullm, z);
}

void Kalman_SLAM::observe_new( unsigned feature, const Feature_observe_inverse& fom, const FM::Vec& z )
// fom: must have a the special from required for SLAM::obeserve_new
{
		// size consistency, single state feature
	if (fom.Hx.size1() != 1)
		error (BF::Logic_exception("observation and model size inconsistent"));
		
		// make new filter with additional (uninitialized) feature state
	if (feature >= nM)
	{
		nM = feature+1;	
		Kalman_filter_generator::Filter_type* nf = fgenerator.generate(nL+nM);
		FM::noalias(nf->x.sub_range(0,full->x.size())) = full->x;
		FM::noalias(nf->X.sub_matrix(0,full->x.size(),0,full->x.size())) = full->X;

		fgenerator.dispose(full);
		full = nf;
	}
		// build augmented location and observation
	FM::Vec sz(nL+z.size());
	sz.sub_range(0,nL) = full->x.sub_range(0,nL);
	sz.sub_range(nL,nL+z.size() )= z;

	// TODO use named references rather then explict Ha Hb
	FM::Matrix Ha (fom.Hx.sub_matrix(0,1, 0,nL) );
	FM::Matrix Hb (fom.Hx.sub_matrix(0,1, nL,nL+z.size()) );
	FM::Matrix tempHa (1,nL);
	FM::Matrix tempHb (1,sz.size());

		// feature covariance with existing location and features
        // X+ = [0 Ha] X [0 Ha]' + Hb Z Hb'
        // - zero exisiting feature covariance
	zero( full->X.sub_matrix(0,full->X.size1(), nL+feature,nL+feature+1) );
	full->X.sub_matrix(nL+feature,nL+feature+1,0,nL+nM) = FM::prod(Ha,full->X.sub_matrix(0,nL, 0,nL+nM) );
		// feature state and variance
	full->x[nL+feature] = fom.h(sz)[0];
	full->X(nL+feature,nL+feature) = ( FM::prod_SPD(Ha,full->X.sub_matrix(0,nL, 0,nL),tempHa) +
													  FM::prod_SPD(Hb,fom.Zv,tempHb)
													 ) (0,0);
		
	full->init ();
}

void Kalman_SLAM::observe_new( unsigned feature, const FM::Float& t, const FM::Float& T )
{
		// Make space in scheme for feature, requires the scheme can deal with resized state
	if (feature >= nM)
	{
		Kalman_filter_generator::Filter_type* nf = fgenerator.generate(nL+feature+1);
		FM::noalias(nf->x.sub_range(0,full->x.size())) = full->x;
		FM::noalias(nf->X.sub_matrix(0,full->x.size(),0,full->x.size())) = full->X;
		zero( nf->X.sub_matrix(0,nf->X.size1(), nL+nM,nf->X.size2()) );

		nf->x[nL+feature] = t;
		nf->X(nL+feature,nL+feature) = T;
		nf->init ();
		fgenerator.dispose(full);
		full = nf;
		nM = feature+1;
	}
	else
	{
		full->x[nL+feature] = t;
		full->X(nL+feature,nL+feature) = T;
		full->init ();
	}
}

void Kalman_SLAM::forget( unsigned feature, bool must_exist )
{
	full->x[nL+feature] = 0.;
			// ISSUE uBLAS has problems accessing the lower symmetry via a sub_matrix proxy, there two two parts seperately
	zero( full->X.sub_matrix(0,nL+feature, nL+feature,nL+feature+1) );
	zero( full->X.sub_matrix(nL+feature,nL+feature+1, nL+feature,full->X.size1()) );
	full->init();
}

void Kalman_SLAM::decorrelate( Bayesian_filter::Bayes_base::Float d )
// Reduce correlation by scaling cross-correlation terms
{
	std::size_t i,j;
	const std::size_t n = full->X.size1();
	for (i = 1; i < n; ++i)
	{
		FM::SymMatrix::Row Xi(full->X,i);
		for (j = 0; j < i; ++j)
		{
			Xi[j] *= d;
		}
		for (j = i+1; j < n; ++j)
		{
			Xi[j] *= d;
		}
	}
	full->init();
}

}//namespace SLAM