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.

Demo video: https://www.youtube.com/watch?v=0_gGXYzjcGE

The equations of this example and the theory behind them are explained in this tutorial.

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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::literals;  // _deg
using namespace mrpt;            // format(), square()
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.transpose() << 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);

      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;
  }
}