-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathplotObserverData.py
executable file
·140 lines (124 loc) · 5.66 KB
/
plotObserverData.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
import matplotlib.pyplot as plt
from matplotlib.lines import Line2D
import autograd.numpy as np
plt.ion() # enable interactive drawing
class plotObserverData:
'''
This class plots the time histories for the ballbeam states and state estimates.
'''
def __init__(self):
# Number of subplots = num_of_rows*num_of_cols
self.num_rows = 3 # Number of subplot rows
self.num_cols = 2 # Number of subplot columns
# Crete figure and axes handles
self.fig, self.ax = plt.subplots(self.num_rows, self.num_cols, sharex=True)
# Instantiate lists to hold the time and data histories
self.time_history = [] # time
self.z_history = [] # position z
self.z_hat_history = [] # estimate of z
self.h_history = [] # altitude h
self.h_hat_history = [] # estimate of h
self.theta_history = [] # angle theta
self.theta_hat_history = [] # estimate of theta
self.z_dot_history = []
self.z_hat_dot_history = []
self.h_dot_history = []
self.h_hat_dot_history = []
self.theta_dot_history = []
self.theta_hat_dot_history = []
# create a handle for every subplot.
self.handle = []
self.handle.append(myPlot(self.ax[0][0], ylabel='z (m)', title='VTOL States'))
self.handle.append(myPlot(self.ax[1][0], ylabel='h (m)'))
self.handle.append(myPlot(self.ax[2][0], ylabel='theta (deg)'))
self.handle.append(myPlot(self.ax[0][1], ylabel='z_dot (m/s)'))
self.handle.append(myPlot(self.ax[1][1], ylabel='h_dot (m/s)'))
self.handle.append(myPlot(self.ax[2][1], xlabel='t(s)', ylabel='theta_dot (deg/s)'))
def updatePlots(self, tt, x, x_hat):
'''
Add to the time and data histories, and update the plots.
'''
# update the time history of all plot variables
self.time_history.append(tt) # time
self.z_history.append(x[0])
self.h_history.append(x[1])
self.theta_history.append(x[2])
self.z_dot_history.append(x[3])
self.h_dot_history.append(x[4])
self.theta_dot_history.append(x[5])
self.z_hat_history.append(x_hat[0])
self.h_hat_history.append(x_hat[1])
self.theta_hat_history.append(x_hat[2])
self.z_hat_dot_history.append(x_hat[3])
self.h_hat_dot_history.append(x_hat[4])
self.theta_hat_dot_history.append(x_hat[5])
# update the plots with associated histories
self.handle[0].updatePlot(self.time_history, [self.z_history, self.z_hat_history])
self.handle[1].updatePlot(self.time_history, [self.h_history, self.h_hat_history])
self.handle[2].updatePlot(self.time_history, [self.theta_history, self.theta_hat_history])
self.handle[3].updatePlot(self.time_history, [self.z_dot_history, self.z_hat_dot_history])
self.handle[4].updatePlot(self.time_history, [self.h_dot_history, self.h_hat_dot_history])
self.handle[5].updatePlot(self.time_history, [self.theta_dot_history, self.theta_hat_dot_history])
class myPlot:
'''
Create each individual subplot.
'''
def __init__(self, ax,
xlabel='',
ylabel='',
title='',
legend=None):
'''
ax - This is a handle to the axes of the figure
xlable - Label of the x-axis
ylable - Label of the y-axis
title - Plot title
legend - A tuple of strings that identify the data.
EX: ("data1","data2", ... , "dataN")
'''
self.legend = legend
self.ax = ax # Axes handle
self.colors = ['b', 'g', 'r', 'c', 'm', 'y', 'b']
# A list of colors. The first color in the list corresponds
# to the first line object, etc.
# 'b' - blue, 'g' - green, 'r' - red, 'c' - cyan, 'm' - magenta
# 'y' - yellow, 'k' - black
self.line_styles = ['-', '-', '--', '-.', ':']
# A list of line styles. The first line style in the list
# corresponds to the first line object.
# '-' solid, '--' dashed, '-.' dash_dot, ':' dotted
self.line = []
# Configure the axes
self.ax.set_ylabel(ylabel)
self.ax.set_xlabel(xlabel)
self.ax.set_title(title)
self.ax.grid(True)
# Keeps track of initialization
self.init = True
def updatePlot(self, time, data):
'''
Adds data to the plot.
time is a list,
data is a list of lists, each list corresponding to a line on the plot
'''
if self.init == True: # Initialize the plot the first time routine is called
for i in range(len(data)):
# Instantiate line object and add it to the axes
self.line.append(Line2D(time,
data[i],
color=self.colors[np.mod(i, len(self.colors) - 1)],
ls=self.line_styles[np.mod(i, len(self.line_styles) - 1)],
label=self.legend if self.legend != None else None))
self.ax.add_line(self.line[i])
self.init = False
# add legend if one is specified
if self.legend != None:
plt.legend(handles=self.line)
else: # Add new data to the plot
# Updates the x and y data of each line.
for i in range(len(self.line)):
self.line[i].set_xdata(time)
self.line[i].set_ydata(data[i])
# Adjusts the axis to fit all of the data
self.ax.relim()
self.ax.autoscale()