bayesclasses-general

[Bayes++] Usage question

[Rob O. <R.O...@No...>]

Hello,
=20
First of all I thank mr.stevens for this fine library. Now my question:
I try to transform the PV-sample into a 'tracker' that tracks 2D points.
If I implement the tracker using two 1D-filters like in the sample it
works fine. But if I use only one 2D-filter my tracker works fine for
the x-coordinate, but not at all for the y-coordinate ( the velocity in
the y-direction quickly becomes almost zero). I fail to see what I do
wrong here. Here follows the code I used:
=20
The header:
=20
#ifndef NECACQPOSITIONTRACKER_H
#define NECACQPOSITIONTRACKER_H

#pragma once

namespace

{

class NECAcqPositionTrackerImpl;

}

#include "boost\tuple\tuple.hpp"

typedef boost::tuple<double, double> PositionType;

typedef boost::tuple<double, double> VelocityType;

typedef boost::tuple<PositionType, VelocityType> StateType;

class NECAcqPositionTracker

{

public:

NECAcqPositionTracker();

virtual ~NECAcqPositionTracker();

virtual void Observe(PositionType position);

virtual void Predict();

virtual void Update();

StateType GetState() const;

private:

NECAcqPositionTrackerImpl* m_pImpl;

};

#endif //NECACQPOSITIONTRACKER_H

The .cpp :

=20

#include ".\necacqpositiontracker.h"

#include "\programming\bayes++\BayesFilter/unsFlt.hpp"

#include "\programming\bayes++\BayesFilter/UDFlt.hpp"

#include "\programming\bayes++\Test/random.hpp"

namespace=20

{

using namespace Bayesian_filter;

using namespace Bayesian_filter_matrix;

const Float m_v_noise =3D 0.1; // Velocity noise, giving mean squared
error bound

// Filter's Initial state uncertainty: System state is unknown

const Float i_P_NOISE =3D 100.;

const Float i_V_NOISE =3D 1.0;

=20

class PVpredict : public Linear_predict_model

{

public:

PVpredict();

private:

const Float m_dt;

const Float m_v_gamma; // Velocity correlation, giving velocity change
time constant

};

class PVobserve : public Linrz_uncorrelated_observe_model

{

mutable Vec z_pred;

public:

PVobserve ();

const Vec& h(const Vec& x) const;

// Noise on observing system state

const Float m_fNoise;

};

=20

class NECAcqPositionTrackerImpl

{

// Filtering Scheme to use

typedef UD_scheme FilterScheme;

public:

NECAcqPositionTrackerImpl();

virtual ~NECAcqPositionTrackerImpl();

void Observe(PositionType position);

void Predict();

void Update();

StateType GetState() const;

private:

PVpredict m_predictor;

FilterScheme m_filterScheme;

PVobserve m_observation;

Vec m_z_true;

};

PVpredict::PVpredict()=20

: Linear_predict_model(4, 2),

m_dt(0.04),

m_v_gamma(1.0)

{

// Position Velocity dependance

Fx(0,0) =3D 1.;

Fx(0,1) =3D m_dt;

Fx(1,0) =3D 0.;

Fx(1,1) =3D exp(-m_dt*m_v_gamma);

Fx(2,0) =3D 0.;

Fx(2,1) =3D 0.;

Fx(3,0) =3D 0.;

Fx(3,1) =3D 0.;

Fx(0,2) =3D 0.;

Fx(1,2) =3D 0.;

Fx(0,3) =3D 0.;

Fx(1,3) =3D 0.;

=20

Fx(2,2) =3D 1.;

Fx(2,3) =3D m_dt;

Fx(3,2) =3D 0.;

Fx(3,3) =3D exp(-m_dt*m_v_gamma);

q[0] =3D q[1] =3D 0.0001;

FM::identity (G);

}

PVobserve::PVobserve ()=20

: Linrz_uncorrelated_observe_model(4, 2),=20

z_pred(2),

m_fNoise(0.001)

{

// Linear model

Hx(0,0) =3D 1;

Hx(0,1) =3D 0.;

Hx(0,2) =3D 0.;

Hx(0,3) =3D 0.;

Hx(1,0) =3D 0.;

Hx(1,1) =3D 0.;

Hx(1,2) =3D 1.;

Hx(1,3) =3D 0.;

// Observation Noise variance

Zv[0] =3D pow(m_fNoise, 2);

Zv[1] =3D pow(m_fNoise, 2);

}

const Vec& PVobserve::h(const Vec& x) const

{

z_pred[0] =3D x[0];

z_pred[1] =3D x[2];

return z_pred;

};

NECAcqPositionTrackerImpl::NECAcqPositionTrackerImpl()

: m_filterScheme(4,4),

m_z_true(2)

{

Vec state_guess(4);

state_guess[0] =3D 78.;

state_guess[1] =3D 1.5;

state_guess[2] =3D 220.;

state_guess[3] =3D -1.5;

m_filterScheme.X.clear();

m_filterScheme.X(0,0) =3D pow(i_P_NOISE, 2);

m_filterScheme.X(1,1) =3D pow(i_V_NOISE, 2);

m_filterScheme.X(2,2) =3D pow(i_P_NOISE, 2);

m_filterScheme.X(3,3) =3D pow(i_V_NOISE, 2);

m_filterScheme.init_kalman(state_guess, m_filterScheme.X);

}

NECAcqPositionTrackerImpl::~NECAcqPositionTrackerImpl()

{

}

void NECAcqPositionTrackerImpl::Observe(PositionType position)

{

m_z_true[0] =3D position.get<0>();

m_z_true[1] =3D position.get<1>();

m_filterScheme.observe(m_observation, m_z_true);

}

void NECAcqPositionTrackerImpl::Predict()

{

m_filterScheme.predict(m_predictor);

}

void NECAcqPositionTrackerImpl::Update()

{

m_filterScheme.update();

}

StateType NECAcqPositionTrackerImpl::GetState() const

{

using boost::make_tuple;

return make_tuple<PositionType, VelocityType>( make_tuple<double,
double>( m_filterScheme.x[0], m_filterScheme.x[2] ),=20

make_tuple<double, double>( m_filterScheme.x[1], m_filterScheme.x[3] )
);

}

}

