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)