def get_effective_measurement(self, t):
# individual measures in array
zs = np.array(
[sensor.measure(self.target, t) for sensor in self.radars])
# convert to cartesian
cart_zs = np.array(
[Radar.cartesian_measurement(measurement) for measurement in zs])
# add the radar's position
cart_zs = np.array([
cart_zs[i] + self.radars[i].location[:2]
for i in range(len(self.radars))
])
# inverted individual covs (R_k^{-1})
Rs = [
np.linalg.inv(
Radar.cartesian_error_covariance(zs[i],
self.radars[i].sigma_range,
self.radars[i].sigma_azimuth))
for i in range(len(self.radars))
]
# effective cov
Rk = np.linalg.inv(np.sum(Rs, axis=0))
# values for the sum to obtain z_k
zk_sum_values = np.array(
[np.matmul(Rs[i], cart_zs[i]) for i in range(len(self.radars))])
# effective measurement
zk = np.matmul(Rk, np.sum(zk_sum_values, axis=0))
return zk, Rk
def exercise_4():
colors = ['b', 'g', 'm', 'c', 'y']
car = target
time = np.linspace(0, car.location[0] / car.v, 900)
trajectory = np.array([car.position(t) for t in time])
# 4.2 Measurements transformation
fig = plt.figure()
for i, radar in enumerate(radars, start=1):
measurements = [radar.measure(car, t) for t in time]
trans_measures = np.array([
Radar.cartesian_measurement(m) + radar.location[:2]
for m in measurements
])
plt.plot(trans_measures[:, 0],
trans_measures[:, 1],
c=colors[(i % 5) - 1],
label='Radar %s Measurements' % i)
plt.plot(trajectory[:, 0],
trajectory[:, 1],
c='r',
label='Real trajectory')
plt.title('Trajectory and Radars Measurements')
plt.legend()
plt.xlabel('X-axis (m)')
plt.ylabel('Y-axis (m)')
plt.show()
# 4.3 Kalman Filter
time_limit = car.location[0] / car.v
kalman = KalmanFilter(radars, car, delta_t=2)
kalman.filter(time_limit)
fig = plt.figure()
plt.plot(trajectory[:, 0],
trajectory[:, 1],
c='r',
label='Target Trajectory')
plt.plot(kalman.track[:, 0], kalman.track[:, 1], c='g', label='Track')
plt.xlabel('X-axis (m)')
plt.ylabel('Y-axis (m)')
plt.title('Real Trajectory vs Track using Kalman Filter')
plt.legend(loc='upper right')
plt.xlim(-1000, 12000)
plt.ylim(-2000, 2000)
# plt.show()
kalman.d_retro(time_limit)
#
fig = plt.figure()
plt.plot(trajectory[:, 0],
trajectory[:, 1],
c='r',
label='Target Trajectory')
plt.plot(kalman.no_filtered_track[:, 0],
kalman.no_filtered_track[:, 1],
c='y',
label='Predicted Track')
plt.plot(kalman.track[:, 0],
kalman.track[:, 1],
c='g',
label='Filtered Track')
plt.plot(kalman.retro_track[:, 0],
kalman.retro_track[:, 1],
c='b',
label='Track with retrodiction')
plt.xlabel('X-axis (m)')
plt.ylabel('Y-axis (m)')
plt.title(
'Real Trajectory vs Track using Kalman Filter vs Track using retrodiction'
)
plt.legend(loc='upper right')
plt.xlim(-1000, 12000)
plt.ylim(-2000, 2000)
plt.show()
# error calculation:
err = np.sum(
np.sqrt(
np.sum((trajectory[:, 0] - kalman.retro_track[:, 0])**2 +
(trajectory[:, 1] - kalman.retro_track[:, 1])**2)))
print('Error of the track when applying retrodiction: ')
print(err)
# error calculation:
err = np.sum(
np.sqrt(
np.sum((trajectory[:, 0] - kalman.track[:, 0])**2 +
(trajectory[:, 1] - kalman.track[:, 1])**2)))
print('Error of the track without retrodiction: ')
print(err)
