Sheet 4 Solutions

October 9, 2020

1 Sheet 4
Kelompok 3 :
1. Muhammad Farhan Fathurrahman (23219344)
2. Adityas Regita P. (23220020)
3. Satrya Budi Pratama (23219305)
Link Question : http://ais.informatik.uni-freiburg.de/teaching/ss20/robotics/exercises/sheet04.pdf

1.1 Exercise 1: Sampling

Implement three functions in python which generate samples of a normal distribution
[1]: # libraries
import numpy as np
import timeit
import scipy.stats
import matplotlib.pyplot as plt

[2]: def sample_normal_dist_12(mu , sigma):

'''
Generate samples from a normal distribution using 12 unfiform samples.
Page 19 on Probabilistic Motion Models.
Params :
- mu : mean of the normal distribution
- sigma : standart deviation of the normal distribution

Return :
- result : the generated samples

'''

# generate unfiorm distribution with 12 samples

uniform_distribution = np.random.uniform(-sigma, sigma, 12)

# scaling to normal distribution

x = 0.5 * np.sum(uniform_distribution)

1
 # addition
result = mu + x

return result

[3]: def sample_normal_dist_rejection(mu, sigma):

'''
Generate samples from a normal distribution using rejection sampling.
Page 25 on Probabilistic Motion Models.
Params :
- mu : mean of the normal distribution
- sigma : standart deviation of the normal distribution

Return :
- result : the generated samples

'''

# length of interval from which samples are drawn

interval = 5 * sigma

# maximum value of the pdf of the desired normal distribution

max_density = scipy.stats.norm(mu, sigma).pdf(mu)

# Rejection loop
while True:
x = np.random.uniform(mu - interval, mu + interval, 1)[0]
y = np.random.uniform(0 , max_density,1 )

# get pdf value

pdf_value = scipy.stats.norm(mu,sigma).pdf(x)

# stop looping when y less than the pdf

if y <= pdf_value:
break

return x

[4]: def sample_normal_dist_boxmuller(mu, sigma):

'''
Generate samples from a normal distribution using Box-Muller method.
Params :
- mu : mean of the normal distribution
- sigma : standart deviation of the normal distribution

2
 Return :
- result : the generated samples

'''

# two uniform random variables

u = np.random.uniform(0, 1, 2)

# Box-Muller formula returns sample from STANDARD normal distribution

x = np.cos(2*np.pi*u[0]) * np.sqrt(-2*np.log(u[1]))

result = mu + sigma * x

return result

[5]: def evaulate_sampling_time(mu, sigma, n_samples, sample_function):

'''
Evaluate how much time taken for each sampling method
Params :
- mu : mean of the normal distribution
- sigma : standart deviation of the normal distribution
- n_samples : number of samples generated
- sample_function : name of the function
Return :
-
'''

# get current time

time_start = timeit.default_timer()

# generate sample
for i in range(n_samples):

# invoke the function

sample_function(mu, sigma)

# get current time

time_finish = timeit.default_timer()

# calculate time taken

required_time = (time_finish - time_start)
time_per_sample = required_time / n_samples * 1e6

# debug
print("%30s : %.3f us" % (sample_function.__name__, time_per_sample))

3
[6]: def evaluate_sampling_distribution(mu, sigma, n_samples, sample_function):
'''
Evaluate the generated samples for each sampling method
Params :
- mu : mean of the normal distribution
- sigma : standart deviation of the normal distribution
- n_samples : number of samples generated
- sample_function : name of the function
Return :
-
'''

n_bins = 100
samples = []

# generate samples
for i in range(n_samples):

# append result to list

samples.append(sample_function(mu, sigma))

# debug
print("Name : {} - Mean : {} - Standart Deviation : {}".
,→format(sample_function.__name__, np.mean(samples), np.std(samples)))

# plotting
plt.figure()

# make histogram
count, bins, ignored = plt.hist(samples, n_bins, density=True)

# plot the distribution

plt.plot(bins, scipy.stats.norm(mu, sigma).pdf(bins), linewidth=2,␣
,→color='r')

