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)