def motion_model(position, mov, priors, map_size, stdev):
# Initialize the position's probability to zero.
position_prob = 0.0
# TODO: Loop over state space for all possible prior positions,
# calculate the probability (using norm_pdf) of the vehicle
# moving to the current position from that prior.
# Multiply this probability to the prior probability of
# the vehicle "was" at that prior position.
# Loop over state space for all possible prior positions
for p in range(map_size):
# calculate the distance that vehicle have to move
# to pseudo position from each possible prior positions
dist = position - p
# calculate the probability of the vehicle moving to
# distance that vehicle have to move
move_prob = norm_pdf(dist, mov, stdev)
# mulyiply the probability of the vehicle moving and
# the prior probaility of the vehicle was at the each possible
# prior positions
position_prob_each = move_prob * priors[p]
# the summation of every probability of each prior positions
# is the probability that the vehicle is at the pseudo position.
position_prob += position_prob_each
return position_prob
def motion_model(position, mov, priors, map_size, stdev):
# Initialize the position's probability to zero.
position_prob = 0.0
# when motion_model function called, below is arguments
# Calculate probability of the vehicle being at position p
###motion_prob = motion_model(
### pseudo_position, mov_per_timestep, priors,
### map_size, control_stdev)
## What is the arguments of norm_pdf?
# Calculate normal (Gaussian) distribution probability.
# norm_pdf(x, m, s) returns the probability of a random variable
# havnig the value of x, assuming that the variable follows
# the Gaussian probability distribution with mean of m and
# standard deviation of s.
###def norm_pdf(x, m, s):
# TODO: Loop over state space for all possible prior positions,
# calculate the probability (using norm_pdf) of the vehicle
# moving to the current position from that prior.
# Multiply this probability to the prior probability of
# the vehicle "was" at that prior position.
##position_prob = norm_pdf(priors, 1, stdev)
for i in range(len(priors)):
g = norm_pdf(position - i, 1, stdev)
position_prob += g * priors[i]
return position_prob
def observation_model(landmarks, observations, pseudo_ranges, stdev):
# Initialize the measurement's probability to one.
distance_prob = 1.0
############################################################################
# TODO: Calculate the observation model probability as follows: #
# (1) If we have no observations, we do not have any probability. #
# (2) Having more observations than the pseudo range indicates that #
# this observation is not possible at all. #
# (3) Otherwise, the probability of this "observation" is the product of #
# probability of observing each landmark at that distance, where #
# that probability follows N(d, mu, sig) with #
# d: observation distance #
# mu: expected mean distance, given by pseudo_ranges #
# sig: squared standard deviation of measurement #
############################################################################
# If pseudo_ranges are more than or same to the # of observations,
# you can calculate observation probability
if (len(observations) <= len(pseudo_ranges)):
# For (observation, pseudo_range) pair, calculate probability
# and multiply it to observation probability(distance_prob)
# As using values from the front in a sorted order,
# the remaining pseudo_range values will be discarded if exist.
for observation, pseudo_range in zip(observations, pseudo_ranges):
distance_prob *= norm_pdf(observation, pseudo_range, stdev)
# If there is no observation--(1) or there are more observations than the psuedo ranges--(2),
# we don't have any probability or can't calculate probabilities because this observation is not possible
# Therefore, set the distance_prob to 0.0
else:
distance_prob = 0.0
return distance_prob
def observation_model(landmarks, observations, pseudo_ranges, stdev):
# Initialize the measurement's probability to one.
distance_prob = 1.0
if len(observations) == 0 or len(observations) > len(pseudo_ranges):
distance_prob = 0.0
else:
for i in range(len(observations)):
distance_prob = distance_prob * norm_pdf(observations[i],
pseudo_ranges[i], stdev)
# The observation model probability can be calculated by using the above code.
# TODO: Calculate the observation model probability as follows:
# (1) If we have no observations, we do not have any probability.
# (2) Having more observations than the pseudo range indicates that
# this observation is not possible at all.
# (3) Otherwise, the probability of this "observation" is the product of
# probability of observing each landmark at that distance, where
# that probability follows N(d, mu, sig) with
# d: observation distance
# mu: expected mean distance, given by pseudo_ranges
# sig: squared standard deviation of measurement
return distance_prob
def motion_model(position, mov, priors, map_size, stdev):
# Initialize the position's probability to zero.
position_prob = 0.0
for i in range(map_size):
position_prob += norm_pdf(position - i, mov, stdev) * priors[i]
return position_prob
def motion_model(position, mov, priors, map_size, stdev):
# Initialize the position's probability to zero.
position_prob = 0.0
# TODO: Loop over state space for all possible prior positions,
# calculate the probability (using norm_pdf) of the vehicle
# moving to the current position from that prior.
# Multiply this probability to the prior probability of
# the vehicle "was" at that prior position.
for i in range(map_size):
position_prob += norm_pdf(position - i, mov, stdev) * priors[i]
return position_prob
def observation_model(landmarks, observations, pseudo_ranges, stdev):
# Initialize the measurement's probability to one.
distance_prob = 1.0
if len(observations
) == 0 or len(observations) > len(pseudo_ranges): # (1)/(2)
return 0.
for i in range(len(observations)):
distance_prob *= norm_pdf(observations[i], pseudo_ranges[i], stdev)
return distance_prob
def motion_model(position, mov, priors, map_size, stdev):
# position에서의 확률을 0으로 초기화
position_prob = 0.0
# prior positions의 가능한 모든 state space를 돌면서
# 해당 위치가 과거 position이라 가정하고, 현재 position까지
# 차량이 움직일 확률을 norm_pdf를 이용하여 계산한다.
# 이전 시점에 차량이 과거 position에 있었을 확률(priors의 과거 position에 대한 확률값)과
# 과거 position부터 현재 position까지 이동하는 확률값(norm_pdf)를 곱한다.
# 모든 범위에 대해서 합하면 최종적으로 time t에 현재 position에 있을 확률이 나오게 된다.
for p in range(0, map_size):
position_prob += norm_pdf(position-p, 1, 1) * priors[p]
return position_prob
def motion_model(position, mov, priors, map_size, stdev):
# Initialize the position's probability to zero.
position_prob = 0.0
for t1 in range(len(priors)):
p_trans = norm_pdf(position - t1, 1, stdev)
prior = priors[t1]
position_prob = position_prob + (p_trans * prior)
# TODO: Loop over state space for all possible prior positions,
# calculate the probability (using norm_pdf) of the vehicle
# moving to the current position from that prior.
# Multiply this probability to the prior probability of
# the vehicle "was" at that prior position.
return position_prob
def observation_model(landmarks, observations, pseudo_ranges, stdev):
# Initialize the measurement's probability to one.
distance_prob = 1.0
if len(observations) == 0 or len(observations) > len(pseudo_ranges):
distance_prob = 0.0
else:
for i in range(len(observations)):
d = observations[i]
mu = pseudo_ranges[i]
sig = stdev
L = norm_pdf(d, mu, sig)
distance_prob *= L
return distance_prob
def motion_model(position, mov, priors, map_size, stdev):
# Initialize the position's probability to zero.
position_prob = 0.0
for p in range(map_size):
position_prob = position_prob + norm_pdf(position - p, mov,
stdev) * priors[p]
# The prediction of motion model probability can be calculated by using the above code.
# The code is sum of multiplying prior position by normal distribution.
# TODO: Loop over state space for all possible prior positions,
# calculate the probability (using norm_pdf) of the vehicle
# moving to the current position from that prior.
# Multiply this probability to the prior probability of
# the vehicle "was" at that prior position.
return position_prob
def observation_model(landmarks, observations, pseudo_ranges, stdev):
# Initialize the measurement's probability to one.
distance_prob = 1.0
# (1) 관측된 landmark가 없다면 확률 분포를 계산할 수가 없다.
# (2) 관측된 landmark의 수가, 차량의 앞에 놓인 landmark까지의 거리 목록(pseudo_ranges)보다
# 많다면 잘못된 값이다.
# (3) 이 관측 결과가 옳을 확률은, 목록의 각 landmark가 해당 거리에서 보일 확률을
# 모두 곱한 것과 같다.
# N(해당 랜드마크까지의 관측된 거리, 해당 랜드마크까지의 거리, 1)
if len(observations) is None:
return 0.0
elif len(observations) > len(pseudo_ranges):
return 0.0
else:
for i in range(len(observations)):
distance_prob *= norm_pdf(observations[i], pseudo_ranges[i], 1)
return distance_prob
def motion_model(position, mov, priors, map_size, stdev):
# Initialize the position's probability to zero.
position_prob = 0.0
###################################################################
# TODO: Loop over state space for all possible prior positions, #
# calculate the probability (using norm_pdf) of the vehicle #
# moving to the current position from that prior. #
# Multiply this probability to the prior probability of #
# the vehicle "was" at that prior position. #
###################################################################
# Loop over state space for all possible position
for i in range(map_size):
# Calculate distance to the pseudo position from current position(i)
dist = position - i
# Calculate transition probability
transition_prob = norm_pdf(dist, mov, stdev)
# Multiply transition probability to the prior probability of the vehicle "was" at that prior position.
position_prob += transition_prob * priors[i]
return position_prob
def observation_model(landmarks, observations, pseudo_ranges, stdev):
# Initialize the measurement's probability to one.
distance_prob = 1.0
# TODO: Calculate the observation model probability as follows:
# (1) If we have no observations, we do not have any probability.
# (2) Having more observations than the pseudo range indicates that
# this observation is not possible at all.
# (3) Otherwise, the probability of this "observation" is the product of
# probability of observing each landmark at that distance, where
# that probability follows N(d, mu, sig) with
# d: observation distance
# mu: expected mean distance, given by pseudo_ranges
# sig: squared standard deviation of measurement
if(len(observations) <= len(pseudo_ranges)):
for p, o in zip(pseudo_ranges, observations):
tmp = norm_pdf(o,p,stdev)
distance_prob *= tmp
else:
distance_prob = 0
return distance_prob
