[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();

}