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.

bayes_tracking_example screenshot

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;
    }
    catch (...)
    {
        printf("Untyped exception!!");
        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;
    }
}