plt.xlim([mu - 5*sigma, mu + 5*sigma])

plt.title(sample_function.__name__)

[7]: # define mu and sigma

mu = 0
sigma = 1

# make 4 list of sampling method name

sample_functions = [
sample_normal_dist_12,
sample_normal_dist_rejection,

4
 sample_normal_dist_boxmuller,
np.random.normal
]

# evaluate sampling time

for function in sample_functions:
evaulate_sampling_time(mu, sigma, 1000, function)

sample_normal_dist_12 : 11.699 us
sample_normal_dist_rejection : 2803.144 us
sample_normal_dist_boxmuller : 9.977 us
normal : 2.028 us

[8]: # evaluate sampling method

n_samples = 10000
print("Evaluating sample distance with : ")
print("\t Mean : {}".format(mu))
print("\t Standart Deviation : {}".format(sigma))
print("\t Samples : {}".format(n_samples))

# evaluate sampling time

for function in sample_functions:
evaluate_sampling_distribution(mu, sigma, n_samples, function)

Evaluating sample distance with :

Mean : 0
Standart Deviation : 1
Samples : 10000
Name : sample_normal_dist_12 - Mean : 0.004585760328715301 - Standart Deviation
: 0.9953742402140046
Name : sample_normal_dist_rejection - Mean : -0.010388817384846148 - Standart
Deviation : 0.989199607884964
Name : sample_normal_dist_boxmuller - Mean : 7.507180853313358e-05 - Standart
Deviation : 0.9963172823355732
Name : normal - Mean : 0.014183578384075543 - Standart Deviation :
1.0011873983468549

5
6
7
 1.2 Exercise 2: Odometry-based Motion Model
Implement simple odometry-based motion model
[9]: def sample_odometry_motion_model(x, u, a):
'''
Implement the odometry-based motion model, on point(a).
Page 27 on Probabilistic Motion Models.

Params:
- x : pose of the robot before moving [ x, y, theta]
- u : odometry reading obtained from the robot [delta rotation1, delta␣
,→rotation2, delta translation]

- a : noise parameters of the motion models [ alpha1, alpha2, alpha3,␣

,→alpha4]

Return :
- result : predict new pose of the robot after moving
'''

# calculate delta odometry

mu = 0

sigma_r1 = a[0]*abs(u[0]) + a[1]*u[2]

delta_hat_r1 = u[0] + sample_normal_dist_12(mu, sigma_r1)

sigma_r2 = a[0]*abs(u[1]) + a[1]*u[2]

delta_hat_r2 = u[1] + sample_normal_dist_12(mu, sigma_r2)

sigma_trans = a[2]*u[2] + a[3]*(abs(u[0]) + abs(u[1]))

delta_hat_trans = u[2] + sample_normal_dist_12(mu, sigma_trans)

# calculate new pose

x_prime = x[0] + delta_hat_trans * np.cos(x[2] + delta_hat_r1)
y_prime = x[1] + delta_hat_trans * np.sin(x[2] + delta_hat_r1)
theta_prime = x[2] + delta_hat_r1 + delta_hat_r2

new_pose = [x_prime , y_prime, theta_prime]

return new_pose

[10]: # point (c)

x = [2, 4, 0]
u = [np.pi / 2, 0.0, 1.0]
alpha = [0.1, 0.1, 0.01, 0.01]

iteration = 5000

8
predicted_pose = []
# looping for 5000
for i in range(iteration):

# get new pose

new_pose = sample_odometry_motion_model(x, u, alpha)

# append to list
predicted_pose.append(new_pose)

# convert list to array

predicted_pose_arr = np.asarray(predicted_pose)

# visualize the list

plt.plot(x[0] , x[1], "bo")
plt.plot(predicted_pose_arr[:,0], predicted_pose_arr[:,1], "r,")
plt.xlim([1, 3])
#plt.axes().set_aspect('equal')
plt.xlabel("x-position [m]")
plt.ylabel("y-position [m]")
plt.title("Sample Motion Model")
plt.show()

9
1.3 Exercise 3 : Velocity-Based Motion Model

10
11
12
13