Example: bayes_tracking_example
This example illustrates how to implement an Extended Kalman Filter (EKF) and a particle filter (PF) using mrpt::bayes classes, for the problem of tracking a 2D mobile target with state space being its location and its velocity vector.
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ // ------------------------------------------------------ // Refer to the description in the wiki: // https://www.mrpt.org/Kalman_Filters // ------------------------------------------------------ #include <mrpt/bayes/CKalmanFilterCapable.h> #include <mrpt/bayes/CParticleFilterData.h> #include <mrpt/gui/CDisplayWindowPlots.h> #include <mrpt/math/distributions.h> #include <mrpt/math/wrap2pi.h> #include <mrpt/obs/CObservationBearingRange.h> #include <mrpt/obs/CSensoryFrame.h> #include <mrpt/random.h> #include <mrpt/system/os.h> #include <chrono> #include <iostream> #include <thread> using namespace mrpt; using namespace mrpt::bayes; using namespace mrpt::gui; using namespace mrpt::math; using namespace mrpt::obs; using namespace mrpt::random; using namespace std; #define BEARING_SENSOR_NOISE_STD DEG2RAD(15.0f) #define RANGE_SENSOR_NOISE_STD 0.3f #define DELTA_TIME 0.1f #define VEHICLE_INITIAL_X 4.0f #define VEHICLE_INITIAL_Y 4.0f #define VEHICLE_INITIAL_V 1.0f #define VEHICLE_INITIAL_W DEG2RAD(20.0f) #define TRANSITION_MODEL_STD_XY 0.03f #define TRANSITION_MODEL_STD_VXY 0.20f #define NUM_PARTICLES 2000 // Uncomment to save text files with grount truth vs. estimated states //#define SAVE_GT_LOGS // ------------------------------------------------------ // Implementation of the system models as a EKF // ------------------------------------------------------ class CRangeBearing : public mrpt::bayes::CKalmanFilterCapable< 4 /* x y vx vy*/, 2 /* range yaw */, 0, 1 /* Atime */> // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t // ACT_SIZE, size typename kftype = double> { public: CRangeBearing(); ~CRangeBearing() override; void doProcess( double DeltaTime, double observationRange, double observationBearing); void getState(KFVector& xkk, KFMatrix& pkk) { xkk = m_xkk; pkk = m_pkk; } protected: float m_obsBearing, m_obsRange; float m_deltaTime; void OnGetAction(KFArray_ACT& out_u) const override; void OnTransitionModel( const KFArray_ACT& in_u, KFArray_VEH& inout_x, bool& out_skipPrediction) const override; void OnTransitionJacobian(KFMatrix_VxV& out_F) const override; void OnTransitionNoise(KFMatrix_VxV& out_Q) const override; void OnGetObservationNoise(KFMatrix_OxO& out_R) const override; void OnGetObservationsAndDataAssociation( vector_KFArray_OBS& out_z, std::vector<int>& out_data_association, const vector_KFArray_OBS& in_all_predictions, const KFMatrix& in_S, const std::vector<size_t>& in_lm_indices_in_S, const KFMatrix_OxO& in_R) override; void OnObservationModel( const std::vector<size_t>& idx_landmarks_to_predict, vector_KFArray_OBS& out_predictions) const override; void OnObservationJacobians( size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy) const override; void OnSubstractObservationVectors( KFArray_OBS& A, const KFArray_OBS& B) const override; }; // --------------------------------------------------------------- // Implementation of the system models as a Particle Filter // --------------------------------------------------------------- struct CParticleVehicleData { float x, y, vx, vy; // Vehicle state (position & velocities) }; class CRangeBearingParticleFilter : public mrpt::bayes::CParticleFilterData<CParticleVehicleData>, public mrpt::bayes::CParticleFilterDataImpl< CRangeBearingParticleFilter, mrpt::bayes::CParticleFilterData<CParticleVehicleData>::CParticleList> { public: void prediction_and_update_pfStandardProposal( const mrpt::obs::CActionCollection* action, const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override; void initializeParticles(size_t numParticles); void getMean(float& x, float& y, float& vx, float& vy); }; // ------------------------------------------------------ // TestBayesianTracking // ------------------------------------------------------ void TestBayesianTracking() { getRandomGenerator().randomize(); CDisplayWindowPlots winEKF("Tracking - Extended Kalman Filter", 450, 400); CDisplayWindowPlots winPF("Tracking - Particle Filter", 450, 400); winEKF.setPos(10, 10); winPF.setPos(480, 10); winEKF.axis(-2, 20, -10, 10); winEKF.axis_equal(); winPF.axis(-2, 20, -10, 10); winPF.axis_equal(); // Create EKF // ---------------------- CRangeBearing EKF; EKF.KF_options.method = kfEKFNaive; EKF.KF_options.verbosity_level = mrpt::system::LVL_DEBUG; EKF.KF_options.enable_profiler = true; // Create PF // ---------------------- CParticleFilter::TParticleFilterOptions PF_options; PF_options.adaptiveSampleSize = false; PF_options.PF_algorithm = CParticleFilter::pfStandardProposal; PF_options.resamplingMethod = CParticleFilter::prSystematic; CRangeBearingParticleFilter particles; particles.initializeParticles(NUM_PARTICLES); CParticleFilter PF; PF.m_options = PF_options; #ifdef SAVE_GT_LOGS CFileOutputStream fo_log_ekf("log_GT_vs_EKF.txt"); fo_log_ekf.printf( "%%%% GT_X GT_Y EKF_MEAN_X EKF_MEAN_Y EKF_STD_X EKF_STD_Y\n"); #endif // Init. simulation: // ------------------------- float x = VEHICLE_INITIAL_X, y = VEHICLE_INITIAL_Y, phi = -180.0_deg, v = VEHICLE_INITIAL_V, w = VEHICLE_INITIAL_W; float t = 0; while (winEKF.isOpen() && winPF.isOpen() && !mrpt::system::os::kbhit()) { // Update vehicle: x += v * DELTA_TIME * (cos(phi) - sin(phi)); y += v * DELTA_TIME * (sin(phi) + cos(phi)); phi += w * DELTA_TIME; v += 1.0f * DELTA_TIME * cos(t); w -= 0.1f * DELTA_TIME * sin(t); // Simulate noisy observation: float realBearing = atan2(y, x); float obsBearing = realBearing + BEARING_SENSOR_NOISE_STD * getRandomGenerator().drawGaussian1D_normalized(); printf( "Real/Simulated bearing: %.03f / %.03f deg\n", RAD2DEG(realBearing), RAD2DEG(obsBearing)); float realRange = sqrt(square(x) + square(y)); float obsRange = max(0.0, realRange + RANGE_SENSOR_NOISE_STD * getRandomGenerator().drawGaussian1D_normalized()); printf("Real/Simulated range: %.03f / %.03f \n", realRange, obsRange); // Process with EKF: EKF.doProcess(DELTA_TIME, obsRange, obsBearing); // Process with PF: CSensoryFrame SF; CObservationBearingRange::Ptr obsRangeBear = CObservationBearingRange::Create(); obsRangeBear->sensedData.resize(1); obsRangeBear->sensedData[0].range = obsRange; obsRangeBear->sensedData[0].yaw = obsBearing; SF.insert(obsRangeBear); // memory freed by SF. EKF.getProfiler().enter("PF:complete_step"); PF.executeOn(particles, nullptr, &SF); // Process in the PF EKF.getProfiler().leave("PF:complete_step"); // Show EKF state: CRangeBearing::KFVector EKF_xkk; CRangeBearing::KFMatrix EKF_pkk; EKF.getState(EKF_xkk, EKF_pkk); printf( "Real: x:%.03f y=%.03f heading=%.03f v=%.03f w=%.03f\n", x, y, phi, v, w); cout << "EKF: " << EKF_xkk << endl; // Show PF state: cout << "Particle filter ESS: " << particles.ESS() << endl; // Draw EKF state: CRangeBearing::KFMatrix COVXY(2, 2); COVXY(0, 0) = EKF_pkk(0, 0); COVXY(1, 1) = EKF_pkk(1, 1); COVXY(0, 1) = COVXY(1, 0) = EKF_pkk(0, 1); winEKF.plotEllipse( EKF_xkk[0], EKF_xkk[1], COVXY, 3, "b-2", "ellipse_EKF"); // Save GT vs EKF state: #ifdef SAVE_GT_LOGS // %% GT_X GT_Y EKF_MEAN_X EKF_MEAN_Y EKF_STD_X EKF_STD_Y: fo_log_ekf.printf( "%f %f %f %f %f %f\n", x, y, // Real (GT) EKF_xkk[0], EKF_xkk[1], std::sqrt(EKF_pkk(0, 0)), std::sqrt(EKF_pkk(1, 1))); #endif // Draw the velocity vector: vector<float> vx(2), vy(2); vx[0] = EKF_xkk[0]; vx[1] = vx[0] + EKF_xkk[2] * 1; vy[0] = EKF_xkk[1]; vy[1] = vy[0] + EKF_xkk[3] * 1; winEKF.plot(vx, vy, "g-4", "velocityEKF"); // Draw PF state: { size_t i, N = particles.m_particles.size(); vector<float> parts_x(N), parts_y(N); for (i = 0; i < N; i++) { parts_x[i] = particles.m_particles[i].d->x; parts_y[i] = particles.m_particles[i].d->y; } winPF.plot(parts_x, parts_y, "b.2", "particles"); // Draw PF velocities: float avrg_x, avrg_y, avrg_vx, avrg_vy; particles.getMean(avrg_x, avrg_y, avrg_vx, avrg_vy); vector<float> vx(2), vy(2); vx[0] = avrg_x; vx[1] = vx[0] + avrg_vx * 1; vy[0] = avrg_y; vy[1] = vy[0] + avrg_vy * 1; winPF.plot(vx, vy, "g-4", "velocityPF"); } // Draw GT: winEKF.plot(vector<float>(1, x), vector<float>(1, y), "k.8", "plot_GT"); winPF.plot(vector<float>(1, x), vector<float>(1, y), "k.8", "plot_GT"); // Draw noisy observations: vector<float> obs_x(2), obs_y(2); obs_x[0] = obs_y[0] = 0; obs_x[1] = obsRange * cos(obsBearing); obs_y[1] = obsRange * sin(obsBearing); winEKF.plot(obs_x, obs_y, "r", "plot_obs_ray"); winPF.plot(obs_x, obs_y, "r", "plot_obs_ray"); // Delay: std::this_thread::sleep_for( std::chrono::milliseconds((int)(DELTA_TIME * 1000))); t += DELTA_TIME; } } // ------------------------------------------------------ // MAIN // ------------------------------------------------------ int main() { try { TestBayesianTracking(); return 0; } catch (const std::exception& e) { std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl; return -1; } } CRangeBearing::CRangeBearing() { // KF_options.method = kfEKFNaive; KF_options.method = kfEKFAlaDavison; // INIT KF STATE m_xkk.resize(4); // State: (x,y,heading,v,w) m_xkk[0] = VEHICLE_INITIAL_X; m_xkk[1] = VEHICLE_INITIAL_Y; m_xkk[2] = -VEHICLE_INITIAL_V; m_xkk[3] = 0; // Initial cov: Large uncertainty m_pkk.setIdentity(4); m_pkk(0, 0) = m_pkk(1, 1) = square(5.0f); m_pkk(2, 2) = m_pkk(3, 3) = square(1.0f); } CRangeBearing::~CRangeBearing() {} void CRangeBearing::doProcess( double DeltaTime, double observationRange, double observationBearing) { m_deltaTime = (float)DeltaTime; m_obsBearing = (float)observationBearing; m_obsRange = (float)observationRange; runOneKalmanIteration(); } void CRangeBearing::OnGetAction(KFArray_ACT& u) const { u[0] = m_deltaTime; } void CRangeBearing::OnTransitionModel( const KFArray_ACT& in_u, KFArray_VEH& inout_x, bool& out_skipPrediction) const { // in_u[0] : Delta time // in_out_x: [0]:x [1]:y [2]:vx [3]: vy inout_x[0] += in_u[0] * inout_x[2]; inout_x[1] += in_u[0] * inout_x[3]; } void CRangeBearing::OnTransitionJacobian(KFMatrix_VxV& F) const { F.setIdentity(); F(0, 2) = m_deltaTime; F(1, 3) = m_deltaTime; } void CRangeBearing::OnTransitionNoise(KFMatrix_VxV& Q) const { Q(0, 0) = Q(1, 1) = square(TRANSITION_MODEL_STD_XY); Q(2, 2) = Q(3, 3) = square(TRANSITION_MODEL_STD_VXY); } void CRangeBearing::OnGetObservationNoise(KFMatrix_OxO& R) const { R(0, 0) = square(BEARING_SENSOR_NOISE_STD); R(1, 1) = square(RANGE_SENSOR_NOISE_STD); } void CRangeBearing::OnGetObservationsAndDataAssociation( vector_KFArray_OBS& out_z, std::vector<int>& out_data_association, const vector_KFArray_OBS& in_all_predictions, const KFMatrix& in_S, const std::vector<size_t>& in_lm_indices_in_S, const KFMatrix_OxO& in_R) { out_z.resize(1); out_z[0][0] = m_obsBearing; out_z[0][1] = m_obsRange; out_data_association.clear(); // Not used } void CRangeBearing::OnObservationModel( const std::vector<size_t>& idx_landmarks_to_predict, vector_KFArray_OBS& out_predictions) const { // predicted bearing: kftype x = m_xkk[0]; kftype y = m_xkk[1]; kftype h_bear = atan2(y, x); kftype h_range = sqrt(square(x) + square(y)); // idx_landmarks_to_predict is ignored in NON-SLAM problems out_predictions.resize(1); out_predictions[0][0] = h_bear; out_predictions[0][1] = h_range; } void CRangeBearing::OnObservationJacobians( size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy) const { // predicted bearing: kftype x = m_xkk[0]; kftype y = m_xkk[1]; Hx.setZero(); Hx(0, 0) = -y / (square(x) + square(y)); Hx(0, 1) = 1 / (x * (1 + square(y / x))); Hx(1, 0) = x / sqrt(square(x) + square(y)); Hx(1, 1) = y / sqrt(square(x) + square(y)); // Hy: Not used } void CRangeBearing::OnSubstractObservationVectors( KFArray_OBS& A, const KFArray_OBS& B) const { A -= B; math::wrapToPiInPlace(A[0]); // The angular component } void CRangeBearingParticleFilter::prediction_and_update_pfStandardProposal( const mrpt::obs::CActionCollection* action, const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options) { size_t i, N = m_particles.size(); // Transition model: for (i = 0; i < N; i++) { m_particles[i].d->x += DELTA_TIME * m_particles[i].d->vx + TRANSITION_MODEL_STD_XY * getRandomGenerator().drawGaussian1D_normalized(); m_particles[i].d->y += DELTA_TIME * m_particles[i].d->vy + TRANSITION_MODEL_STD_XY * getRandomGenerator().drawGaussian1D_normalized(); m_particles[i].d->vx += TRANSITION_MODEL_STD_VXY * getRandomGenerator().drawGaussian1D_normalized(); m_particles[i].d->vy += TRANSITION_MODEL_STD_VXY * getRandomGenerator().drawGaussian1D_normalized(); } CObservationBearingRange::Ptr obs = observation->getObservationByClass<CObservationBearingRange>(); ASSERT_(obs); ASSERT_(obs->sensedData.size() == 1); float obsRange = obs->sensedData[0].range; float obsBearing = obs->sensedData[0].yaw; // Update weights for (i = 0; i < N; i++) { float predicted_range = sqrt(square(m_particles[i].d->x) + square(m_particles[i].d->y)); float predicted_bearing = atan2(m_particles[i].d->y, m_particles[i].d->x); m_particles[i].log_w += log(math::normalPDF( predicted_range - obsRange, 0, RANGE_SENSOR_NOISE_STD)) + log(math::normalPDF( math::wrapToPi(predicted_bearing - obsBearing), 0, BEARING_SENSOR_NOISE_STD)); } // Resample is automatically performed by CParticleFilter when required. } void CRangeBearingParticleFilter::initializeParticles(size_t M) { clearParticles(); m_particles.resize(M); for (CParticleList::iterator it = m_particles.begin(); it != m_particles.end(); it++) it->d.reset(new CParticleVehicleData()); for (CParticleList::iterator it = m_particles.begin(); it != m_particles.end(); it++) { (*it).d->x = getRandomGenerator().drawUniform( VEHICLE_INITIAL_X - 2.0f, VEHICLE_INITIAL_X + 2.0f); (*it).d->y = getRandomGenerator().drawUniform( VEHICLE_INITIAL_Y - 2.0f, VEHICLE_INITIAL_Y + 2.0f); (*it).d->vx = getRandomGenerator().drawGaussian1D(-VEHICLE_INITIAL_V, 0.2f); (*it).d->vy = getRandomGenerator().drawGaussian1D(0, 0.2f); it->log_w = 0; } } void CRangeBearingParticleFilter::getMean( float& x, float& y, float& vx, float& vy) { double sumW = 0; for (CParticleList::iterator it = m_particles.begin(); it != m_particles.end(); it++) sumW += exp(it->log_w); ASSERT_(sumW > 0); x = y = vx = vy = 0; for (CParticleList::iterator it = m_particles.begin(); it != m_particles.end(); it++) { const double w = exp(it->log_w) / sumW; x += (float)w * (*it).d->x; y += (float)w * (*it).d->y; vx += (float)w * (*it).d->vx; vy += (float)w * (*it).d->vy; } }