34 #define BEARING_SENSOR_NOISE_STD DEG2RAD(15.0f)
35 #define RANGE_SENSOR_NOISE_STD 0.3f
36 #define DELTA_TIME 0.1f
38 #define VEHICLE_INITIAL_X 4.0f
39 #define VEHICLE_INITIAL_Y 4.0f
40 #define VEHICLE_INITIAL_V 1.0f
41 #define VEHICLE_INITIAL_W DEG2RAD(20.0f)
43 #define TRANSITION_MODEL_STD_XY 0.03f
44 #define TRANSITION_MODEL_STD_VXY 0.20f
46 #define NUM_PARTICLES 2000
64 double DeltaTime,
double observationRange,
double observationBearing);
66 void getState(KFVector& xkk, KFMatrix& pkk)
73 float m_obsBearing, m_obsRange;
83 void OnGetAction(KFArray_ACT& out_u)
const override;
94 void OnTransitionModel(
95 const KFArray_ACT& in_u, KFArray_VEH& inout_x,
96 bool& out_skipPrediction)
const override;
103 void OnTransitionJacobian(KFMatrix_VxV& out_F)
const override;
110 void OnTransitionNoise(KFMatrix_VxV& out_Q)
const override;
119 void OnGetObservationNoise(KFMatrix_OxO& out_R)
const override;
147 void OnGetObservationsAndDataAssociation(
148 vector_KFArray_OBS& out_z, std::vector<int>& out_data_association,
149 const vector_KFArray_OBS& in_all_predictions,
const KFMatrix& in_S,
150 const std::vector<size_t>& in_lm_indices_in_S,
151 const KFMatrix_OxO& in_R)
override;
160 void OnObservationModel(
161 const std::vector<size_t>& idx_landmarks_to_predict,
162 vector_KFArray_OBS& out_predictions)
const override;
173 void OnObservationJacobians(
174 size_t idx_landmark_to_predict, KFMatrix_OxV& Hx,
175 KFMatrix_OxF& Hy)
const override;
180 void OnSubstractObservationVectors(
181 KFArray_OBS&
A,
const KFArray_OBS& B)
const override;
198 CRangeBearingParticleFilter,
199 mrpt::bayes::CParticleFilterData<CParticleVehicleData>::CParticleList>
214 void prediction_and_update_pfStandardProposal(
220 void initializeParticles(
size_t numParticles);
224 void getMean(
float& x,
float& y,
float& vx,
float& vy);
237 winEKF.setPos(10, 10);
238 winPF.setPos(480, 10);
240 winEKF.axis(-2, 20, -10, 10);
242 winPF.axis(-2, 20, -10, 10);
268 "%%%% GT_X GT_Y EKF_MEAN_X EKF_MEAN_Y EKF_STD_X EKF_STD_Y\n");
288 float realBearing = atan2(y, x);
293 "Real/Simulated bearing: %.03f / %.03f deg\n",
RAD2DEG(realBearing),
301 printf(
"Real/Simulated range: %.03f / %.03f \n", realRange, obsRange);
309 CObservationBearingRange::Create();
310 obsRangeBear->sensedData.resize(1);
311 obsRangeBear->sensedData[0].range = obsRange;
312 obsRangeBear->sensedData[0].yaw = obsBearing;
326 "Real: x:%.03f y=%.03f heading=%.03f v=%.03f w=%.03f\n", x, y, phi,
328 cout <<
"EKF: " << EKF_xkk << endl;
331 cout <<
"Particle filter ESS: " << particles.
ESS() << endl;
335 COVXY(0, 0) = EKF_pkk(0, 0);
336 COVXY(1, 1) = EKF_pkk(1, 1);
337 COVXY(0, 1) = COVXY(1, 0) = EKF_pkk(0, 1);
340 EKF_xkk[0], EKF_xkk[1], COVXY, 3,
"b-2",
"ellipse_EKF");
346 "%f %f %f %f %f %f\n", x, y,
347 EKF_xkk[0], EKF_xkk[1], std::sqrt(EKF_pkk(0, 0)),
348 std::sqrt(EKF_pkk(1, 1)));
352 vector<float> vx(2), vy(2);
354 vx[1] = vx[0] + EKF_xkk[2] * 1;
356 vy[1] = vy[0] + EKF_xkk[3] * 1;
357 winEKF.plot(vx, vy,
"g-4",
"velocityEKF");
362 vector<float> parts_x(N), parts_y(N);
363 for (i = 0; i < N; i++)
369 winPF.plot(parts_x, parts_y,
"b.2",
"particles");
372 float avrg_x, avrg_y, avrg_vx, avrg_vy;
374 particles.
getMean(avrg_x, avrg_y, avrg_vx, avrg_vy);
376 vector<float> vx(2), vy(2);
378 vx[1] = vx[0] + avrg_vx * 1;
380 vy[1] = vy[0] + avrg_vy * 1;
381 winPF.plot(vx, vy,
"g-4",
"velocityPF");
385 winEKF.plot(vector<float>(1, x), vector<float>(1, y),
"k.8",
"plot_GT");
386 winPF.plot(vector<float>(1, x), vector<float>(1, y),
"k.8",
"plot_GT");
389 vector<float> obs_x(2), obs_y(2);
390 obs_x[0] = obs_y[0] = 0;
391 obs_x[1] = obsRange * cos(obsBearing);
392 obs_y[1] = obsRange * sin(obsBearing);
394 winEKF.plot(obs_x, obs_y,
"r",
"plot_obs_ray");
395 winPF.plot(obs_x, obs_y,
"r",
"plot_obs_ray");
398 std::this_thread::sleep_for(
399 std::chrono::milliseconds((
int)(
DELTA_TIME * 1000)));
414 catch (
const std::exception& e)
421 printf(
"Untyped exception!!");
439 m_pkk.setIdentity(4);
440 m_pkk(0, 0) = m_pkk(1, 1) =
square(5.0f);
441 m_pkk(2, 2) = m_pkk(3, 3) =
square(1.0f);
446 double DeltaTime,
double observationRange,
double observationBearing)
448 m_deltaTime = (float)DeltaTime;
449 m_obsBearing = (float)observationBearing;
450 m_obsRange = (float)observationRange;
452 runOneKalmanIteration();
469 const KFArray_ACT& in_u, KFArray_VEH& inout_x,
470 bool& out_skipPrediction)
const
474 inout_x[0] += in_u[0] * inout_x[2];
475 inout_x[1] += in_u[0] * inout_x[3];
487 F(0, 2) = m_deltaTime;
488 F(1, 3) = m_deltaTime;
516 vector_KFArray_OBS& out_z, std::vector<int>& out_data_association,
517 const vector_KFArray_OBS& in_all_predictions,
const KFMatrix& in_S,
518 const std::vector<size_t>& in_lm_indices_in_S,
const KFMatrix_OxO& in_R)
521 out_z[0][0] = m_obsBearing;
522 out_z[0][1] = m_obsRange;
524 out_data_association.clear();
535 const std::vector<size_t>& idx_landmarks_to_predict,
536 vector_KFArray_OBS& out_predictions)
const
542 kftype h_bear = atan2(y, x);
546 out_predictions.resize(1);
547 out_predictions[0][0] = h_bear;
548 out_predictions[0][1] = h_range;
560 size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy)
const
568 Hx(0, 1) = 1 / (x * (1 +
square(y / x)));
580 KFArray_OBS&
A,
const KFArray_OBS& B)
const
603 size_t i, N = m_particles.size();
606 for (i = 0; i < N; i++)
608 m_particles[i].d->x +=
612 m_particles[i].d->y +=
617 m_particles[i].d->vx +=
620 m_particles[i].d->vy +=
628 ASSERT_(obs->sensedData.size() == 1);
629 float obsRange = obs->sensedData[0].range;
630 float obsBearing = obs->sensedData[0].yaw;
633 for (i = 0; i < N; i++)
635 float predicted_range =
636 sqrt(
square(m_particles[i].d->x) +
square(m_particles[i].d->y));
637 float predicted_bearing =
638 atan2(m_particles[i].d->y, m_particles[i].d->x);
640 m_particles[i].log_w +=
654 m_particles.resize(M);
655 for (CParticleList::iterator it = m_particles.begin();
656 it != m_particles.end(); it++)
659 for (CParticleList::iterator it = m_particles.begin();
660 it != m_particles.end(); it++)
678 float& x,
float& y,
float& vx,
float& vy)
681 for (CParticleList::iterator it = m_particles.begin();
682 it != m_particles.end(); it++)
683 sumW += exp(it->log_w);
689 for (CParticleList::iterator it = m_particles.begin();
690 it != m_particles.end(); it++)
692 const double w = exp(it->log_w) / sumW;
694 x += (float)w * (*it).d->x;
695 y += (
float)w * (*it).d->y;
696 vx += (float)w * (*it).d->vx;
697 vy += (
float)w * (*it).d->vy;