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)
#from data import measurements
#from data import true_velocity
from data_test_input import measurements
from data_test_input import true_velocity
from kalman import KalmanFilter
from plot import plot_graphs
dt = 0.1
A = np.array([[1, dt], [0, 1]])
# if we measure the velocity instead of position,
# we have to modify the measurement matrix(C) from [1, 0] to [0, 1]
C = np.array([[0, 1]])
R = np.array([[1, 0], [0, 3]])
Q = np.array([10])
KF = KalmanFilter(A, C, R, Q)
filtered_positions = []
filtered_velocity = []
for m in measurements:
x = KF.filter(m, dt)
filtered_positions.append(x[0])
filtered_velocity.append(x[1])
plot_graphs(measurements, filtered_positions, true_velocity, filtered_velocity)
