# Sampling from guassian distribution for odometry based motion?

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);
while(1){
if(temp>(lower) && temp<(upper))
break;
else
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 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] • I don't really know about this but I'm wondering if the line auto lower=-(mean+3*standard_deviation); is correct. – Petch Puttichai Aug 14 '19 at 0:48
• Here I am just checking the sigma bounds for the output value I am constraining it to (-3sigma - 3sigma). – Zain Aug 14 '19 at 1:06
• should it be mean - 3*standard_deviation? – Petch Puttichai Aug 14 '19 at 1:07
• Yes you are right. – Zain Aug 14 '19 at 2:35
• 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. – Manuel Rodriguez Aug 14 '19 at 12:16