/
/
/
1
2#include "fast_slam/Particle.hpp"
3
4namespace fastslam{
5
6
7Particle::Particle(StateVector initial_state, StateMatrix initial_covariance, uint32_t iteration):
8 // M(3,0), K(1), kt(0), w_DA(0.), dAindices(), lc(false) // default Constructor definition
9 current_iteration_(iteration),
10 weight_(1),
11 weight_distance_(0.0),
12 loop_is_closed_(false),
13 particle_covariance_(initial_covariance),
14 spatial_index_(nullptr),
15 spatial_index_dirty_(true){
16
17 // make path and map
18 path_ = std::make_unique<Path>(initial_state, iteration);
19 map_ = std::make_unique<MapTree>();
20}
21
22Particle::Particle(const Particle &particle_to_copy){
23
24 path_ = std::make_unique<Path>(*(particle_to_copy.path_));
25 map_ = std::make_unique<MapTree>(*(particle_to_copy.map_));
26 weight_ = particle_to_copy.weight_;
27 weight_distance_ = particle_to_copy.weight_distance_;
28 loop_is_closed_ = particle_to_copy.loop_is_closed_;
29
30 slow_init_ = particle_to_copy.slow_init_;
31 use_motion_model_jacobian_ = particle_to_copy.use_motion_model_jacobian_;
32 always_reset_particle_covariance_ = particle_to_copy.always_reset_particle_covariance_;
33 use_numerical_stabilized_kalman_filters_ = particle_to_copy.use_numerical_stabilized_kalman_filters_;
34 force_covariance_symmetry_ = particle_to_copy.force_covariance_symmetry_;
35 add_landmarks_after_resampling_ = particle_to_copy.add_landmarks_after_resampling_;
36
37 particle_covariance_ = particle_to_copy.particle_covariance_;
38
39 // Spatial index will be rebuilt on demand - don't copy the index itself
40 spatial_index_ = nullptr;
41 spatial_index_dirty_ = true;
42}
43
44Particle::~Particle(){}
45
46void Particle::updateSpatialIndex() const {
47 if (!spatial_index_dirty_ && spatial_index_) {
48 return; // Index is already up to date
49 }
50
51 if (map_->getNLandmarks() == 0) {
52 spatial_index_.reset();
53 spatial_index_dirty_ = false;
54 return;
55 }
56
57 // Build landmark position matrix for spatial indexing
58 Eigen::MatrixXd landmark_positions(2, map_->getNLandmarks());
59 for(uint32_t i = 1; i <= map_->getNLandmarks(); i++){
60 auto landmark_ptr = map_->extractLandmarkNodePointer(i);
61 if (landmark_ptr != nullptr){
62 landmark_positions.col(i-1) = landmark_ptr->landmark_pose;
63 }
64 }
65
66 // Create new spatial index - properly manages memory with smart pointer
67 spatial_index_.reset(Nabo::NNSearchD::createKDTreeLinearHeap(landmark_positions));
68 spatial_index_dirty_ = false;
69}
70
71std::vector<std::shared_ptr<Landmark>> Particle::getMap(){
72 std::vector<std::shared_ptr<Landmark>> out;
73
74 for(uint32_t i = 1; i<=map_->getNLandmarks(); i++)
75 out.push_back(map_->extractLandmarkNodePointer(i));
76
77 return out;
78}
79
80void Particle::updateParticle(std::shared_ptr<MeasurementSet> measurement_set, StateVectorDerivative* state_dot, uint32_t iteration, std::chrono::nanoseconds delta_time, bool loop_closure){
81
82 loop_is_closed_ = loop_closure;
83
84 StateVector state_proposal;
85 StateVector state_old = path_->getPose();
86 current_iteration_ = iteration;
87
88 MeasurementSet measurement_set_new;
89 MeasurementSet measurement_set_existing;
90
91 std::vector<int> id_new;
92 std::vector<int> id_existing;
93
94 StateVector state_propagated = motionModel(state_old, *state_dot, delta_time);
95
96
97 associateData(state_propagated, measurement_set, measurement_set_existing, measurement_set_new, id_existing, id_new);
98
99
100
101 StateMatrix Fs, Fw;
102 if(use_motion_model_jacobian_){
103 Fs = calculateFs(state_old, *state_dot, delta_time);
104 Fw = calculateFw(state_old, *state_dot, delta_time);
105 }
106 else{
107 Fs = StateMatrix::Identity();
108 Fw = StateMatrix::Identity();
109 }
110
111
112 state_proposal = drawSampleFromProposaleDistribution(state_propagated, Fs, Fw, measurement_set_existing, id_existing);
113
114 // we are done estimating our pose and add it to the path!
115 path_->addPose(state_proposal, current_iteration_, delta_time);
116
117 calculateImportanceWeight(measurement_set_existing, state_proposal, Fw, id_existing);
118
119 handleExMeas(measurement_set_existing, state_proposal, id_existing);
120
121 handleNewMeas(measurement_set_new, state_proposal, id_new);
122
123
124
125 // we are done estimating our pose and add it to the path!
126
127
128 // handleExMeas(measurement_set_existing, state_propagated, id_existing);
129
130 // handleNewMeas(measurement_set_new, state_propagated, id_new);
131
132 // calculateImportanceWeight(measurement_set_existing, state_propagated, Fw, id_existing);
133
134 // state_proposal = drawSampleFromProposaleDistribution(state_propagated, Fs, Fw, measurement_set_existing, id_existing);
135
136 // path_->addPose(state_proposal, current_iteration_, delta_time);
137
138 std::cout << "state old: " << state_old.transpose() << "\n";
139 std::cout << "state propagated: " << state_propagated.transpose() << "\n";
140 std::cout << "state proposed: " << state_proposal.transpose() << "\n";
141 std::cout << "number of existing measurements: " << measurement_set_existing.getNumberOfMeasurements() << " number of new: " << measurement_set_new.getNumberOfMeasurements() << " number of landmarks: " << map_->getNLandmarks() << "\n";
142 std::cout << "Particle weight: " << getWeight() << "\n\n";
143}
144
145void Particle::associateData(StateVector &state_propagated, std::shared_ptr<MeasurementSet> &measurement_set, MeasurementSet &measurement_set_existing, MeasurementSet &measurement_set_new, std::vector<int> &id_existing, std::vector<int> &id_new){
146 // if no landmarks exist in map add all as new
147
148 if(map_->getNLandmarks() == 0){
149 std::unique_ptr<MeasurementSetNode> *curr;
150 if(measurement_set != nullptr)
151 curr = measurement_set->getHeadPointer();
152
153 for(size_t i=1; i<=measurement_set->getNumberOfMeasurements(); i++){
154 id_new.push_back(i);
155 measurement_set_new.addMeasurement((*curr)->measurement);
156 curr = &(*curr)->next_node;
157 }
158 return;
159 }
160
161
162 Eigen::MatrixXd map_landmark_matrix(2,0);
163 Eigen::MatrixXd landmark_calculated(2,0);
164 Eigen::MatrixXd measurement_calculated(2,0);
165
166 //Extract Landmarks from map
167 //TODO: not necessary, integrate kd tree directly
168 for(size_t i=1; i<= map_->getNLandmarks(); i++){
169 auto landmark_ptr = map_->extractLandmarkNodePointer(i);
170 if (landmark_ptr != NULL){
171 map_landmark_matrix.conservativeResize(map_landmark_matrix.rows(), map_landmark_matrix.cols()+1);
172 map_landmark_matrix.col(map_landmark_matrix.cols()-1) = landmark_ptr->landmark_pose;
173 }
174 }
175
176 //Extract measurements from the set.
177 std::unique_ptr<MeasurementSetNode> *curr;
178 if(measurement_set != nullptr)
179 curr = measurement_set->getHeadPointer();
180
181 // if we have a non-emtpy measurement set update the particles, otherwise just do resampling
182 while(*curr != nullptr){
183 landmark_calculated.conservativeResize(landmark_calculated.rows(),landmark_calculated.cols()+1);
184 landmark_calculated.col(landmark_calculated.cols()-1) = (*curr)->measurement->inverseMeasurementModel(state_propagated);
185
186 curr = &(*curr)->next_node;
187 }
188
189
190 Eigen::MatrixXi nn_indices(1, landmark_calculated.cols());
191 Eigen::MatrixXd nn_distances(1, landmark_calculated.cols());
192
193 // Use persistent spatial index for efficient nearest neighbor search
194 updateSpatialIndex();
195
196 if (spatial_index_) {
197 int n_nearest_neighbors = 1;
198 spatial_index_->knn(landmark_calculated, nn_indices, nn_distances, n_nearest_neighbors, 0, Nabo::NNSearchD::ALLOW_SELF_MATCH, 1.0);
199 } else {
200 // No landmarks in map, all distances are infinity
201 nn_distances.setConstant(std::numeric_limits<double>::infinity());
202 }
203
204 curr = measurement_set->getHeadPointer();
205
206 //add measurements to the sets of existing and new measurements
207 int j = 1;
208 for(int i=0; i<nn_distances.cols(); i++){
209 if(nn_distances(i) < 0.8 || loop_is_closed_){
210 id_existing.push_back(nn_indices(i)+1);
211 measurement_set_existing.addMeasurement((*curr)->measurement);
212 }
213 else{
214 id_new.push_back(map_landmark_matrix.cols()+j);
215 j++;
216 measurement_set_new.addMeasurement((*curr)->measurement);
217 }
218 curr = &(*curr)->next_node;
219 }
220}
221
222void Particle::handleExMeas(MeasurementSet &measurent_set_existing, StateVector &state_proposal, std::vector<int> &id_existing){
223 if (measurent_set_existing.getNumberOfMeasurements() == 0 )
224 return;
225
226
227 for(size_t i = 1; i <= measurent_set_existing.getNumberOfMeasurements(); i++) {
228
229 auto current_measurement = measurent_set_existing.getMeasurement(i);
230
231 auto landmark_old = map_->extractLandmarkNodePointer(id_existing.at(i-1));
232
233 Eigen::VectorXd measurement_calculated = current_measurement->MeasurementModel(state_proposal, landmark_old->landmark_pose);
234 Eigen::VectorXd measurement_difference = current_measurement->getMeasurement() - measurement_calculated;
235
236 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_proposal, landmark_old->landmark_pose);
237
238
239 auto landmark_update = std::make_shared<Landmark>();
240 if(use_numerical_stabilized_kalman_filters_){
241 Eigen::VectorXd x = landmark_old->landmark_pose;
242 Eigen::MatrixXd P = landmark_old->landmark_covariance;
243 Eigen::MatrixXd H = landmark_jacobian;
244 Eigen::MatrixXd R = current_measurement->getzCov();
245 Eigen::VectorXd v = measurement_difference;
246
247 KFCholeskyUpdate(x, P, v, R, H);
248
249 landmark_update->landmark_identifier = id_existing.at(i-1);
250 landmark_update->landmark_pose = x;
251 landmark_update->landmark_covariance = P;
252 }
253 else{
254 // innovation covariance
255 Eigen::MatrixXd Zk = current_measurement->getzCov() + landmark_jacobian * landmark_old->landmark_covariance * landmark_jacobian.transpose();
256 // kalman gain
257 Eigen::MatrixXd Kk = landmark_old->landmark_covariance * landmark_jacobian.transpose() * Zk.inverse();
258
259 landmark_update->landmark_identifier = id_existing.at(i-1);
260 landmark_update->landmark_pose += Kk*(current_measurement->getMeasurement() - measurement_calculated);
261
262 Eigen::MatrixXd tmp_matrix = Kk * landmark_jacobian;
263 landmark_update->landmark_covariance = (Eigen::MatrixXd::Identity(tmp_matrix.rows(),tmp_matrix.cols()) - tmp_matrix) * landmark_update->landmark_covariance;
264 }
265
266 //make symmetric
267 if(force_covariance_symmetry_)
268 landmark_update->landmark_covariance = (landmark_update->landmark_covariance + landmark_update->landmark_covariance.transpose()) * 0.5;
269
270
271 if (landmark_update->landmark_pose != landmark_update->landmark_pose) {
272 cout << "landmark update NaN err, ID: " << current_measurement->getC() << endl;
273 }
274
275 map_->correctLandmark(landmark_update);
276 spatial_index_dirty_ = true; // Mark spatial index as needing update
277 }
278}
279
280
281void Particle::handleNewMeas(MeasurementSet &measurement_set_new, StateVector &state_proposal, std::vector<int> &id_new){
282
283 if (measurement_set_new.getNumberOfMeasurements() == 0 )
284 return;
285
286 for(uint32_t i = 1; i <= measurement_set_new.getNumberOfMeasurements(); i++) {
287
288 auto current_measurement = measurement_set_new.getMeasurement(i);
289
290 auto landmark = std::make_shared<Landmark>();
291 landmark->landmark_identifier = id_new.at(i-1);
292 landmark->landmark_pose = current_measurement->inverseMeasurementModel(state_proposal);
293
294 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_proposal, landmark->landmark_pose);
295
296 landmark->landmark_covariance = (landmark_jacobian.transpose() * (current_measurement->getzCov()).inverse() * landmark_jacobian).inverse();
297
298 //make symmetric
299 if(force_covariance_symmetry_)
300 landmark->landmark_covariance = (landmark->landmark_covariance + (landmark->landmark_covariance).transpose()) * 0.5;
301
302 map_->insertLandmark(landmark);
303 spatial_index_dirty_ = true; // Mark spatial index as needing update
304
305 }
306}
307
308StateVector Particle::drawSampleFromProposaleDistribution(StateVector &state_propagated, StateMatrix &Fs, StateMatrix &Fw, MeasurementSet &measurement_set_existing, std::vector<int> &id_existing){
309
310 StateVector state_proposal = state_propagated;
311
312 if(always_reset_particle_covariance_)
313 particle_covariance_ = StateMatrix::Zero();
314
315 StateMatrix state_proposal_covariance = Fs.transpose() * particle_covariance_ * Fs +
316 Fw.transpose() * motion_model_covariance_ * Fw;
317
318 if (measurement_set_existing.getNumberOfMeasurements() != 0){
319
320 for(size_t i = 1; i <= measurement_set_existing.getNumberOfMeasurements(); i++) {
321
322 auto current_measurement = measurement_set_existing.getMeasurement(i);
323 auto landmark_old = map_->extractLandmarkNodePointer(id_existing.at(i-1));
324
325 //resizes automatically due to the "=" operator
326 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_propagated, landmark_old->landmark_pose);
327 Eigen::MatrixXd state_jacobian = current_measurement->calculateHs(state_propagated, landmark_old->landmark_pose);
328
329
330 Eigen::MatrixXd Zki = current_measurement->getzCov() + landmark_jacobian * (landmark_old->landmark_covariance) * landmark_jacobian.transpose();
331
332 Eigen::VectorXd measurement_calculated = current_measurement->MeasurementModel(state_propagated, landmark_old->landmark_pose);
333
334 if(use_numerical_stabilized_kalman_filters_){
335 Eigen::VectorXd x = state_proposal;
336 Eigen::MatrixXd P = state_proposal_covariance;
337 Eigen::VectorXd v = (current_measurement->getMeasurement() - measurement_calculated);
338 Eigen::MatrixXd R = Zki;
339 Eigen::MatrixXd H = state_jacobian;
340
341 KFCholeskyUpdate(x, P, v, R, H);
342
343 state_proposal = x;
344 state_proposal_covariance = P;
345 }
346 else{
347 Eigen::MatrixXd Kk;
348 Kk = state_proposal_covariance * state_jacobian.transpose() *
349 (state_jacobian * state_proposal_covariance * state_jacobian.transpose() + Zki).inverse();
350
351 state_proposal_covariance = (state_jacobian.transpose() * Zki.inverse() * state_jacobian + state_proposal_covariance.inverse()).inverse();
352 state_proposal = state_proposal + state_proposal_covariance * state_jacobian.transpose() * Zki.inverse() *
353 (current_measurement->getMeasurement() - measurement_calculated);
354 }
355
356 if(force_covariance_symmetry_){
357 //make symmetric
358 state_proposal_covariance = (state_proposal_covariance + state_proposal_covariance.transpose()) * 0.5;
359 }
360 }
361 }
362
363 particle_covariance_ = state_proposal_covariance;
364
365 return drawSampleRandomPose(state_proposal, state_proposal_covariance);
366
367}
368
369
370void Particle::KFCholeskyUpdate(Eigen::VectorXd &x, Eigen::MatrixXd &P, Eigen::VectorXd &v, Eigen::MatrixXd &R, Eigen::MatrixXd &H){
371 Eigen::MatrixXd PHt = P*H.transpose();
372 Eigen::MatrixXd S = H*PHt + R;
373
374 // FIXME: why use conjugate()?
375 //make symmetric
376 if(force_covariance_symmetry_)
377 S = (S+S.transpose()) * 0.5;
378
379 Eigen::MatrixXd SChol = S.llt().matrixU();
380
381 //tri matrix
382 Eigen::MatrixXd SCholInv = SChol.inverse();
383 Eigen::MatrixXd W1 = PHt * SCholInv;
384 Eigen::MatrixXd W = W1 * SCholInv.transpose();
385
386 x = x + W*v;
387 P = P - W1*W1.transpose();
388}
389
390
391//http://moby.ihme.washington.edu/bradbell/mat2cpp/randn.cpp.xml
392Eigen::MatrixXd randn(int m, int n){
393 // use formula 30.3 of Statistical Distributions (3rd ed)
394 // Merran Evans, Nicholas Hastings, and Brian Peacock
395 int urows = m * n + 1;
396 Eigen::VectorXd u(urows);
397
398 //u is a random matrix
399 for (int r=0; r<urows; r++) {
400 // FIXME: better way?
401 u(r) = std::rand() * 1.0/RAND_MAX;
402 }
403
404
405
406 Eigen::MatrixXd x(m,n);
407
408 int i, j, k;
409 double square, amp, angle;
410
411 k = 0;
412 for(i = 0; i < m; i++) {
413 for(j = 0; j < n; j++) {
414 if( k % 2 == 0 ) {
415 square = - 2. * std::log( u(k) );
416 if( square < 0. )
417 square = 0.;
418 amp = sqrt(square);
419 angle = 2. * M_PI * u(k+1);
420 x(i, j) = amp * std::sin( angle );
421 }
422 else
423 x(i, j) = amp * std::cos( angle );
424
425 k++;
426 }
427 }
428
429 return x;
430}
431
432Eigen::MatrixXd rand(int m, int n)
433{
434 Eigen::MatrixXd x(m,n);
435 int i, j;
436 float rand_max = float(RAND_MAX);
437
438 for(i = 0; i < m; i++) {
439 for(j = 0; j < n; j++)
440 x(i, j) = float(std::rand()) / rand_max;
441 }
442 return x;
443}
444
445StateVector Particle::drawSampleRandomPose(StateVector &state_proposal, StateMatrix &state_proposal_covariance){
446 Eigen::MatrixXd Cov = state_proposal_covariance;
447
448 //make symmetric
449 if(force_covariance_symmetry_)
450 Cov = (Cov+Cov.transpose()) * 0.5;
451
452 //choleksy decomposition
453 Eigen::MatrixXd S = Cov.llt().matrixL();
454 Eigen::MatrixXd X = randn(3,1);
455
456 auto sample = S * X + state_proposal;
457 return sample;
458}
459
460// Ts == sample time
461StateVector Particle::motionModel(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds delta_time) {
462 StateVector state_propagated(state_old);
463
464 double theta = state_old(2);
465 double dt_in_s = std::chrono::duration<double>(delta_time).count();
466
467 if (dt_in_s > 3) {
468 cout << "Motion model: Sample rate error" << endl;
469 return state_propagated; // error with the sampling time, just return old pose estimate
470 }
471
472 // Extract velocity components from state_dot
473 double vx = state_dot(0);
474 double vy = state_dot(1);
475 double omega = state_dot(2);
476
477 // Convert to body frame velocity
478 double v_forward = vx * cos(theta) + vy * sin(theta);
479 double v_lateral = -vx * sin(theta) + vy * cos(theta);
480
481 // Differential drive motion model with proper kinematics
482 if (abs(omega) < 1e-6) {
483 // Straight line motion
484 state_propagated(0) += v_forward * cos(theta) * dt_in_s;
485 state_propagated(1) += v_forward * sin(theta) * dt_in_s;
486 state_propagated(2) += omega * dt_in_s;
487 } else {
488 // Curved motion with instantaneous center of rotation
489 double radius = v_forward / omega;
490 double dtheta = omega * dt_in_s;
491
492 state_propagated(0) += radius * (sin(theta + dtheta) - sin(theta));
493 state_propagated(1) += radius * (-cos(theta + dtheta) + cos(theta));
494 state_propagated(2) += dtheta;
495 }
496
497 // Normalize angle to [-pi, pi]
498 while (state_propagated(2) > M_PI) state_propagated(2) -= 2.0 * M_PI;
499 while (state_propagated(2) < -M_PI) state_propagated(2) += 2.0 * M_PI;
500
501 return state_propagated;
502}
503
504// Motion model Jacobian relative to pose
505StateMatrix Particle::calculateFs(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds &delta_time){
506 StateMatrix Fs = StateMatrix::Identity();
507 double dt_in_s = std::chrono::duration<double>(delta_time).count();
508
509 double theta = state_old(2);
510 double vx = state_dot(0);
511 double vy = state_dot(1);
512 double omega = state_dot(2);
513
514 // Convert to body frame velocity
515 double v_forward = vx * cos(theta) + vy * sin(theta);
516
517 if (abs(omega) < 1e-6) {
518 // Straight line motion Jacobian
519 Fs(0,2) = -v_forward * sin(theta) * dt_in_s;
520 Fs(1,2) = v_forward * cos(theta) * dt_in_s;
521 } else {
522 // Curved motion Jacobian
523 double radius = v_forward / omega;
524 double dtheta = omega * dt_in_s;
525
526 Fs(0,2) = radius * (cos(theta + dtheta) - cos(theta));
527 Fs(1,2) = radius * (sin(theta + dtheta) - sin(theta));
528 }
529
530 return Fs;
531}
532
533// Motion model Jacobian relative to motion
534StateMatrix Particle::calculateFw(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds &delta_time){
535 StateMatrix Fw = StateMatrix::Zero();
536 double dt_in_s = std::chrono::duration<double>(delta_time).count();
537
538 double theta = state_old(2);
539 double vx = state_dot(0);
540 double vy = state_dot(1);
541 double omega = state_dot(2);
542
543 // Convert to body frame velocity
544 double v_forward = vx * cos(theta) + vy * sin(theta);
545
546 if (abs(omega) < 1e-6) {
547 // Straight line motion Jacobian w.r.t control inputs
548 Fw(0,0) = cos(theta) * cos(theta) * dt_in_s;
549 Fw(0,1) = cos(theta) * sin(theta) * dt_in_s;
550 Fw(1,0) = sin(theta) * cos(theta) * dt_in_s;
551 Fw(1,1) = sin(theta) * sin(theta) * dt_in_s;
552 Fw(2,2) = dt_in_s;
553 } else {
554 // Curved motion Jacobian w.r.t control inputs
555 double radius = v_forward / omega;
556 double dtheta = omega * dt_in_s;
557
558 Fw(0,0) = (cos(theta) / omega) * (sin(theta + dtheta) - sin(theta));
559 Fw(0,1) = (sin(theta) / omega) * (sin(theta + dtheta) - sin(theta));
560 Fw(1,0) = (cos(theta) / omega) * (-cos(theta + dtheta) + cos(theta));
561 Fw(1,1) = (sin(theta) / omega) * (-cos(theta + dtheta) + cos(theta));
562 Fw(2,2) = dt_in_s;
563 }
564
565 return Fw;
566}
567
568void Particle::calculateImportanceWeight(MeasurementSet &measurment_set_existing, StateVector &state_proposal, StateMatrix &Fw, std::vector<int> &id_existing){
569 if (measurment_set_existing.getNumberOfMeasurements() == 0)
570 return;
571
572 Eigen::MatrixXd weight_covariance;
573 double wi = 1;
574
575 for(uint32_t i = 1; i <= measurment_set_existing.getNumberOfMeasurements(); i++ ) {
576
577 auto current_measurement = measurment_set_existing.getMeasurement(i);
578 auto landmark_old = map_->extractLandmarkNodePointer(id_existing.at(i-1));
579
580 //resizes automatically due to the "=" operator
581 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_proposal, landmark_old->landmark_pose);
582 Eigen::MatrixXd state_jacobian = current_measurement->calculateHs(state_proposal, landmark_old->landmark_pose);
583
584 Eigen::VectorXd measurement_calculated = current_measurement->MeasurementModel(state_proposal, landmark_old->landmark_pose);
585
586 Eigen::VectorXd measurement_difference = current_measurement->getMeasurement() - measurement_calculated;
587
588
589 // weight_covariance = state_jacobian * Fw * motion_model_covariance_ * Fw.transpose() * state_jacobian.transpose() +
590 // landmark_jacobian * landmark_old->landmark_covariance * landmark_jacobian.transpose() + current_measurement->getzCov();
591 weight_covariance = landmark_jacobian * landmark_old->landmark_covariance * landmark_jacobian.transpose() + current_measurement->getzCov();
592
593 double exponent = measurement_difference.transpose() * weight_covariance.inverse() * measurement_difference;
594 double determinant = (2 * M_PI * weight_covariance).determinant();
595
596 wi *= exp(-0.5 * exponent) / (sqrt(determinant));
597
598 // std::cout << "See here state component: \n" << state_jacobian * Fw * motion_model_covariance_ * Fw.transpose() * state_jacobian.transpose() << "\n And see here the measurement component: \n" << landmark_jacobian * landmark_old->landmark_covariance * landmark_jacobian.transpose() + current_measurement->getzCov() << "\n";
599 // std::cout << "partial weight: " << exp(-0.5 * exponent) / (sqrt(determinant)) << "\n";
600 }
601
602 weight_ = wi;
603}
604
605// StateMatrix Particle::motion_model_covariance_ = 0.000001*StateMatrix::Identity(); // static variable - has to be declared outside class!
606StateMatrix Particle::motion_model_covariance_ = (StateMatrix() << 0.001, 0.0, 0.0,
607 0.0, 0.001, 0.0,
608 0.0, 0.0, 0.000001).finished(); // static variable - has to be declared outside class!
609// (Eigen::Matrix4d() << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16).finished();
610boost::mt19937 Particle::rng; // Creating a new random number generator every time could be optimized
611//rng.seed(static_cast<unsigned int>(time(0)));
612
613
614} //namespace fastslam
615