HW7

Question 3.

Here is the code I used:

X = [2.0 4.0 0.0];

U = [pi/2 0.0 1.0];

alpha = [0.1 0.1 0.01 0.01];

hold on;

for i= 1:5000

point = odoMotion(X,U,alpha);

plot(point(1), point(2),'ro')

end

Where the code for odoMotion is:

function points = odoMotion( X,U,alpha )

%motionModel odometry-based motion model

% function takes 3 arguments:

% X = (x , y, theta)

% U = (deltaR1 , deltaR2, deltaT)

% alpha = (alpha1, alpha2, alpha3, alpha 4)

dr1 = U(1) + sample_normal_distribution(alpha(1)*abs(U(1)) + alpha(2)*U(3));

dt = U(3) + sample_normal_distribution(alpha(3)*U(3) + alpha(4)*(abs(U(1)) + abs(U(2))));

dr2 = U(2) + sample_normal_distribution(alpha(1)*abs(U(2))+alpha(2)*U(3));

points(1) = X(1) + dt*cos(X(3)+dr1);

points(2) = X(2) + dt*sin(X(3)+dr1);

points(3) = X(3) + dr1+dr2;

end

And the sample_normal_distribution code is:

function num = sample_normal_distribution( n )

%sample_normal_distribution returns a sample from a normally distributed

%curve

temp=0;

for j = 1:12

temp = temp+ n*randn();

end

num = temp/2;

end

As expected, the plot forms the bell curve of the normal distribution, with the major concentration of the points in the center, and very few outliers.

And here is the requested plot to the given input:

And just for fun, I tried the same input, except U = [pi 0.0 1.0]; and U = [pi/4 0.0 1.0];

Google Sites

Report abuse