I am trying to generate an odometry based motion model as described in the lectures. I have implemented a C++ version of the code. The issue I am having is that when I try to simulate successive motion using odometry commands the uncertainty in the pose is not increasing. It almost remains the same as the robot moves.

Here is the sampling function

double MotionModels::sample_normal_distribution_gaussian(float mean, float standard_deviation) {

   auto lower=-(mean+3*standard_deviation);
   auto upper =mean+3*standard_deviation;
    std::normal_distribution<double> distribution(mean,standard_deviation);
    double temp= distribution(generator);
        if(temp>(lower) && temp<(upper))
            temp= distribution(generator);

    return temp;


Here is the motion model :

Pose MotionModels::MM_Odometry(Pose &p, Odometry &u, NoiseParams &np) {

    Odometry u_n;
    u_n.delta_rot1 = u.delta_rot1 + sample_normal_distribution_gaussian(0,(np.a1*abs(u.delta_rot1)) + (np.a2*u.delta_trans));
    u_n.delta_trans= u.delta_trans + sample_normal_distribution_gaussian(0,np.a3*u.delta_trans + np.a4*(abs(u.delta_rot1)+abs(u.delta_rot2)));
    u_n.delta_rot2 = u.delta_rot2 + sample_normal_distribution_gaussian(0,np.a1*abs(u.delta_rot2)+np.a2*u.delta_trans);

    Pose out;
    out.x       = p.x + u_n.delta_trans*cos(p.theta+u_n.delta_rot1);
    out.y       = p.y + u_n.delta_trans*sin(p.theta+u_n.delta_rot1);
    out.theta   = p.theta + u_n.delta_rot1 + u_n.delta_rot2;

    return  out;


Simulation results with a series of motion are as under enter image description here

Expected Results enter image description here

Would be highly thankful for any help.

Update : Follwoing @Petch's suggestion I was able to get the following output. The uncertainty increases as the robot moves away from starting point but when I take a 90 degrees turn the behavior is unexpected .

These are the odometery commands I am using along with paramaters

  • u1=[0,0,1] (rot1, rot2,trans)
  • u2=[0,0,1.1]
  • u3=[0,0,1.3]
  • u4=[0,pi/2,1]
  • u5=[0,0,1]

Noise Parameters : -[alpha1,alpha2,aplha3,alpha4]=[0.1,0.1,0.01,0.01]

enter image description here

  • $\begingroup$ I don't really know about this but I'm wondering if the line auto lower=-(mean+3*standard_deviation); is correct. $\endgroup$ – Petch Puttichai Aug 14 '19 at 0:48
  • $\begingroup$ Here I am just checking the sigma bounds for the output value I am constraining it to (-3sigma - 3sigma). $\endgroup$ – Zain Aug 14 '19 at 1:06
  • $\begingroup$ should it be mean - 3*standard_deviation? $\endgroup$ – Petch Puttichai Aug 14 '19 at 1:07
  • $\begingroup$ Yes you are right. $\endgroup$ – Zain Aug 14 '19 at 2:35
  • $\begingroup$ If i have understood the posting correct, the idea is that the robot follows a predefined trajectory and during the ride the robot's position becomes uncertain which is realized with a random generator which is equal to a gaussian model. The problem is, that somebody has to control the robot which is perhaps a macro. In the macro it's defined that the robot is at timecode 0 at waypoint #0, at timecode1 at waypoint #1 and so on. $\endgroup$ – Manuel Rodriguez Aug 14 '19 at 12:16

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.