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