test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
// ------------------------------------------------------
//  Refer to the description in the wiki:
//  http://www.mrpt.org/Kalman_Filters
// ------------------------------------------------------
 
#include <mrpt/bayes/CKalmanFilterCapable.h>
#include <mrpt/bayes/CParticleFilterData.h>
 
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/random.h>
#include <mrpt/system/os.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/math/distributions.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <iostream>
 
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();
        virtual ~CRangeBearing();
 
        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;
 
        /** @name Virtual methods for Kalman Filter implementation
                @{
         */
 
        /** Must return the action vector u.
         * \param out_u The action vector which will be passed to OnTransitionModel
         */
        void OnGetAction(KFArray_ACT& out_u) const;
 
        /** Implements the transition model \f$ \hat{x}_{k|k-1} = f(
         * \hat{x}_{k-1|k-1}, u_k ) \f$
         * \param in_u The vector returned by OnGetAction.
         * \param inout_x At input has \f$ \hat{x}_{k-1|k-1} \f$, at output must
         * have \f$ \hat{x}_{k|k-1} \f$.
         * \param out_skip Set this to true if for some reason you want to skip the
         * prediction step (to do not modify either the vector or the covariance).
         * Default:false
         */
        void OnTransitionModel(
                const KFArray_ACT& in_u, KFArray_VEH& inout_x,
                bool& out_skipPrediction) const;
 
        /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
         * \param out_F Must return the Jacobian.
         *  The returned matrix must be \f$N \times N\f$ with N being either the
         * size of the whole state vector or get_vehicle_size().
         */
        void OnTransitionJacobian(KFMatrix_VxV& out_F) const;
 
        /** Implements the transition noise covariance \f$ Q_k \f$
         * \param out_Q Must return the covariance matrix.
         *  The returned matrix must be of the same size than the jacobian from
         * OnTransitionJacobian
         */
        void OnTransitionNoise(KFMatrix_VxV& out_Q) const;
 
        /** Return the observation NOISE covariance matrix, that is, the model of
         * the Gaussian additive noise of the sensor.
         * \param out_R The noise covariance matrix. It might be non diagonal, but
         * it'll usually be.
         * \note Upon call, it can be assumed that the previous contents of out_R
         * are all zeros.
         */
        void OnGetObservationNoise(KFMatrix_OxO& out_R) const;
 
        /** This is called between the KF prediction step and the update step, and
         * the application must return the observations and, when applicable, the
         * data association between these observations and the current map.
         *
         * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
         * being the number of "observations": how many observed landmarks for a
         * map, or just one if not applicable.
         * \param out_data_association An empty vector or, where applicable, a
         * vector where the i'th element corresponds to the position of the
         * observation in the i'th row of out_z within the system state vector (in
         * the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map
         * element and we want to insert it at the end of this KF iteration.
         * \param in_all_predictions A vector with the prediction of ALL the
         * landmarks in the map. Note that, in contrast, in_S only comprises a
         * subset of all the landmarks.
         * \param in_S The full covariance matrix of the observation predictions
         * (i.e. the "innovation covariance matrix"). This is a M·O x M·O matrix
         * with M=length of "in_lm_indices_in_S".
         * \param in_lm_indices_in_S The indices of the map landmarks (range
         * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix
         * in_S.
         *
         *  This method will be called just once for each complete KF iteration.
         * \note It is assumed that the observations are independent, i.e. there
         * are NO cross-covariances between them.
         */
        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);
 
        /** Implements the observation prediction \f$ h_i(x) \f$.
         * \param idx_landmark_to_predict The indices of the landmarks in the map
         * whose predictions are expected as output. For non SLAM-like problems,
         * this input value is undefined and the application should just generate
         * one observation for the given problem.
         * \param out_predictions The predicted observations.
         */
        void OnObservationModel(
                const std::vector<size_t>& idx_landmarks_to_predict,
                vector_KFArray_OBS& out_predictions) const;
 
        /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x}
         * \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
         * \param idx_landmark_to_predict The index of the landmark in the map
         * whose prediction is expected as output. For non SLAM-like problems, this
         * will be zero and the expected output is for the whole state vector.
         * \param Hx  The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
         * \param Hy  The output Jacobian \f$ \frac{\partial h_i}{\partial y_i}
         * \f$.
         */
        void OnObservationJacobians(
                const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
                KFMatrix_OxF& Hy) const;
 
        /** Computes A=A-B, which may need to be re-implemented depending on the
         * topology of the individual scalar components (eg, angles).
         */
        void OnSubstractObservationVectors(
                KFArray_OBS& A, const KFArray_OBS& B) const;
 
        /** @}
         */
};
 
