/* +------------------------------------------------------------------------+
   |                     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 <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;

	/** @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 override;

	/** 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 override;

	/** 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 override;

	/** 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 override;

	/** 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 override;

	/** 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) override;

	/** 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 override;

	/** 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(
		size_t idx_landmark_to_predict, KFMatrix_OxV& Hx,
		KFMatrix_OxF& Hy) const override;

	/** 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 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:
	/** 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)
		override;

	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 = -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();
}

/** 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.setIdentity();

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

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