class Animation(object):
def __init__(self,
ax1,
ax2,
ax3,
ax4,
ax5,
air,
sensor_list,
sen_num=6,
delta_t=5):
"""Setting up all local variables and calculate plot data"""
# set up objects
self.ax1 = ax1
self.ax2 = ax2
self.ax3 = ax3
self.ax4 = ax4
self.ax5 = ax5
# set aircraft object
self.air = air
# add kalman object
self.kalman_filter = KalmanFilter(air, 5, sensor_list)
# variable number of sensors
self.sensors = sensor_list
self.sen_num = sen_num
self.delta_t = delta_t
# set up lines
# object trace
self.ln_trace = Line2D([], [], color='b')
# moving object
self.ln_object = Line2D([], [], color='k', marker='o')
# prediction
self.ln_prediction = Line2D([], [], color='m', marker='o', linewidth=0)
self.ln_pred_filtered = Line2D([], [],
color='m',
marker='^',
linewidth=1)
# polar measurement for each sensor
self.ln_meas = Line2D([], [], color='g', marker='x', linewidth=0)
# fused polar measurements
self.ln_fused = Line2D([], [], color='r', marker='^', linewidth=0)
# acceleration
self.ln_acc = Line2D([], [], color='b')
# velocity
self.ln_vel = Line2D([], [], color='b')
# vector products with r''(t) & t(t)
self.ln_vec_t = Line2D([], [], color='b')
# vector products with r''(t) & n(t)
self.ln_vec_n = Line2D([], [], color='b')
# set up ax1
self.ax1.set_xlim(-12000, 12000)
self.ax1.set_ylim(-12000, 12000)
ax1.set_title("aircraft trajectory")
self.ax1.set_xlabel("y in meter")
self.ax1.set_ylabel("x in meter")
ax1.grid(True)
# add lines to ax1
ax1.add_line(self.ln_trace)
ax1.add_line(self.ln_object)
ax1.add_line(self.ln_prediction)
ax1.add_line(self.ln_meas)
ax1.add_line(self.ln_fused)
ax1.add_line(self.ln_pred_filtered)
# set up legend
legend_lines = [
self.ln_prediction, self.ln_pred_filtered, self.ln_meas,
self.ln_fused
]
box = ax1.get_position()
ax1.set_position(
[box.x0, box.y0 + box.height * 0.1, box.width, box.height * 0.9])
# Put a legend below current axis
ax1.legend(legend_lines, [
'prediction', 'filtered prediction', 'polar measurements',
'fused measurements'
],
loc='upper center',
bbox_to_anchor=(0.5, -0.05),
fancybox=True,
shadow=True,
ncol=5)
# set up ax2
self.ax2.set_xlim(-10, 430)
self.ax2.set_ylim(0, 400)
ax2.set_title("aircraft velocity")
self.ax2.set_xlabel("r'(t) in meter")
self.ax2.set_ylabel("t in meter")
ax2.grid(True)
ax2.add_line(self.ln_vel)
# set up ax3
self.ax3.set_xlim(-10, 430)
self.ax3.set_ylim(-1, 10)
ax3.set_title("aircraft acceleration")
self.ax3.set_xlabel("r''(t) in m/s^2")
self.ax3.set_ylabel("t in meter")
ax3.grid(True)
ax3.add_line(self.ln_acc)
# set up ax4
self.ax4.set_xlim(-10, 430)
self.ax4.set_ylim(-10, 10)
ax4.set_title("r''(t)t(t)")
self.ax4.set_xlabel("r''(t)t(t) in m/s^2")
self.ax4.set_ylabel("t in meter")
ax4.grid(True)
ax4.add_line(self.ln_vec_t)
# set up ax5
self.ax5.set_xlim(-10, 430)
self.ax5.set_ylim(-10, 10)
ax5.set_title("r''(t)n(t)")
self.ax5.set_xlabel("r''(t)n(t) in m/s^2")
self.ax5.set_ylabel("t in meter")
ax5.grid(True)
ax5.add_line(self.ln_vec_n)
# set up data for lines
# time array
self.t = np.arange(0, 420, 1)
# velocity of aircraft
self.v_abs = np.array(
[np.linalg.norm(air.get_velocity(i)) for i in self.t])
# acceleration of aircraft
self.q_abs = np.array(
[np.linalg.norm(air.get_acceleration(i)) for i in self.t])
# tangential vector of aircraft
self.tang = np.array([
np.dot(air.get_acceleration(i), air.get_tangential(i))
for i in self.t
])
# normal vector of aircraft
self.norm = np.array([
np.dot(air.get_acceleration(i), air.get_normal(i)) for i in self.t
])
# prediction
self.prediction_x = np.zeros(420)
self.prediction_y = np.zeros(420)
# filtered prediction
self.pred_filtered_x = np.zeros(420)
self.pred_filtered_y = np.zeros(420)
# polar measurement transformed to cartesian for each sensor
self.polar_meas_x = np.zeros((self.sen_num, 420))
self.polar_meas_y = np.zeros((self.sen_num, 420))
# fused polar measurements transformed to cartesian
self.polar_fused_x = np.zeros(420)
self.polar_fused_y = np.zeros(420)
# calculate sensor data for each instance delta t
for i in self.t:
if i % self.delta_t == 0 or i == 0:
# get fused polar measurements
self.kalman_filter.update_polar(i)
self.polar_fused_x[i] = self.kalman_filter.z[0]
self.polar_fused_y[i] = self.kalman_filter.z[1]
# get prediction after filtering
# self.kalman_filter.update_polar(i)
self.pred_filtered_x[i] = self.kalman_filter.x[0]
self.pred_filtered_y[i] = self.kalman_filter.x[1]
# get measurement for each sensor
for j, arg in enumerate(self.sensors):
if isinstance(arg, Sensor):
arg.update_stats(i)
z = arg.z_r * np.array(
[np.cos(arg.z_az),
np.sin(arg.z_az)]).reshape((2, 1)) + arg.pos
self.polar_meas_x[j, i] = z[0]
self.polar_meas_y[j, i] = z[1]
else:
# fill up filtered prediction
self.pred_filtered_x[i] = self.pred_filtered_x[i - 1]
self.pred_filtered_y[i] = self.pred_filtered_y[i - 1]
# calc normal prediction
self.kalman_filter.x, self.kalman_filter.P = self.kalman_filter.prediction(
i)
self.prediction_x[i] = self.kalman_filter.x[0]
self.prediction_y[i] = self.kalman_filter.x[1]
# fill up measurements of each sensor
for j, arg in enumerate(self.sensors):
if isinstance(arg, Sensor):
# arg.update_stats(i)
self.polar_meas_x[j, i] = self.polar_meas_x[j, i - 1]
self.polar_meas_y[j, i] = self.polar_meas_y[j, i - 1]
# calculate position
self.pos_x = np.array([self.air.get_position(i)[0] for i in self.t])
self.pos_y = np.array([self.air.get_position(i)[1] for i in self.t])
def init_plot(self):
"""Setting all initial values for the data plots"""
lines = [
self.ln_trace, self.ln_object, self.ln_vel, self.ln_acc,
self.ln_vec_t, self.ln_vec_n, self.ln_prediction,
self.ln_pred_filtered, self.ln_meas, self.ln_fused
]
for l in lines:
l.set_data([], [])
# set up sensor position for variable num of sensors
for arg in self.sensors:
if isinstance(arg, Sensor):
self.ax1.plot(arg.pos[0], arg.pos[1], 'go')
return lines
def __call__(self, i):
"""Updates for each frame the animation"""
self.air.update_stats(i)
x = self.air.position[0]
y = self.air.position[1]
# update aircraft trajectory
self.ln_trace.set_data(self.pos_x[:i], self.pos_y[:i])
# update aircraft position
self.ln_object.set_data(x, y)
# update aircraft velocity
self.ln_vel.set_data(self.t[:i], self.v_abs[:i])
# update aircraft acceleration
self.ln_acc.set_data(self.t[:i], self.q_abs[:i])
# update rt
self.ln_vec_t.set_data(self.t[:i], self.norm[:i])
# update rn
self.ln_vec_n.set_data(self.t[:i], self.tang[:i])
# kalman plot prediction BEFORE filter
# show last 5 predictions
if i > 5:
self.ln_prediction.set_data(self.prediction_x[i - 4:i + 1],
self.prediction_y[i - 4:i + 1])
# kalman plot prediction AFTER filter
self.ln_pred_filtered.set_data(self.pred_filtered_x[:i],
self.pred_filtered_y[:i])
# kalman plot measurements for each sensor
self.ln_meas.set_data(self.polar_meas_x[:, :i],
self.polar_meas_y[:, :i])
# kalman plot fused measurement
if i > 50:
self.ln_fused.set_data(self.polar_fused_x[i - 49:i + 1],
self.polar_fused_y[i - 49:i + 1])
# plot quiver for object
q_tang = self.ax1.quiver(x,
y,
self.air.tang[0],
self.air.tang[1],
pivot='tail',
color='black',
width=0.004,
angles='xy',
scale=35)
# plot normal vector
q_norm = self.ax1.quiver(x,
y,
self.air.norm[0],
self.air.norm[1],
pivot='tail',
color='black',
width=0.004,
scale=35)
artists = [
self.ln_trace, self.ln_object, self.ln_vel, self.ln_acc,
self.ln_vec_t, self.ln_vec_n, self.ln_prediction,
self.ln_pred_filtered, self.ln_meas, self.ln_fused, q_norm, q_tang
]
# plot quivers for each sensor
for j, arg in enumerate(self.sensors):
if isinstance(arg, Sensor):
arg.update_stats(i)
# all vectors pointing to their own measurement
# z = arg.z_r * np.array([np.cos(arg.z_az), np.sin(arg.z_az)]).reshape((2, 1)) + arg.pos
z = np.array(
[self.polar_meas_x[j, i], self.polar_meas_y[j, i]])
artists.append(
ax1.quiver(arg.pos[0],
arg.pos[1],
z[0] - arg.pos[0],
z[1] - arg.pos[1],
pivot='tail',
color='green',
angles='xy',
units='xy',
scale=1,
scale_units='xy',
width=70))
return artists