// ---------------------------------------------------------------
//              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:
        /** Update the m_particles, predicting the posterior of robot pose and map
         * after a movement command.
         *  This method has additional configuration parameters in "options".
         *  Performs the update stage of the RBPF, using the sensed Sensorial Frame:
         *
         *   \param action This is a pointer to CActionCollection, containing the
         * pose change the robot has been commanded.
         *   \param observation This must be a pointer to a CSensoryFrame object,
         * with robot sensed observations.
         *
         * \sa options
         */
        void prediction_and_update_pfStandardProposal(
                const mrpt::obs::CActionCollection* action,
                const mrpt::obs::CSensoryFrame* observation,
                const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
 
        void initializeParticles(size_t numParticles);
 
        /** Computes the average velocity & position
         */
        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 = DEG2RAD(-180),
                  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 =
                        mrpt::make_aligned_shared<CObservationBearingRange>();
                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 (std::exception& e)
        {
                std::cout << "MRPT exception caught: " << e.what() << 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.setSize(4, 4);
        m_pkk.unit();
        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();
}
 
/** Must return the action vector u.
 * \param out_u The action vector which will be passed to OnTransitionModel
 */
void CRangeBearing::OnGetAction(KFArray_ACT& u) const { u[0] = m_deltaTime; }
/** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1},
 * u_k ) \f$
 * \param in_u The vector returned by OnGetAction.
 * \param inout_x At input has \f$ \hat{x}_{k-1|k-1} \f$, at output must have
 * \f$ \hat{x}_{k|k-1} \f$.
 * \param out_skip Set this to true if for some reason you want to skip the
 * prediction step (to do not modify either the vector or the covariance).
 * Default:false
 */
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];
}
 
/** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
 * \param out_F Must return the Jacobian.
 *  The returned matrix must be \f$N \times N\f$ with N being either the size
 * of the whole state vector or get_vehicle_size().
 */
void CRangeBearing::OnTransitionJacobian(KFMatrix_VxV& F) const
{
        F.unit();
 
        F(0, 2) = m_deltaTime;
        F(1, 3) = m_deltaTime;
}
 
/** Implements the transition noise covariance \f$ Q_k \f$
 * \param out_Q Must return the covariance matrix.
 *  The returned matrix must be of the same size than the jacobian from
 * OnTransitionJacobian
 */
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);
}
 
/** Return the observation NOISE covariance matrix, that is, the model of the
 * Gaussian additive noise of the sensor.
 * \param out_R The noise covariance matrix. It might be non diagonal, but it'll
 * usually be.
 * \note Upon call, it can be assumed that the previous contents of out_R are
 * all zeros.
 */
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
}
 
/** Implements the observation prediction \f$ h_i(x) \f$.
 * \param idx_landmark_to_predict The indices of the landmarks in the map whose
 * predictions are expected as output. For non SLAM-like problems, this input
 * value is undefined and the application should just generate one observation
 * for the given problem.
 * \param out_predictions The predicted observations.
 */
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;
}
 
/** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$
 * and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
 * \param idx_landmark_to_predict The index of the landmark in the map whose
 * prediction is expected as output. For non SLAM-like problems, this will be
 * zero and the expected output is for the whole state vector.
 * \param Hx  The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
 * \param Hy  The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
 */
void CRangeBearing::OnObservationJacobians(
        const 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.zeros();
        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
}
 
/** Computes A=A-B, which may need to be re-implemented depending on the
 * topology of the individual scalar components (eg, angles).
 */
void CRangeBearing::OnSubstractObservationVectors(
        KFArray_OBS& A, const KFArray_OBS& B) const
{
        A -= B;
        math::wrapToPiInPlace(A[0]);  // The angular component
}
 
/** Update the m_particles, predicting the posterior of robot pose and map after
 * a movement command.
 *  This method has additional configuration parameters in "options".
 *  Performs the update stage of the RBPF, using the sensed Sensorial Frame:
 *
 *   \param action This is a pointer to CActionCollection, containing the pose
 * change the robot has been commanded.
 *   \param observation This must be a pointer to a CSensoryFrame object, with
 * robot sensed observations.
 *
 * \sa options
 */
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;
        }
}
 
/** Computes the average velocity
 */
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;
        }
}