=20

NECAcqPositionTracker::NECAcqPositionTracker()

{

m_pImpl =3D new NECAcqPositionTrackerImpl();

}

NECAcqPositionTracker::~NECAcqPositionTracker()

{

delete m_pImpl;

}

void NECAcqPositionTracker::Observe(PositionType position)

{

m_pImpl->Observe(position);

}

void NECAcqPositionTracker::Predict()

{

m_pImpl->Predict();

}

void NECAcqPositionTracker::Update()

{

m_pImpl->Update();

}

StateType NECAcqPositionTracker::GetState() const

{

return m_pImpl->GetState();

}

Re: [Bayes++] Usage question

[Michael S. <Mic...@ep...>]

Rob Ottenhoff wrote:
> Hello,
>  
> First of all I thank mr.stevens for this fine library. Now my question:
> I try to transform the PV-sample into a 'tracker' that tracks 2D points.
> If I implement the tracker using two 1D-filters like in the sample it 
> works fine. But if I use only one 2D-filter my tracker works fine for 
> the x-coordinate, but not at all for the y-coordinate ( the velocity in 
> the y-direction quickly becomes almost zero). I fail to see what I do 
> wrong here. Here follows the code I used:
>  

Hello Rob,

I have had a quick look at your problem. Firstly you are correct in
suspecting a coding error. In this special case the solution with one 2D
filters should give identical results as the case with two 1D filters.

Looking at your code the mistake seems to be in your noise coupling
matrix G.
After the identity(G) it will have the value
1 0
0 1
0 0
0 0

I think what you want is
1 0
0 0
0 1
0 0

That way the noise is correctly added to the X position and Y position
states. I think that is all that is wrong. Can you tell me if that
doesn't work.


Of course the problem could be you have backslashes instead of forward
slashes in your #included filenames!! :-)
Slightly more seriously, the 'b' in Bayes++ is always capitalised in my
distributions. So unless you have a case agnostic OS your code will no
be portable.

All the best,
	Michael

-- 
___________________________________
Michael Stevens Systems Engineering
Lilienweg 13a
34128 Kassel, Germany

Phone/Fax: +49 561 5218038
___________________________________