pybounds 0.0.5__py3-none-any.whl → 0.0.6__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Potentially problematic release.
This version of pybounds might be problematic. Click here for more details.
- pybounds/drone_simulator.py +422 -0
- pybounds/observability.py +7 -6
- pybounds/simulator.py +12 -3
- pybounds/util.py +42 -7
- {pybounds-0.0.5.dist-info → pybounds-0.0.6.dist-info}/METADATA +3 -3
- pybounds-0.0.6.dist-info/RECORD +10 -0
- pybounds-0.0.5.dist-info/RECORD +0 -9
- {pybounds-0.0.5.dist-info → pybounds-0.0.6.dist-info}/LICENSE +0 -0
- {pybounds-0.0.5.dist-info → pybounds-0.0.6.dist-info}/WHEEL +0 -0
- {pybounds-0.0.5.dist-info → pybounds-0.0.6.dist-info}/top_level.txt +0 -0
|
@@ -0,0 +1,422 @@
|
|
|
1
|
+
|
|
2
|
+
import warnings
|
|
3
|
+
import numpy as np
|
|
4
|
+
import matplotlib.pyplot as plt
|
|
5
|
+
import do_mpc
|
|
6
|
+
from .util import FixedKeysDict, SetDict
|
|
7
|
+
|
|
8
|
+
|
|
9
|
+
class Simulator(object):
|
|
10
|
+
def __init__(self, f, h, dt=0.01, n=None, m=None,
|
|
11
|
+
state_names=None, input_names=None, measurement_names=None,
|
|
12
|
+
params_simulator=None, mpc_horizon=10):
|
|
13
|
+
|
|
14
|
+
""" Simulator.
|
|
15
|
+
|
|
16
|
+
:param callable f: dynamics function f(X, U, t)
|
|
17
|
+
:param callable h: measurement function h(X, U, t)
|
|
18
|
+
:param float dt: sampling time in seconds
|
|
19
|
+
:param int n: number of states, optional but cannot be set if state_names is set
|
|
20
|
+
:param int m: number of inputs, optional but cannot be set if input_names is set
|
|
21
|
+
:param list state_names: names of states, optional but cannot be set if n is set
|
|
22
|
+
:param list input_names: names of inputs, optional but cannot be set if m is set
|
|
23
|
+
:param list measurement_names: names of measurements, optional
|
|
24
|
+
:param dict params_simulator: simulation parameters, optional
|
|
25
|
+
"""
|
|
26
|
+
|
|
27
|
+
self.f = f
|
|
28
|
+
self.h = h
|
|
29
|
+
self.dt = dt
|
|
30
|
+
|
|
31
|
+
# Set state names
|
|
32
|
+
if state_names is None: # default state names
|
|
33
|
+
if n is None:
|
|
34
|
+
raise ValueError('must set state_names or n')
|
|
35
|
+
else:
|
|
36
|
+
self.n = int(n)
|
|
37
|
+
|
|
38
|
+
self.state_names = ['x_' + str(n) for n in range(self.n)]
|
|
39
|
+
else: # state names given
|
|
40
|
+
if n is not None:
|
|
41
|
+
raise ValueError('cannot set state_names and n')
|
|
42
|
+
|
|
43
|
+
self.state_names = list(state_names)
|
|
44
|
+
self.n = len(self.state_names)
|
|
45
|
+
# if len(self.state_names) != self.n:
|
|
46
|
+
# raise ValueError('state_names must have length equal to x0')
|
|
47
|
+
|
|
48
|
+
# Set input names
|
|
49
|
+
if input_names is None: # default input names
|
|
50
|
+
if m is None:
|
|
51
|
+
raise ValueError('must set input_names or m')
|
|
52
|
+
else:
|
|
53
|
+
self.m = int(m)
|
|
54
|
+
|
|
55
|
+
self.input_names = ['u_' + str(m) for m in range(self.m)]
|
|
56
|
+
else: # input names given
|
|
57
|
+
if m is not None:
|
|
58
|
+
raise ValueError('cannot set in and n')
|
|
59
|
+
|
|
60
|
+
self.input_names = list(input_names)
|
|
61
|
+
self.m = len(self.input_names)
|
|
62
|
+
# if len(self.input_names) != self.m:
|
|
63
|
+
# raise ValueError('input_names must have length equal to u0')
|
|
64
|
+
|
|
65
|
+
# Run measurement function to get measurement size
|
|
66
|
+
x0 = np.ones(self.n)
|
|
67
|
+
u0 = np.ones(self.m)
|
|
68
|
+
y = self.h(x0, u0)
|
|
69
|
+
self.p = len(y) # number of measurements
|
|
70
|
+
|
|
71
|
+
# Set measurement names
|
|
72
|
+
if measurement_names is None: # default measurement names
|
|
73
|
+
self.measurement_names = ['y_' + str(p) for p in range(self.p)]
|
|
74
|
+
else:
|
|
75
|
+
self.measurement_names = measurement_names
|
|
76
|
+
if len(self.measurement_names) != self.p:
|
|
77
|
+
raise ValueError('measurement_names must have length equal to y')
|
|
78
|
+
|
|
79
|
+
# Initialize time vector
|
|
80
|
+
self.w = 11 # initialize for w time-steps, but this can change later
|
|
81
|
+
self.time = np.arange(0, self.w * self.dt, step=self.dt) # time vector
|
|
82
|
+
|
|
83
|
+
# Define initial states & initialize state time-series
|
|
84
|
+
self.x0 = {}
|
|
85
|
+
self.x = {}
|
|
86
|
+
for n, state_name in enumerate(self.state_names):
|
|
87
|
+
self.x0[state_name] = x0[n]
|
|
88
|
+
self.x[state_name] = x0[n] * np.ones(self.w)
|
|
89
|
+
|
|
90
|
+
self.x0 = FixedKeysDict(self.x0)
|
|
91
|
+
self.x = FixedKeysDict(self.x)
|
|
92
|
+
|
|
93
|
+
# Initialize input time-series
|
|
94
|
+
self.u = {}
|
|
95
|
+
for m, input_name in enumerate(self.input_names):
|
|
96
|
+
self.u[input_name] = u0[m] * np.ones(self.w)
|
|
97
|
+
|
|
98
|
+
self.u = FixedKeysDict(self.u)
|
|
99
|
+
|
|
100
|
+
# Initialize measurement time-series
|
|
101
|
+
self.y = {}
|
|
102
|
+
for p, measurement_name in enumerate(self.measurement_names):
|
|
103
|
+
self.y[measurement_name] = 0.0 * np.ones(self.w)
|
|
104
|
+
|
|
105
|
+
self.y = FixedKeysDict(self.y)
|
|
106
|
+
|
|
107
|
+
# Initialize state set-points
|
|
108
|
+
self.setpoint = {}
|
|
109
|
+
for n, state_name in enumerate(self.state_names):
|
|
110
|
+
self.setpoint[state_name] = 0.0 * np.ones(self.w)
|
|
111
|
+
|
|
112
|
+
self.setpoint = FixedKeysDict(self.setpoint)
|
|
113
|
+
|
|
114
|
+
# Define continuous-time MPC model
|
|
115
|
+
self.model = do_mpc.model.Model('continuous')
|
|
116
|
+
|
|
117
|
+
# Define state variables
|
|
118
|
+
X = []
|
|
119
|
+
for n, state_name in enumerate(self.state_names):
|
|
120
|
+
x = self.model.set_variable(var_type='_x', var_name=state_name, shape=(1, 1))
|
|
121
|
+
X.append(x)
|
|
122
|
+
|
|
123
|
+
# Define input variables
|
|
124
|
+
U = []
|
|
125
|
+
for m, input_name in enumerate(self.input_names):
|
|
126
|
+
u = self.model.set_variable(var_type='_u', var_name=input_name, shape=(1, 1))
|
|
127
|
+
U.append(u)
|
|
128
|
+
|
|
129
|
+
# Define dynamics
|
|
130
|
+
Xdot = self.f(X, U)
|
|
131
|
+
for n, state_name in enumerate(self.state_names):
|
|
132
|
+
self.model.set_rhs(state_name, Xdot[n])
|
|
133
|
+
|
|
134
|
+
# Add time-varying set-point variables for later use with MPC
|
|
135
|
+
for n, state_name in enumerate(self.state_names):
|
|
136
|
+
x = self.model.set_variable(var_type='_tvp', var_name=state_name + str('_set'), shape=(1, 1))
|
|
137
|
+
|
|
138
|
+
# Build model
|
|
139
|
+
self.model.setup()
|
|
140
|
+
|
|
141
|
+
# Define simulator & simulator parameters
|
|
142
|
+
self.simulator = do_mpc.simulator.Simulator(self.model)
|
|
143
|
+
|
|
144
|
+
# Set simulation parameters
|
|
145
|
+
if params_simulator is None:
|
|
146
|
+
self.params_simulator = {
|
|
147
|
+
'integration_tool': 'idas', # cvodes, idas
|
|
148
|
+
'abstol': 1e-8,
|
|
149
|
+
'reltol': 1e-8,
|
|
150
|
+
't_step': self.dt
|
|
151
|
+
}
|
|
152
|
+
else:
|
|
153
|
+
self.params_simulator = params_simulator
|
|
154
|
+
|
|
155
|
+
self.simulator.set_param(**self.params_simulator)
|
|
156
|
+
|
|
157
|
+
# Setup MPC
|
|
158
|
+
self.mpc = do_mpc.controller.MPC(self.model)
|
|
159
|
+
self.mpc_horizon = mpc_horizon
|
|
160
|
+
setup_mpc = {
|
|
161
|
+
'n_horizon': self.mpc_horizon,
|
|
162
|
+
'n_robust': 0,
|
|
163
|
+
'open_loop': 0,
|
|
164
|
+
't_step': self.dt,
|
|
165
|
+
'state_discretization': 'collocation',
|
|
166
|
+
'collocation_type': 'radau',
|
|
167
|
+
'collocation_deg': 2,
|
|
168
|
+
'collocation_ni': 1,
|
|
169
|
+
'store_full_solution': False,
|
|
170
|
+
|
|
171
|
+
# Use MA27 linear solver in ipopt for faster calculations:
|
|
172
|
+
'nlpsol_opts': {'ipopt.linear_solver': 'mumps', # mumps, MA27
|
|
173
|
+
'ipopt.print_level': 0,
|
|
174
|
+
'ipopt.sb': 'yes',
|
|
175
|
+
'print_time': 0,
|
|
176
|
+
}
|
|
177
|
+
}
|
|
178
|
+
|
|
179
|
+
self.mpc.set_param(**setup_mpc)
|
|
180
|
+
|
|
181
|
+
# Get template's for MPC time-varying parameters
|
|
182
|
+
self.mpc_tvp_template = self.mpc.get_tvp_template()
|
|
183
|
+
self.simulator_tvp_template = self.simulator.get_tvp_template()
|
|
184
|
+
|
|
185
|
+
# Set time-varying set-point functions
|
|
186
|
+
self.mpc.set_tvp_fun(self.mpc_tvp_function)
|
|
187
|
+
self.simulator.set_tvp_fun(self.simulator_tvp_function)
|
|
188
|
+
|
|
189
|
+
# Setup simulator
|
|
190
|
+
self.simulator.setup()
|
|
191
|
+
|
|
192
|
+
def simulator_tvp_function(self, t):
|
|
193
|
+
""" Set the set-point function for MPC simulator.
|
|
194
|
+
:param t: current time
|
|
195
|
+
"""
|
|
196
|
+
|
|
197
|
+
mpc_horizon = self.mpc._settings.n_horizon
|
|
198
|
+
|
|
199
|
+
# Set current step index
|
|
200
|
+
k_step = int(np.round(t / self.dt))
|
|
201
|
+
if k_step >= mpc_horizon: # point is beyond end of input data
|
|
202
|
+
k_step = mpc_horizon - 1 # set point beyond input data to last point
|
|
203
|
+
|
|
204
|
+
# Update current set-point
|
|
205
|
+
for n, state_name in enumerate(self.state_names):
|
|
206
|
+
self.simulator_tvp_template[state_name + '_set'] = self.setpoint[state_name][k_step]
|
|
207
|
+
|
|
208
|
+
return self.simulator_tvp_template
|
|
209
|
+
|
|
210
|
+
def mpc_tvp_function(self, t):
|
|
211
|
+
""" Set the set-point function for MPC optimizer.
|
|
212
|
+
"""
|
|
213
|
+
|
|
214
|
+
mpc_horizon = self.mpc._settings.n_horizon
|
|
215
|
+
|
|
216
|
+
# Set current step index
|
|
217
|
+
k_step = int(np.round(t / self.dt))
|
|
218
|
+
|
|
219
|
+
# Update set-point time horizon
|
|
220
|
+
for k in range(mpc_horizon + 1):
|
|
221
|
+
k_set = k_step + k
|
|
222
|
+
if k_set >= self.w: # horizon is beyond end of input data
|
|
223
|
+
k_set = self.w - 1 # set part of horizon beyond input data to last point
|
|
224
|
+
|
|
225
|
+
# Update each set-point over time horizon
|
|
226
|
+
for n, state_name in enumerate(self.state_names):
|
|
227
|
+
self.mpc_tvp_template['_tvp', k, state_name + '_set'] = self.setpoint[state_name][k_set]
|
|
228
|
+
|
|
229
|
+
return self.mpc_tvp_template
|
|
230
|
+
|
|
231
|
+
def set_initial_state(self, x0):
|
|
232
|
+
""" Update the initial state.
|
|
233
|
+
"""
|
|
234
|
+
|
|
235
|
+
if x0 is not None: # initial state given
|
|
236
|
+
if isinstance(x0, dict): # in dict format
|
|
237
|
+
SetDict().set_dict_with_overwrite(self.x0, x0) # update only the states in the dict given
|
|
238
|
+
elif isinstance(x0, list) or isinstance(x0, tuple) or (
|
|
239
|
+
x0, np.ndarray): # list, tuple, or numpy array format
|
|
240
|
+
x0 = np.array(x0).squeeze()
|
|
241
|
+
for n, key in enumerate(self.x0.keys()): # each state
|
|
242
|
+
self.x0[key] = x0[n]
|
|
243
|
+
else:
|
|
244
|
+
raise Exception('x0 must be either a dict, tuple, list, or numpy array')
|
|
245
|
+
|
|
246
|
+
def update_dict(self, data=None, name=None):
|
|
247
|
+
""" Update.
|
|
248
|
+
"""
|
|
249
|
+
|
|
250
|
+
update = getattr(self, name)
|
|
251
|
+
|
|
252
|
+
if data is not None: # data given
|
|
253
|
+
if isinstance(data, dict): # in dict format
|
|
254
|
+
SetDict().set_dict_with_overwrite(update, data) # update only the inputs in the dict given
|
|
255
|
+
elif isinstance(data, list) or isinstance(data, tuple): # list or tuple format, each input vector in each element
|
|
256
|
+
for n, k in enumerate(update.keys()): # each state
|
|
257
|
+
update[k] = data[n]
|
|
258
|
+
elif isinstance(data, np.ndarray): # numpy array format given as matrix where columns are the different inputs
|
|
259
|
+
if len(data.shape) <= 1: # given as 1d array, so convert to column vector
|
|
260
|
+
data = np.atleast_2d(data).T
|
|
261
|
+
|
|
262
|
+
for n, key in enumerate(update.keys()): # each input
|
|
263
|
+
update[key] = data[:, n]
|
|
264
|
+
|
|
265
|
+
else:
|
|
266
|
+
raise Exception(name + ' must be either a dict, tuple, list, or numpy array')
|
|
267
|
+
|
|
268
|
+
# Make sure inputs are the same size
|
|
269
|
+
points = np.array([update[key].shape[0] for key in update.keys()])
|
|
270
|
+
points_check = points == points[0]
|
|
271
|
+
if not np.all(points_check):
|
|
272
|
+
raise Exception(name + ' not the same size')
|
|
273
|
+
|
|
274
|
+
def simulate(self, x0=None, u=None, mpc=False, return_full_output=False):
|
|
275
|
+
"""
|
|
276
|
+
Simulate the system.
|
|
277
|
+
|
|
278
|
+
:params x0: initial state dict or array
|
|
279
|
+
:params u: input dict or array
|
|
280
|
+
:params return_full_output: boolean to run (time, x, u, y) instead of y
|
|
281
|
+
"""
|
|
282
|
+
|
|
283
|
+
if (mpc is True) and (u is not None):
|
|
284
|
+
raise Exception('u must be None if running MPC')
|
|
285
|
+
|
|
286
|
+
if (mpc is False) and (u is None):
|
|
287
|
+
warnings.warn('not running MPC or setting u directly')
|
|
288
|
+
|
|
289
|
+
# Update the initial state
|
|
290
|
+
if x0 is None:
|
|
291
|
+
if mpc: # set the initial state to start at set-point if running MPC
|
|
292
|
+
x0 = {}
|
|
293
|
+
for state_name in self.state_names:
|
|
294
|
+
x0[state_name] = self.setpoint[state_name][0]
|
|
295
|
+
|
|
296
|
+
self.set_initial_state(x0=x0)
|
|
297
|
+
else:
|
|
298
|
+
self.set_initial_state(x0=x0)
|
|
299
|
+
|
|
300
|
+
# Update the inputs
|
|
301
|
+
self.update_dict(u, name='u')
|
|
302
|
+
|
|
303
|
+
# Concatenate the inputs, where rows are individual inputs and columns are time-steps
|
|
304
|
+
if mpc:
|
|
305
|
+
self.w = np.vstack(list(self.setpoint.values())).shape[1]
|
|
306
|
+
u_sim = np.zeros((self.w, self.m)) # preallocate input array
|
|
307
|
+
else:
|
|
308
|
+
self.w = np.vstack(list(self.u.values())).shape[1]
|
|
309
|
+
u_sim = np.vstack(list(self.u.values())).T
|
|
310
|
+
|
|
311
|
+
# Update time vector
|
|
312
|
+
T = (self.w - 1) * self.dt
|
|
313
|
+
self.time = np.linspace(0, T, num=self.w)
|
|
314
|
+
|
|
315
|
+
# Set array to store simulated states, where rows are individual states and columns are time-steps
|
|
316
|
+
x_step = np.array(list(self.x0.values())) # initialize state
|
|
317
|
+
x = np.nan * np.zeros((self.w, self.n))
|
|
318
|
+
x[0, :] = x_step.copy()
|
|
319
|
+
|
|
320
|
+
# Initialize the simulator
|
|
321
|
+
self.simulator.t0 = self.time[0]
|
|
322
|
+
self.simulator.x0 = x_step.copy()
|
|
323
|
+
self.simulator.set_initial_guess()
|
|
324
|
+
|
|
325
|
+
# Initialize MPC
|
|
326
|
+
if mpc:
|
|
327
|
+
self.mpc.setup()
|
|
328
|
+
self.mpc.t0 = self.time[0]
|
|
329
|
+
self.mpc.x0 = x_step.copy()
|
|
330
|
+
self.mpc.u0 = np.zeros((self.m, 1))
|
|
331
|
+
self.mpc.set_initial_guess()
|
|
332
|
+
|
|
333
|
+
# Run simulation
|
|
334
|
+
for k in range(1, self.w):
|
|
335
|
+
# Set input
|
|
336
|
+
if mpc: # run MPC step
|
|
337
|
+
u_step = self.mpc.make_step(x_step)
|
|
338
|
+
else: # use inputs directly
|
|
339
|
+
u_step = u_sim[k - 1:k, :].T
|
|
340
|
+
|
|
341
|
+
# Store inputs
|
|
342
|
+
u_sim[k - 1, :] = u_step.squeeze()
|
|
343
|
+
|
|
344
|
+
# Simulate one time step given current inputs
|
|
345
|
+
x_step = self.simulator.make_step(u_step)
|
|
346
|
+
|
|
347
|
+
# Store new states
|
|
348
|
+
x[k, :] = x_step.squeeze()
|
|
349
|
+
|
|
350
|
+
# Last input has no effect, so keep it the same as previous time-step
|
|
351
|
+
if mpc:
|
|
352
|
+
u_sim[-1, :] = u_sim[-2, :]
|
|
353
|
+
|
|
354
|
+
# Update the inputs
|
|
355
|
+
self.update_dict(u_sim, name='u')
|
|
356
|
+
|
|
357
|
+
# Update state trajectory
|
|
358
|
+
self.update_dict(x, name='x')
|
|
359
|
+
|
|
360
|
+
# Calculate measurements
|
|
361
|
+
x_list = list(self.x.values())
|
|
362
|
+
u_list = list(self.u.values())
|
|
363
|
+
y = self.h(x_list, u_list)
|
|
364
|
+
|
|
365
|
+
# Set measurements
|
|
366
|
+
self.update_dict(y, name='y')
|
|
367
|
+
|
|
368
|
+
# Return the measurements in array format
|
|
369
|
+
y_array = np.vstack(list(self.y.values())).T
|
|
370
|
+
|
|
371
|
+
if return_full_output:
|
|
372
|
+
return self.time.copy(), self.x.copy(), self.u.copy(), self.u.copy()
|
|
373
|
+
else:
|
|
374
|
+
return y_array
|
|
375
|
+
|
|
376
|
+
def get_time_states_inputs_measurements(self):
|
|
377
|
+
return self.time.copy(), self.x.copy(), self.u.copy(), self.u.copy()
|
|
378
|
+
|
|
379
|
+
def plot(self, name='x', dpi=150, plot_kwargs=None):
|
|
380
|
+
""" Plot states, inputs.
|
|
381
|
+
"""
|
|
382
|
+
|
|
383
|
+
if plot_kwargs is None:
|
|
384
|
+
plot_kwargs = {
|
|
385
|
+
'color': 'black',
|
|
386
|
+
'linewidth': 2.0,
|
|
387
|
+
'linestyle': '-',
|
|
388
|
+
'marker': '.',
|
|
389
|
+
'markersize': 0
|
|
390
|
+
}
|
|
391
|
+
|
|
392
|
+
if name == 'x':
|
|
393
|
+
plot_kwargs['color'] = 'firebrick'
|
|
394
|
+
elif name == 'u':
|
|
395
|
+
plot_kwargs['color'] = 'royalblue'
|
|
396
|
+
elif name == 'y':
|
|
397
|
+
plot_kwargs['color'] = 'seagreen'
|
|
398
|
+
elif name == 'setpoint':
|
|
399
|
+
plot_kwargs['color'] = 'gray'
|
|
400
|
+
|
|
401
|
+
plot_dict = getattr(self, name)
|
|
402
|
+
plot_data = np.array(list(plot_dict.values()))
|
|
403
|
+
n = plot_data.shape[0]
|
|
404
|
+
|
|
405
|
+
fig, ax = plt.subplots(n, 1, figsize=(4, n * 1.5), dpi=dpi, sharex=True)
|
|
406
|
+
ax = np.atleast_1d(ax)
|
|
407
|
+
|
|
408
|
+
for n, key in enumerate(plot_dict.keys()):
|
|
409
|
+
ax[n].plot(self.time, plot_dict[key], label='set-point', **plot_kwargs)
|
|
410
|
+
ax[n].set_ylabel(key, fontsize=7)
|
|
411
|
+
|
|
412
|
+
# Also plot the states if plotting setpoint
|
|
413
|
+
if name == 'setpoint':
|
|
414
|
+
ax[n].plot(self.time, self.x[key], label=key, color='firebrick', linestyle='-', linewidth=0.5)
|
|
415
|
+
ax[n].legend(fontsize=6)
|
|
416
|
+
|
|
417
|
+
ax[-1].set_xlabel('time', fontsize=7)
|
|
418
|
+
ax[0].set_title(name, fontsize=8, fontweight='bold')
|
|
419
|
+
|
|
420
|
+
|
|
421
|
+
for a in ax.flat:
|
|
422
|
+
a.tick_params(axis='both', labelsize=6)
|
pybounds/observability.py
CHANGED
|
@@ -12,10 +12,10 @@ from .util import LatexStates
|
|
|
12
12
|
|
|
13
13
|
|
|
14
14
|
class EmpiricalObservabilityMatrix:
|
|
15
|
-
def __init__(self, simulator, x0,
|
|
15
|
+
def __init__(self, simulator, x0, u, eps=1e-5, parallel=False):
|
|
16
16
|
""" Construct an empirical observability matrix O.
|
|
17
17
|
|
|
18
|
-
:param callable
|
|
18
|
+
:param callable simulator: simulator object that has a method y = simulator.simulate(x0, u, **kwargs)
|
|
19
19
|
y is (w x p) array. w is the number of time-steps and p is the number of measurements
|
|
20
20
|
:param dict/list/np.array x0: initial state for Simulator
|
|
21
21
|
:param dict/np.array u: inputs array
|
|
@@ -25,7 +25,6 @@ class EmpiricalObservabilityMatrix:
|
|
|
25
25
|
|
|
26
26
|
# Store inputs
|
|
27
27
|
self.simulator = simulator
|
|
28
|
-
self.time = time.copy()
|
|
29
28
|
self.eps = eps
|
|
30
29
|
self.parallel = parallel
|
|
31
30
|
|
|
@@ -48,6 +47,9 @@ class EmpiricalObservabilityMatrix:
|
|
|
48
47
|
# Number of outputs
|
|
49
48
|
self.p = self.y_nominal.shape[1]
|
|
50
49
|
|
|
50
|
+
# Number of time-steps
|
|
51
|
+
self.w = self.y_nominal.shape[0] # of points in time window
|
|
52
|
+
|
|
51
53
|
# Check for state/measurement names
|
|
52
54
|
if hasattr(self.simulator, 'state_names'):
|
|
53
55
|
self.state_names = self.simulator.state_names
|
|
@@ -60,7 +62,6 @@ class EmpiricalObservabilityMatrix:
|
|
|
60
62
|
self.measurement_names = ['y_' + str(p) for p in range(self.p)]
|
|
61
63
|
|
|
62
64
|
# Perturbation amounts
|
|
63
|
-
self.w = len(self.time) # of points in time window
|
|
64
65
|
self.delta_x = eps * np.eye(self.n) # perturbation amount for each state
|
|
65
66
|
self.delta_y = np.zeros((self.p, self.n, self.w)) # preallocate delta_y
|
|
66
67
|
self.y_plus = np.zeros((self.w, self.n, self.p))
|
|
@@ -154,7 +155,7 @@ class SlidingEmpiricalObservabilityMatrix:
|
|
|
154
155
|
|
|
155
156
|
:param callable simulator: Simulator object : y = simulator(x0, u, **kwargs)
|
|
156
157
|
y is (w x p) array. w is the number of time-steps and p is the number of measurements
|
|
157
|
-
:param np.array t_sim: time
|
|
158
|
+
:param np.array t_sim: time values along state trajectory array (N, 1)
|
|
158
159
|
:param np.array x_sim: state trajectory array (N, n), can also be dict
|
|
159
160
|
:param np.array u_sim: input array (N, m), can also be dict
|
|
160
161
|
:param np.array w: window size for O calculations, will automatically set how many windows to compute
|
|
@@ -270,7 +271,7 @@ class SlidingEmpiricalObservabilityMatrix:
|
|
|
270
271
|
u_win = self.u_sim[win, :] # inputs in window
|
|
271
272
|
|
|
272
273
|
# Calculate O for window
|
|
273
|
-
EOM = EmpiricalObservabilityMatrix(self.simulator, x0,
|
|
274
|
+
EOM = EmpiricalObservabilityMatrix(self.simulator, x0, u_win, eps=self.eps,
|
|
274
275
|
parallel=self.parallel_perturbation)
|
|
275
276
|
self.EOM = EOM
|
|
276
277
|
|
pybounds/simulator.py
CHANGED
|
@@ -369,7 +369,7 @@ class Simulator(object):
|
|
|
369
369
|
y_array = np.vstack(list(self.y.values())).T
|
|
370
370
|
|
|
371
371
|
if return_full_output:
|
|
372
|
-
return self.time.copy(), self.x.copy(), self.u.copy(), self.
|
|
372
|
+
return self.time.copy(), self.x.copy(), self.u.copy(), self.y.copy()
|
|
373
373
|
else:
|
|
374
374
|
return y_array
|
|
375
375
|
|
|
@@ -406,7 +406,7 @@ class Simulator(object):
|
|
|
406
406
|
ax = np.atleast_1d(ax)
|
|
407
407
|
|
|
408
408
|
for n, key in enumerate(plot_dict.keys()):
|
|
409
|
-
ax[n].plot(self.time, plot_dict[key], label=
|
|
409
|
+
ax[n].plot(self.time, plot_dict[key], label=name, **plot_kwargs)
|
|
410
410
|
ax[n].set_ylabel(key, fontsize=7)
|
|
411
411
|
|
|
412
412
|
# Also plot the states if plotting setpoint
|
|
@@ -414,9 +414,18 @@ class Simulator(object):
|
|
|
414
414
|
ax[n].plot(self.time, self.x[key], label=key, color='firebrick', linestyle='-', linewidth=0.5)
|
|
415
415
|
ax[n].legend(fontsize=6)
|
|
416
416
|
|
|
417
|
+
y = self.x[key]
|
|
418
|
+
y_min = np.min(y)
|
|
419
|
+
y_max = np.max(y)
|
|
420
|
+
delta = y_max - y_min
|
|
421
|
+
if np.abs(delta) < 0.01:
|
|
422
|
+
margin = 0.1
|
|
423
|
+
ax[n].set_ylim(y_min - margin, y_max + margin)
|
|
424
|
+
else:
|
|
425
|
+
margin = 0.0
|
|
426
|
+
|
|
417
427
|
ax[-1].set_xlabel('time', fontsize=7)
|
|
418
428
|
ax[0].set_title(name, fontsize=8, fontweight='bold')
|
|
419
429
|
|
|
420
|
-
|
|
421
430
|
for a in ax.flat:
|
|
422
431
|
a.tick_params(axis='both', labelsize=6)
|
pybounds/util.py
CHANGED
|
@@ -1,9 +1,9 @@
|
|
|
1
|
-
|
|
2
1
|
import numpy as np
|
|
3
2
|
import matplotlib.pyplot as plt
|
|
4
3
|
import matplotlib.collections as mcoll
|
|
5
4
|
import matplotlib.patheffects as path_effects
|
|
6
5
|
|
|
6
|
+
|
|
7
7
|
class FixedKeysDict(dict):
|
|
8
8
|
def __init__(self, *args, **kwargs):
|
|
9
9
|
super(FixedKeysDict, self).__init__(*args, **kwargs)
|
|
@@ -96,11 +96,46 @@ class LatexStates:
|
|
|
96
96
|
'v_para_dot': r'$\dot{v_{\parallel}}$',
|
|
97
97
|
'v_perp_dot': r'$\dot{v_{\perp}}$',
|
|
98
98
|
'v_para_dot_ratio': r'$\frac{\Delta v_{\parallel}}{v_{\parallel}}$',
|
|
99
|
-
'
|
|
100
|
-
'
|
|
101
|
-
'
|
|
102
|
-
'
|
|
103
|
-
'
|
|
99
|
+
'x': r'$x$',
|
|
100
|
+
'y': r'$y$',
|
|
101
|
+
'v_x': r'$v_{x}$',
|
|
102
|
+
'v_y': r'$v_{y}$',
|
|
103
|
+
'v_z': r'$v_{z}$',
|
|
104
|
+
'w_x': r'$w_{x}$',
|
|
105
|
+
'w_y': r'$w_{y}$',
|
|
106
|
+
'w_z': r'$w_{z}$',
|
|
107
|
+
'a_x': r'$a_{x}$',
|
|
108
|
+
'a_y': r'$a_{y}$',
|
|
109
|
+
'vx': r'$v_x$',
|
|
110
|
+
'vy': r'$v_y$',
|
|
111
|
+
'vz': r'$v_z$',
|
|
112
|
+
'wx': r'$w_x$',
|
|
113
|
+
'wy': r'$w_y$',
|
|
114
|
+
'wz': r'$w_z$',
|
|
115
|
+
'ax': r'$ax$',
|
|
116
|
+
'ay': r'$ay$',
|
|
117
|
+
'beta': r'$\beta',
|
|
118
|
+
'thetadot': r'$\dot{\theta}$',
|
|
119
|
+
'theta_dot': r'$\dot{\theta}$',
|
|
120
|
+
'psidot': r'$\dot{\psi}$',
|
|
121
|
+
'psi_dot': r'$\dot{\psi}$',
|
|
122
|
+
'theta': r'$\theta$',
|
|
123
|
+
'Yaw': r'$\psi$',
|
|
124
|
+
'R': r'$\phi$',
|
|
125
|
+
'P': r'$\theta$',
|
|
126
|
+
'dYaw': r'$\dot{\psi}$',
|
|
127
|
+
'dP': r'$\dot{\theta}$',
|
|
128
|
+
'dR': r'$\dot{\phi}$',
|
|
129
|
+
'acc_x': r'$\dot{v}x$',
|
|
130
|
+
'acc_y': r'$\dot{v}y$',
|
|
131
|
+
'acc_z': r'$\dot{v}z$',
|
|
132
|
+
'Psi': r'$\Psi$',
|
|
133
|
+
'Ix': r'$I_x$',
|
|
134
|
+
'Iy': r'$I_y$',
|
|
135
|
+
'Iz': r'$I_z$',
|
|
136
|
+
'Jr': r'$J_r$',
|
|
137
|
+
'Dl': r'$D_l$',
|
|
138
|
+
'Dr': r'$D_r$',
|
|
104
139
|
}
|
|
105
140
|
|
|
106
141
|
if dict is not None:
|
|
@@ -162,4 +197,4 @@ def colorline(x, y, z, ax=None, cmap=plt.get_cmap('copper'), norm=None, linewidt
|
|
|
162
197
|
|
|
163
198
|
ax.add_collection(lc)
|
|
164
199
|
|
|
165
|
-
return lc
|
|
200
|
+
return lc
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.1
|
|
2
2
|
Name: pybounds
|
|
3
|
-
Version: 0.0.
|
|
3
|
+
Version: 0.0.6
|
|
4
4
|
Summary: Bounding Observability for Uncertain Nonlinear Dynamics Systems (BOUNDS)
|
|
5
5
|
Home-page: https://pypi.org/project/pybounds/
|
|
6
6
|
Author: Ben Cellini, Burak Boyacioglu, Floris van Breugel
|
|
@@ -39,13 +39,13 @@ For a simple system
|
|
|
39
39
|
* Monocular camera with optic fow measurements: [mono_camera_example.ipynb](examples%2Fmono_camera_example.ipynb)
|
|
40
40
|
|
|
41
41
|
For a more complex system
|
|
42
|
-
* Fly-wind: [fly_wind_example.ipynb](examples%2Ffly_wind_example.ipynb)
|
|
42
|
+
* Fly-wind: [fly_wind_example.ipynb](examples%2Ffly_wind_example.ipynb)
|
|
43
43
|
|
|
44
44
|
## Citation
|
|
45
45
|
|
|
46
46
|
If you use the code or methods from this package, please cite the following paper:
|
|
47
47
|
|
|
48
|
-
Benjamin Cellini, Burak
|
|
48
|
+
Benjamin Cellini, Burak Boyacioglu, Stanley David Stupski, and Floris van Breugel. Discovering and exploiting active sensing motifs for estimation with empirical observability. (2024) bioRxiv.
|
|
49
49
|
|
|
50
50
|
## Related packages
|
|
51
51
|
This repository is the evolution of the EISO repo (https://github.com/BenCellini/EISO), and is intended as a companion to the repository directly associated with the paper above.
|
|
@@ -0,0 +1,10 @@
|
|
|
1
|
+
pybounds/__init__.py,sha256=so9LuRNw2V8MGsDs-RPstGGgJtBFp2YGolQbS0rVfhw,348
|
|
2
|
+
pybounds/drone_simulator.py,sha256=-CrQVpNfiBDFECd6H7FU5has4sYGW1gyS2RhOgXUqZg,15858
|
|
3
|
+
pybounds/observability.py,sha256=4tdK6AK678zoorbkQ2psvzMRLY32CIj2QwVb-0w-GXk,27541
|
|
4
|
+
pybounds/simulator.py,sha256=ReaCRHA-DjiE1EbmCStw2Top9EyJeg41S_lO-iqnjv4,16194
|
|
5
|
+
pybounds/util.py,sha256=Gs0UgqgLXTJI9FZww90iJhqU02iJ31bXBURjGiq3YzM,7401
|
|
6
|
+
pybounds-0.0.6.dist-info/LICENSE,sha256=kqeyRXtRGgBVZdXYeIX4zR9l2KZ2rqIBVEiPMTjxjcI,1093
|
|
7
|
+
pybounds-0.0.6.dist-info/METADATA,sha256=MAp33xdjWLOfB17N32E-GdKRZnaCHtIwBc65XmFQXb4,2155
|
|
8
|
+
pybounds-0.0.6.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
|
|
9
|
+
pybounds-0.0.6.dist-info/top_level.txt,sha256=V-ofnWE3m_UkXTXJwNRD07n14m5R6sc6l4NadaCCP_A,9
|
|
10
|
+
pybounds-0.0.6.dist-info/RECORD,,
|
pybounds-0.0.5.dist-info/RECORD
DELETED
|
@@ -1,9 +0,0 @@
|
|
|
1
|
-
pybounds/__init__.py,sha256=so9LuRNw2V8MGsDs-RPstGGgJtBFp2YGolQbS0rVfhw,348
|
|
2
|
-
pybounds/observability.py,sha256=MUdwufFf200SYseP42JCP978O4F7GbeBVtm0yGjGzOE,27490
|
|
3
|
-
pybounds/simulator.py,sha256=-CrQVpNfiBDFECd6H7FU5has4sYGW1gyS2RhOgXUqZg,15858
|
|
4
|
-
pybounds/util.py,sha256=l6S9G88S-OZO9mi7F_58bVAPSr3PGQKcbHUghgni4JY,5956
|
|
5
|
-
pybounds-0.0.5.dist-info/LICENSE,sha256=kqeyRXtRGgBVZdXYeIX4zR9l2KZ2rqIBVEiPMTjxjcI,1093
|
|
6
|
-
pybounds-0.0.5.dist-info/METADATA,sha256=sQnyiQHrQHFzXHh_mr4ymujDAh7MZved-QnSKC08JTM,2226
|
|
7
|
-
pybounds-0.0.5.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
|
|
8
|
-
pybounds-0.0.5.dist-info/top_level.txt,sha256=V-ofnWE3m_UkXTXJwNRD07n14m5R6sc6l4NadaCCP_A,9
|
|
9
|
-
pybounds-0.0.5.dist-info/RECORD,,
|
|
File without changes
|
|
File without changes
|
|
File without changes
|