pull_request

code/week-2/README.md

@@ -7,6 +7,45 @@
 
 ## Assignment
 
+[HW_Week2]
+
+
+[1] Motion model (assuming 1-D Gaussian dist)
+def motion_model(position, mov, priors, map_size, stdev):
+
+    position_prob = 0.0
+
+    for i in range(map_size):
+        position_prob += norm_pdf(position-i,stdev, mov)*priors[i]
+
+    return position_prob
+
+- Motion Model 은 차량이 이동할 수 있는 모든 직전 위치의 확률과 이동에 따른 현재 위치를 예측하여 확률로 나타낸다.
+- Position Prob 는 차량 위치를 예측한 확률로, 차량이 존재할 수 있는 모든 이전 위치의 확률을 Priors 로 나타낸다
+또한 이전 위치에서 현재 차량이 존재하는 위치로 이동할 수 있는 확률은 Position 으로 , 직전 위치 확률 * 현재 위치로 올 확률로 계산한다.    
+
+[2] Observation model (assuming independent Gaussian)
+def observation_model(landmarks, observations, pseudo_ranges, stdev):
+
+    distance_prob = 1.0
+
+    if len(observations) == len(pseudo_ranges):
+        for i in range(len(pseudo_ranges)):
+            distance_prob *= norm_pdf(pseudo_ranges[i],observations[i],1)
+    else : distance_prob = 0
+
+    return distance_prob
+
+- Observations Model 은 차량 주변의 나무(LandMark)를 관측할 수 있지만, 불확실성을 가지고 있다. 
+관측한 나무 위치 값을 기준으로 차량이 존재할 수 있는 위치에 대한 신뢰도를 높인다.
+
+- 나무가 존재하여도 관측 값이 0이거나, 나무 수보다 많게 관측되면 사용할 수 없는 값으로 처리한다.
+신뢰할만한 관측 결과는 (관측된 나무 위치 * 현재 차량 위치에서 나무가 관측될 확률)로 나타낼 수 있다. 
+
+- 정확도가 점차 커지는 과정은 관측된 수만큼 반복하여 차량에서 관측한 나무 사이의 거리와 나무가 그 거리에서 보일 확률을 곱하여 누적한다. 
+위와 같은 반복 작업으로 계산된 위치에 대한 확률과 센서 측정에 대한 확률을 이용하여 현재 차량이 위치할 수 있는 확률을 나타낸다. 
+
+
 You will complete the implementation of a simple Markov localizer by writing the following two functions in `markov_localizer.py`:
 
 * `motion_model()`: For each possible prior positions, calculate the probability that the vehicle will move to the position specified by `position` given as input.
@@ -21,3 +60,4 @@ If you correctly implement the above functions, you expect to see a plot similar
 ![Expected Result of Markov Localization][plot]
 
 If you run the program (`main.py`) without any modification to the code, it will generate only the frame of the above plot because all probabilities returned by `motion_model()` are zero by default.
+

code/week-2/mov_localizer.py

@@ -0,0 +1,85 @@
+from helper import norm_pdf
+
+# Initialize prior probabilities
+# taking into account that the vehicle is initially parked
+# around one of the landmarks and we do not know which.
+def initialize_priors(map_size, landmarks, stdev):
+    # Set all probabilities to zero initially.
+    priors = [0.0] * map_size
+    # Initialize prior distribution assuming the vehicle is at
+    # landmark +/- 1.0 meters * stdev.
+    positions = []
+    for p in landmarks:
+        start = int(p - stdev) - 1
+        if start < p:
+            start += 1
+        c = 0
+        while start + c <= p + stdev:
+            # Gather positions to set initial probability.
+            positions.append(start + c)
+            c += 1
+    # Calculate actual probability to be uniformly distributed.
+    prob = 1.0 / len(positions)
+    # Set the probability to each position.
+    for p in positions:
+        priors[p] += prob
+    return priors
+
+# Estimate pseudo range determined according to the
+# given pseudo position.
+def estimate_pseudo_range(landmarks, p):
+    pseudo_ranges = []
+    # Loop over each landmark and estimate pseudo ranges
+    for landmark in landmarks:
+        dist = landmark - p
+        # Consider only those landmarks ahead of the vehicle.
+        if dist > 0:
+            pseudo_ranges.append(dist)
+    return pseudo_ranges
+
+# Motion model (assuming 1-D Gaussian dist)
+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,stdev, mov)*priors[i]
+
+    # 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
+
+# Observation model (assuming independent Gaussian)
+def observation_model(landmarks, observations, pseudo_ranges, stdev):
+    # Initialize the measurement's probability to one.
+    distance_prob = 1.0
+
+    if len(observations) == len(pseudo_ranges):
+        for i in range(len(pseudo_ranges)):
+            distance_prob *= norm_pdf(pseudo_ranges[i],observations[i],1)
+    else : distance_prob = 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
+    return distance_prob
+
+# Normalize a probability distribution so that the sum equals 1.0.
+def normalize_distribution(prob_dist):
+    normalized = [0.0] * len(prob_dist)
+    total = sum(prob_dist)
+    for i in range(len(prob_dist)):
+        if (total != 0.0):
+            normalized[i] = prob_dist[i] / total
+    return normalized
+

