some renaming and corrected a minor issue where in the gf tracker should work now for multiple objects (not tested)

Author: wumanu

source/dbot/tracker/gaussian_tracker.cpp

@@ -50,12 +50,15 @@ auto GaussianTracker::on_initialize(
 
 auto GaussianTracker::on_track(const Obsrv& obsrv) -> State
 {
+    // the following is approximately ok, but to be correct in a differential
+    // geometry sense, the covariance matrix would also have to change due
+    // to the parallel transport
+
     State old_pose = belief_.mean();
-    filter_->sensor().local_sensor().body_model().nominal_pose(
-        old_pose);
+    filter_->sensor().local_sensor().body_model().nominal_pose(old_pose);
 
     State zero_pose = belief_.mean();
-    zero_pose.component(0).set_zero_pose();
+    zero_pose.set_zero_pose();
     belief_.mean(zero_pose);
 
     filter_->predict(belief_, zero_input(), belief_);

source/dbot/tracker/particle_tracker.cpp

@@ -47,7 +47,7 @@ auto ParticleTracker::on_initialize(
 
     for (size_t i = 0; i < filter_->belief().size(); i++)
     {
-        filter_->belief().location(i).center_around_zero(delta_mean);
+        filter_->belief().location(i).subtract(delta_mean);
     }
 
     auto& integrated_poses = filter_->sensor()->integrated_poses();
@@ -64,7 +64,7 @@ auto ParticleTracker::on_track(const Obsrv& image) -> State
 
     for (size_t i = 0; i < filter_->belief().size(); i++)
     {
-        filter_->belief().location(i).center_around_zero(delta_mean);
+        filter_->belief().location(i).subtract(delta_mean);
     }
 
     auto& integrated_poses = filter_->sensor()->integrated_poses();