def update_ekf(self, z): def calc_h_x(x): a = sqrt(x[0] ** 2 + x[1] ** 2) b = atan2(x[1], x[0]) c = (x[0] * x[2] + x[1] * x[3]) / a return np.array([a, b, c]) # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R # 3. Calculate Kalman gain K = P' * H_j^T * S^-1 K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') y = z - calc_h_x(self.x) # 5. Normalize phi so that it is between -PI and +PI # y[1] %= np.pi if y[1] >= 0: y[1] %= np.pi else: y[1] = -(-y[1] % np.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 = np.dot((np.eye(4) - np.dot(K, H_j)), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') px, py, vx, vy = self.x # h(x') h_x_prime = np.zeros(3) # RHO P h_x_prime[0] = sqrt(px * px + py * py) # THETA P h_x_prime[1] = atan2(py, px) # RHO DOT P h_x_prime[2] = (px * vx + py * vy) / h_x_prime[0] y = z - h_x_prime # 5. Normalize phi so that it is between -PI and +PI if y[1] > np.pi: y[1] = y[1] - 2 * np.pi elif y[1] < -np.pi: y[1] = y[1] + 2 * np.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 = np.dot((np.eye(4, dtype=float) - np.dot(K, H_j)), self.P)
def update_ekf(self, z): self.Hj = Jacobian(self.x) S = np.dot(np.dot(self.Hj, self.P), self.Hj.T) + self.R K = np.dot(np.dot(self.P, self.Hj.T), np.linalg.inv(S)) px, py, vx, vy = self.x rho = np.sqrt(px * px + py * py) phi = np.arctan2(py, px) rhodot = (px * vx + py * vy) / np.sqrt(px * px + py * py) hx = [rho, phi, rhodot] y = z - hx while (y[1] > np.pi or y[1] < -np.pi): if y[1] > np.pi: y[1] -= np.pi else: y[1] += np.pi self.x = self.x + np.dot(K, y) I = np.identity(len(self.x)) self.P = np.dot((I - np.dot(K, self.Hj)), 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 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)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R # 3. Calculate Kalman gain K = P'*H_j^T * S^-1 K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') px, py, vx, vy = self.x rho_est = sqrt(px**2 + py**2) phi_est = atan2(py, px) rho_dot_est = (px * vx + py * vy) / sqrt(px**2 + py**2) y = z - np.array([rho_est, phi_est, rho_dot_est], dtype=np.float32) # 5. Normalize phi so that it is between -PI and +PI phi = y[1] while phi > pi: phi -= 2 * pi while phi < -pi: phi += 2 * pi y[1] = phi # 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)
def update_ekf(self, z): H_j = Jacobian(self.x) S = np.dot(H_j, np.dot(self.P, H_j.T)) + self.R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) px = self.x[0] py = self.x[1] vx = self.x[2] vy = self.x[3] H_x = [ sqrt(px * px + py * py), atan2(py, px), (px * vx + py * vy) / sqrt(px * px + py * py) ] y = z - H_x if y[1] > np.pi: y[1] += -2 * np.pi elif y[1] < -np.pi: y[1] += 2 * np.pi self.x = self.x + np.dot(K, y) print([ "SELF.X : ", round(self.x[0], 2), round(self.x[1], 2), round(self.x[2], 2), round(self.x[3], 2) ]) self.P = np.dot((np.eye(len(self.P)) - np.dot(K, H_j)), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j # 2. Calculate S = H_j * P' * H_j^T + R # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R # 4. Estimate y = z - h(x') # 5. Normalize phi so that it is between -PI and +PI # 6. Calculate new estimates # x = x' + K * y # P = (I - K * H_j) * P print("self.x", self.x) # 1. H_j = Jacobian(self.x) S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. px, py, vx, vy = self.x rho = np.sqrt(px**2 + py**2) phi = np.arctan2(py, px) rho_dot = (px * vx + py * vy) / rho y = z - np.array([rho, phi, rho_dot]) y[1] = normalizeAngle(y[1]) self.x = self.x + np.dot(K, y) self.P = np.dot((np.identity(4) - np.dot(K, H_j)), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R # 3. Calculate Kalman gain K = P' * H_j^T * S^-1 K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') px, py, vx, vy = self.x h_x_dot = np.array([ sqrt(px * px + py * py), atan2(py, px), (px * vx + py * vy) / sqrt(px * px + py * py) ]) y = z - h_x_dot # 5. Normalize phi so that it is between -PI and +PI if y[1] > pi: while y[1] > pi: y[1] -= 2 * pi if y[1] < pi: while y[1] < -pi: y[1] += 2 * pi # 6. Calculate new estimates # x = x' + K * y self.x = self.x + np.dot(K, y) # P = (I - K * H_j) * P self.P = np.dot((np.identity(K.shape[0]) - np.dot(K, H_j)), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j # 2. Calculate S = H_j * P' * H_j^T + R # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R # 4. Estimate y = z - h(x') # 5. Normalize phi so that it is between -PI and +PI # 6. Calculate new estimates # x = x' + K * y # P = (I - K * H_j) * P H_j = Jacobian(self.x) S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) px, py, vx, vy = self.x h_x_1 = px * px + py * py h_x_2 = sqrt(h_x_1) h_x_3 = atan2(py, px) h_x_4 = (px * vx + py * vy) / h_x_2 y = z - [h_x_2, h_x_3, h_x_4] if y[1] > np.pi: y[1] = y[1] - 2 * np.pi elif y[1] < -np.pi: y[1] = y[1] + 2 * np.pi self.x = self.x + np.dot(K, y) self.P = np.dot((np.eye(4, dtype=float) - np.dot(K, H_j)), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j Hj = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(Hj, self.P), Hj.T) + self.R # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R K = np.dot(np.dot(self.P, Hj.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') sqrt_x = sqrt(self.x[0] * self.x[0] + self.x[1] * self.x[1]) tan_1 = atan2(self.x[1], self.x[0]) sqrt_x_2 = (self.x[0] * self.x[2] + self.x[1] * self.x[3]) / sqrt_x hx = np.array([sqrt_x, tan_1, sqrt_x_2]) y = z - hx # 5. Normalize phi so that it is between -PI and +PI # y[1] = y[1] / (math.pi*2) if y[1] < 0: y[1] = y[1] % -3.14 else: y[1] = y[1] % 3.14 # 6. Calculate new estimates # x = x' + K * y # P = (I - K * H_j) * P self.x = self.x + np.dot(K, y) IK = np.eye(4) - np.dot(K, Hj) self.P = np.dot(IK, self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j # 2. Calculate S = H_j * P' * H_j^T + R # 3. Calculate Kalman gain K = P' * H_j^T * S^-1 # 4. Estimate y = z - h(x') # 5. Normalize phi so that it is between -PI and +PI # 6. Calculate new estimates # x = x' + K * y # P = (I - K * H_j) * P ## the function to normalize phi into between -PI and +PI def norm_phi(angle): if angle < 0: angle *= -1 angle %= np.pi angle *= -1 else: angle = angle % np.pi return angle # h(x') function def radar_h(x): sqrt_x = sqrt(x[0] * x[0] + x[1] * x[1]) atan_val = atan2(x[1], x[0]) row = (x[0] * x[2] + x[1] * x[3]) / sqrt_x h_x = np.array([sqrt_x, atan_val, row]) return h_x ## 1. compute Jacobian Matrix H_j H_j = Jacobian(self.x) ## 2. Calculate S = H_j*P'*H_j^T S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R ## 3. Calcualte Kalman gain K = P' * H_j * S^-1 K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) ## 4. Estimate y = z - h(x') y = z - radar_h(self.x) ## 5. Normalize phi so that it is between -PI and +PI _y_1 = norm_phi(y[1]) y[1] = _y_1 ## 6. Calculate new estimates self.x = self.x + np.dot(K, y) I = np.eye(4) self.P = np.dot(I - np.dot(K, H_j), self.P)
def update_ekf(self, z): def H(px, py, vx, vy): return np.array([ np.sqrt(px**2 + py**2), np.arctan(py / px) + (0 if (px >= 0) else (np.pi * ((py >= 0) * 2 - 1))), (px * vx + py * vy) / np.sqrt(px**2 + py**2) ]) px, py, vx, vy = self.x y = z - H(px, py, vx, vy) y[1] = y[1] % -np.pi if y[1] < 0 else y[1] % np.pi H_j = Jacobian(self.x) S = H_j.dot(self.P).dot(H_j.T) + self.R K = self.P.dot(H_j.T).dot(np.linalg.inv(S)) self.x += np.dot(K, y) self.P -= np.dot(np.dot(K, H_j), self.P)
def update_ekf(self, z): H_j = Jacobian(self.x) S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) y = z - compute_h_x(self.x) if y[1] >= 0: y[1] %= np.pi else: y[1] = -(-y[1] % np.pi) self.x = self.x + np.dot(K, y) self.P = np.dot((np.eye(4) - np.dot(K, H_j)), self.P)
def update_ekf(self, z): H_j = Jacobian(self.x) S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) rad = sqrt(self.x[0] ** 2 + self.x[1] ** 2) rad_v = ((self.x[0] * self.x[2]) + (self.x[1] * self.x[3])) / rad hx = np.array([rad, atan2(self.x[1], self.x[0]), rad_v]) y = z - hx y[1] = y[1] % -np.pi if y[1] < 0 else y[1] % np.pi self.x += np.dot(K, y) self.P = self.P - np.dot(np.dot(K, H_j), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j # H_j = [ # [drho/dp_x, drho/dp_y, drho/dv_x, drho/dv_y] # [dphi/dp_x, dphi/dp_y, dphi/dv_x, dphi/dv_y] # [drho_dot/dp_x, drho_dot/dp_y, drho_dot/dv_x, drho_dot/dv_y] # ] # p_x = self.x[0], # p_y = self.x[1], # v_x = self.x[2], v_y = self.x[3] # H_j = [ # [self.x[0] / np.sqrt(np.power(self.x[0],2)+np.power(self.x[1],2)), # self.x[1] / np.sqrt(np.power(self.x[0],2)+np.power(self.x[1],2)), 0, 0], # [-(self.x[1] / (np.power(self.x[0],2)+np.power(self.x[1],2))), # self.x[0] / (np.power(self.x[0],2)+np.power(self.x[1],2)), 0, 0], # [(self.x[1]*((self.x[2]*self.x[1])-(self.x[3]*self.x[0]))) / (np.power((np.power(self.x[0],2)+np.power(self.x[1],2)),(3/2))), # (self.x[0]*((self.x[3]*self.x[0])-(self.x[2]*self.x[1]))) / (np.power((np.power(self.x[0],2)+np.power(self.x[1],2)),(3/2))), # self.x[0] / np.sqrt(np.power(self.x[0],2)+np.power(self.x[1],2)), # self.x[1] / np.sqrt(np.power(self.x[0],2)+np.power(self.x[1],2))] # ] H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R # 3. Calculate Kalman gain K = P' * H_j^T * (S)^-1 K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') y = z - [np.sqrt(np.power(self.x[0], 2)+np.power(self.x[1], 2)), atan2(self.x[1], self.x[0]), (self.x[0]*self.x[2]+self.x[1]*self.x[3])/(np.sqrt(np.power(self.x[0], 2)+np.power(self.x[1], 2)))] # 5. Normalize phi so that it is between -PI and +PI if y[1] > np.pi : y[1] = 2*np.pi - y[1] elif y[1] < -np.pi : y[1] = 2*np.pi + y[1] # 6. Calculate new estimates # x = x' + K * y self.x = self.x + np.dot(K, y) # P = (I - K * H_j) * P self.P = self.P - np.dot(np.dot(K, H_j), self.P)
def update_ekf(self, z): ######################################################### # TODO: Implement EKF update for radar measurements # # 1. Compute Jacobian Matrix H_j # # 2. Calculate S = H_j * P' * H_j^T + R # # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R # # 4. Estimate y = z - h(x') # # 5. Normalize phi so that it is between -PI and +PI # # 6. Calculate new estimates # # x = x' + K * y # # P = (I - K * H_j) * P # ######################################################### # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R # 3. Calculate Kalman gain K = P * H_j^T * S^-1 S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # calculate h(x') px, py, vx, vy = self.x c1 = (px * px) + (py * py) c2 = sqrt(c1) rho = c2 phi = np.arctan2(py, px) rho_dot = (px * vx + py * vy) / c2 h_of_x = np.array([rho, phi, rho_dot], dtype=np.float32) # 4. Estimate y = z - h(x') y = z - h_of_x # 5. Normalize phi so that it is between -PI and +PI y[1] = normalize(y[1], -math.pi, math.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)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # shape (4,4) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R #shape (3,3) # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) #shape (4,3) # 4. Estimate y = z - h(x') h = np.array([ [sqrt(self.x[0]*self.x[0] + self.x[1]*self.x[1])], [atan2(self.x[1],self.x[0])], [(self.x[0]*self.x[2]+self.x[1]*self.x[3])/(sqrt(self.x[0]*self.x[0] + self.x[1]*self.x[1]))] ], dtype=np.float32) #shape (3,3) h = np.ravel(h) # z1 = np.array([[z[0],], # [z[1],], # [z[2],] # ]) y = z-h #shpae (3,1) if y[1]> np.pi: y[1] = y[1]- 2*np.pi elif y[1]<-np.pi: y[1] = y[1] + 2*np.pi # 5. Normalize phi so that it is between -PI and +PI print(y[1]) # 6. Calculate new estimates # x = x' + K * y self.x = self.x + np.dot(K, y) # P = (I - K * H_j) * P self.P = self.P - np.dot(np.dot(K, H_j), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j # 2. Calculate S = H_j * P' * H_j^T + R # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R # 4. Estimate y = z - h(x') # 5. Normalize phi so that it is between -PI and +PI # 6. Calculate new estimates # x = x' + K * y # P = (I - K * H_j) * P px, py, vx, vy = self.x H_j = Jacobian(self.x) S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) c1 = px * px + py * py c2 = sqrt(c1) c3 = atan2(py, px) c4 = (px * vx + py * vy) / c2 y = z - [c2, c3, c4] if y[1] > math.pi: y[1] = y[1] - 2 * math.pi elif y[1] < -math.pi: y[1] = y[1] + 2 * math.pi self.x = self.x + np.dot(K, y) self.P = self.P - np.dot((np.dot(K, H_j)), self.P) print("x = ") print(self.x) print("P = ") print(self.P) print("R = ") print(self.R) print("S = ") print(S)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') px, py, vx, vy = self.x radar = sqrt(px**2 + py**2) bearing = atan2(py, px) radial_v = ((px * vx) + (py * vy)) / radar hx = np.array([radar, bearing, radial_v]) y = z - hx # 5. Normalize phi so that it is between -PI and +PI y[1] = y[1] % -np.pi if y[1] < 0 else y[1] % np.pi # 6. Calculate new estimates # x = x' + K * y # P = (I - K * H_j) * P self.x += np.dot(K, y) self.P = self.P - np.dot(np.dot(K, H_j), self.P)
def update_ekf(self, z): px, py, vx, vy = self.x Hj = Jacobian(self.x) # 3X4 matrix S = np.dot(np.dot(Hj, self.P), Hj.T) + self.R #3X3 matrix K = np.dot(np.dot(self.P, Hj.T), np.linalg.inv(S)) #4X3 matrix c1 = px * px + py * py c2 = sqrt(c1) hx = [c2, atan2(py,px), (px * vx + py * vy)/c2] PI = 3.14159; y = z - hx if (y[1] > PI) : y[1] = (y[1]-2*PI) if (y[1] < -PI) : y[1] = (y[1]+2*PI) self.x = self.x + np.dot(K, y) self.P = self.P - np.dot(np.dot(K, Hj), self.P)
def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j H_j = Jacobian(self.x) # 2. Calculate S = H_j * P' * H_j^T + R S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R # 3. Calculate Kalman gain K = P'*H_j*S^-1 K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) # 4. Estimate y = z - h(x') px, py, vx, vy = self.x r = sqrt(px * px + py * py) p = atan2(py, px) r_dot = (px * vx + py * vy) / r h = np.array([r, p, r_dot], dtype=np.float32) y = z - h # 5. Normalize phi so that it is between -PI and +PI if (z[1] > pi): z[1] -= 2 * pi elif (z[1] < -pi): z[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 = np.dot(np.eye(4) - np.dot(K, H_j), self.P)