code/week-3/EKF/kalman_filter.py

@@ -2,6 +2,7 @@
 from math import sqrt
 from math import atan2
 from tools import Jacobian
+from math import pi
 
 class KalmanFilter:
     def __init__(self, x_in, P_in, F_in, H_in, R_in, Q_in):
@@ -19,17 +20,33 @@ def predict(self):
     def update(self, z):
         S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
         K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
+
         # Calculate new estimates
         self.x = self.x + np.dot(K, z - np.dot(self.H, self.x))
         self.P = self.P - np.dot(np.dot(K, self.H), self.P)
 
     def update_ekf(self, z):
         # TODO: Implement EKF update for radar measurements
         # 1. Compute Jacobian Matrix H_j
+        px, py, vx, vy = self.x
+        H_j = Jacobian(self.x)
+
         # 2. Calculate S = H_j * P' * H_j^T + R
-        # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R
+        S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R
+
+        # 3. Calculate Kalman gain K = H_j * P' * H_j^T + R
+        K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S))
+
         # 4. Estimate y = z - h(x')
+        y = z - [sqrt(px*px+py*py), atan2(py,px), (px*vx+py*vy)/sqrt(px*px+py*py)]
+
         # 5. Normalize phi so that it is between -PI and +PI
+        while (y[1] > pi): y[1] -= 2*pi
+        while (y[1] < -pi): y[1] += 2*pi
+
         # 6. Calculate new estimates
         #    x = x' + K * y
         #    P = (I - K * H_j) * P
+        self.x = self.x + np.dot(K, y)
+        self.P = self.P - np.dot(np.dot(K, H_j), self.P)
+

code/week-3/README.md

@@ -1,5 +1,57 @@
 # Week 3 - Kalman Filters, EKF and Sensor Fusion
 
