[Bayes++] Usage question
Brought to you by:
mistevens
From: Rob O. <R.O...@No...> - 2004-01-31 03:35:01
|
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(); } |