+### Assignment
+
+Complete the implementation of EKF with sensor fusion by writing the function `update_ekf()` in the module `kalman_filter`. Details are given in class and instructions are included in comments.
+
+[HW_week3]
+Update Extended Kalman filter
+
+- EKF는 기존의 Kalman Filter와 유사하나 선형화하는 기준점을 계속 갱신한다는 특징을 가지고 있다.
+EKF 업데이트는 레이더 관측을 활용한다.         
+
+       def update_ekf(self, z):
+        # TODO: Implement EKF update for radar measurements       
+        # 1. Compute Jacobian Matrix H_j
+        px, py, vx, vy = self.x
+        H_j = Jacobian(self.x)
+
+- 자코비안(Jacobian) 행렬은 원소들이 모두 1차 미분 계수로 구성되어 있으며, 미소 영역에서 ‘비선형 변환’을 ‘선형 변환으로 근사화 시키는 것이 특징이다.
+Jacobian 함수를 이용하여 상태변수 Jacobian 행렬을 계산한다.        
+
+        # 2. Calculate S = H_j * P' * H_j^T + R
+        S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R
+
+- Kalman Gain "k" 를 구하기 위해 (자코비안 행렬 * 예측 공분산 P * 측정 에러 공분산 R)을 계산한다.      
+
+        # 3. Calculate Kalman gain K = H_j * P' * H_j^T + R
+        K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S))
+
+- Kalman Gain "K" 계산      
+
+        # 4. Estimate y = z - h(x')
+        y = z - [sqrt(px*px+py*py), atan2(py,px), (px*vx+py*vy)/sqrt(px*px+py*py)]
+
+- 레이더 관측 변수 "z" 와 관측 상태변수 "y" 차이로 에러를 구한다.
+
+        # 5. Normalize phi so that it is between -PI and +PI
+        while (y[1] > pi): y[1] -= 2*pi
+        while (y[1] < -pi): y[1] += 2*pi
+
+- 관측 변수의 단위는 라디안(rad)이고, 모든 각도를 pi : -pi 사이의 값을 가질 수 있다.
+위 각도 범위를 벗어나지 않도록 조건을 생성한다.          
+
+        # 6. Calculate new estimates
+        #    x = x' + K * y
+        #    P = (I - K * H_j) * P
+        self.x = self.x + np.dot(K, y)
+        self.P = self.P - np.dot(np.dot(K, H_j), self.P)
+
+- 예측 상태변수 "x" 와 관측 에러 "y", Kalman Gain "K" 를 활용하여 새로운 예측 상태변수 "x" 를 구할 수 있다.
+새로운 공분산 "P" 도 자코비안 "H_j" 와 Kalman Gain "K" 를 활용하여 계산할 수 있다.
+
+- 측정 에러와 새로운 예측을 반복하여 매 Sample 마다 위치 예측을 갱신한다.
+
 ---
 
 [//]: # (Image References)
@@ -48,7 +100,3 @@ The program consists of five modules:
 *  `plot.py` creates a 2D plot comparing the ground truth against our estimation. The following figure illustrates an example:
 
 ![Testing of EKF with Sensor Fusion][EKF-results]
-
-### Assignment
-
-Complete the implementation of EKF with sensor fusion by writing the function `update_ekf()` in the module `kalman_filter`. Details are given in class and instructions are included in comments.

code/week-4/README.md

@@ -8,6 +8,126 @@
 
 ## Assignment
 
+[HW_week4] 
+[1] Update Weights
+
+ # Update the weights of each particle using a multi-variate
+    #   Gaussian distribution.
+    def update_weights(self, sensor_range, std_landmark_x, std_landmark_y,
+                       observations, map_landmarks):
+        import math
+       # TODO: For each particle, do the following:
+        # 1. Select the set of landmarks that are visible
+        #    (within the sensor range)
+
+        for p in self.particles:
+            visible_landmarks = []
+            for id, landmark in map_landmarks.items():
+
+                if distance(p, landmark) < sensor_range:
+                    visible = {'x': landmark['x'], 
+                               'y': landmark['y'], 'id': id}
+                    visible_landmarks.append(visible)
+
+- distance(p, landmark) 함수를 이용하여 map에 표시된 랜드마크와 파티클 사이의 거리를 계산한다.
+- 계측된 거리가 Sensor range 보다 작으면 visible 함수에 랜드마크 id와 x, y 값을 저장한다.                      
+
+       # 2. Transform each observed landmark's coordinates from the
+        #    particle's coordinate system to the map's coordinates.
+
+            transformed_obs = []
+            for obs in observations:
+                t_observ = {}
+                t_observ['x'] = p['x'] + (obs['x'] * np.cos(p['t'])) - \ (obs['y'] * np.sin(p['t']))
+                t_observ['y'] = p['y'] + (obs['x'] * np.sin(p['t'])) + \ (obs['y'] * np.cos(p['t']))
+                transformed_obs.append(t_observ)
+
+            if len(visible_landmarks) == 0:
+                continue
+
+- 계측된 랜드마크의 x, y 좌표를 Map 기준의 global 좌표로 변환시킨다.
+
+       # 3. Associate each transformed observation to one of the
+        #    predicted (selected in Step 1) landmark positions.
+        #    Use self.associate() for this purpose - it receives
+        #    the predicted landmarks and observations; and returns
+        #    the list of landmarks by implementing the nearest-neighbour
+        #    association algorithm.
+
+             assoc_landmarks = self.associate(visible_landmarks, transformed_obs)
+            p['assoc'] = [landmark['id'] for landmark in assoc_landmarks]
+
+- Sensor range 내의 랜드마크와 global 좌표로 변환된 랜드마크를 매칭 시킨다.
+- 가장 가까운 랜드마크를 반환하기 위해 랜드마크의 id 를 "assoc" 의 값으로 입력한다.            
+
+       # 4. Calculate probability of this set of observations based on
+        #    a multi-variate Gaussian distribution (two variables being
+        #    the x and y positions with means from associated positions
+        #    and variances from std_landmark_x and std_landmark_y).
+        #    The resulting probability is the product of probabilities
+        #    for all the observations.
+
+            for t, a in zip(transformed_obs, assoc_landmarks):
+
+                Gaussian_x = norm_pdf(t['x'], a['x'], std_landmark_x)
+                Gaussian_y = norm_pdf(t['y'], a['y'], std_landmark_y)
+
+                Gaussian = Gaussian_x * Gaussian_y
+
+- transformed 랜드마크가 해당 위치에 존재할 확률은 multi-variate Gaussian distribution 으로 계산 가능하며,              
+assoc의 랜드마크 좌표 x, y가 표준 편차이여야 한다.             
+
+
+      # 5. Update the particle's weight by the calculated probability.
+
+                 p['w'] *= Gaussian
+
+- transformed 의 랜드마크에 대한 확률 값을 Gaussian 으로 구하여 최종 값이 Particle 의 Weight로 곱해진다.
+- 랜드마크 매칭 및 파티클 확률을 가우시안 분포로 계산하고, Particle weight 를 업데이트한다.   
+
+[2] Resample
+
+ # Resample particles with replacement with probability proportional to
+    #   their weights.
+
+    def resample(self):
+
+        # TODO: Select (possibly with duplicates) the set of particles
+        #       that captures the posteior belief distribution, by
+        # 1. Drawing particle samples according to their weights.
+
+        weights = [p['w'] for p in self.particles.copy()] 
+        resampled_particles = []
+        positions = (np.arange(len(weights)) + np.random.random()) / len(weights)
+
+        idx = np.zeros(len(weights), 'i')
+        cum_sum = np.cumsum(weights)
+
+        i, j = 0, 0
+        while i < len(weights) and j< len(weights) :
+            if positions[i] < cum_sum[j]:
+                idx[i] = j
+                i += 1
+            else:
+                j += 1
+
+- 파티클 weight 에 따라 resampling 되며 가중치가 크면 여러 번, 가중치가 작으면 클 때보다는 작게 sampling 된다.
+- weight idx 함수를 이용하여 다음 주기의 Particle 에 weihted sampling 하게 된다.
+
+        # 2. Make a copy of the particle; otherwise the duplicate particles
+        #    will not behave independently from each other - they are
+        #    references to mutable objects in Python.
+        #    Finally, self.particles shall contain the newly drawn set of
+        #    particles.
+
+       resampled_particles.append(self.particles[idx[i]])
+        self.particles = resampled_particles
+
+        # return
+
+- resampling 된 파티클을 복사하여 재입력한다.
+
+
 You will complete the implementation of a simple particle filter by writing the following two methods of `class ParticleFilter` defined in `particle_filter.py`:
 
 * `update_weights()`: For each particle in the sample set, calculate the probability of the set of observations based on a multi-variate Gaussian distribution.

code/week-4/helpers.py

@@ -2,7 +2,14 @@
 
 # Calculate Euclidean distance between two 2-D points.
 def distance(p1, p2):
+
     dx = p1['x'] - p2['x']
     dy = p1['y'] - p2['y']
     d = np.sqrt(dx ** 2 + dy ** 2)
     return d
+
+
+def norm_pdf(x, m, s):
+
+    one_over_sqrt_2pi = 1 / np.sqrt(2 * np.pi)
+    return (one_over_sqrt_2pi / s) * np.exp(-0.5 * ((x - m) / s) ** 2)