Skip to content

Commit 536b97e

Browse files
murrayrmmurrayrm
committed
stochsys ipynb examples + labeling fixes
1 parent dff5206 commit 536b97e

5 files changed

Lines changed: 1440 additions & 4 deletions

File tree

‎control/stochsys.py‎

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -305,7 +305,7 @@ def dlqe(*args, **keywords):
305305
def create_estimator_iosystem(
306306
sys, QN, RN, P0=None, G=None, C=None,
307307
state_labels='xhat[{i}]', output_labels='xhat[{i}]',
308-
covariance_labels='P[{i},{j}]'):
308+
covariance_labels='P[{i},{j}]', sensor_labels=None):
309309
"""Create an I/O system implementing a linqear quadratic estimator
310310
311311
This function creates an input/output system that implements a
@@ -386,6 +386,8 @@ def create_estimator_iosystem(
386386
else:
387387
# Use the system outputs as the sensor outputs
388388
C = sys.C
389+
if sensor_labels is None:
390+
sensor_labels = sys.output_labels
389391

390392
# Initialize the covariance matrix
391393
if P0 is None:
@@ -407,12 +409,17 @@ def create_estimator_iosystem(
407409
# Generate the list of labels using the argument as a format string
408410
output_labels = [output_labels.format(i=i) for i in range(sys.nstates)]
409411

412+
sensor_labels = 'y[{i}]' if sensor_labels is None else sensor_labels
413+
if isinstance(sensor_labels, str):
414+
# Generate the list of labels using the argument as a format string
415+
sensor_labels = [sensor_labels.format(i=i) for i in range(C.shape[0])]
416+
410417
if isctime(sys):
411418
raise NotImplementedError("continuous time not yet implemented")
412419

413420
else:
414421
# Create an I/O system for the state feedback gains
415-
# Note: reshape various vectors into column vectors for legacy matrix
422+
# Note: reshape vectors into column vectors for legacy np.matrix
416423
def _estim_update(t, x, u, params):
417424
# See if we are estimating or predicting
418425
correct = params.get('correct', True)
@@ -448,8 +455,8 @@ def _estim_output(t, x, u, params):
448455
# Define the estimator system
449456
return NonlinearIOSystem(
450457
_estim_update, _estim_output, states=state_labels + covariance_labels,
451-
inputs=sys.output_labels + sys.input_labels, outputs=output_labels,
452-
dt=sys.dt)
458+
inputs=sensor_labels + sys.input_labels,
459+
outputs=output_labels, dt=sys.dt)
453460

454461

455462
def white_noise(T, Q, dt=0):

‎examples/kincar-fusion.ipynb‎

Lines changed: 525 additions & 0 deletions
Large diffs are not rendered by default.

‎examples/pvtol-outputfbk.ipynb‎

Lines changed: 478 additions & 0 deletions
Large diffs are not rendered by default.

‎examples/pvtol.py‎

Lines changed: 315 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,315 @@
1+
# pvtol.py - (planar) vertical takeoff and landing system model
2+
# RMM, 19 Jan 2022
3+
#
4+
# This file contains a model and utility function for a (planar)
5+
# vertical takeoff and landing system, as described in FBS2e and OBC.
6+
# This system is approximately differentially flat and the flat system
7+
# mappings are included.
8+
#
9+
10+
import numpy as np
11+
import matplotlib.pyplot as plt
12+
import control as ct
13+
import control.flatsys as fs
14+
from math import sin, cos
15+
from warnings import warn
16+
17+
# PVTOL dynamics
18+
def pvtol_update(t, x, u, params):
19+
# Get the parameter values
20+
m = params.get('m', 4.) # mass of aircraft
21+
J = params.get('J', 0.0475) # inertia around pitch axis
22+
r = params.get('r', 0.25) # distance to center of force
23+
g = params.get('g', 9.8) # gravitational constant
24+
c = params.get('c', 0.05) # damping factor (estimated)
25+
26+
# Get the inputs and states
27+
x, y, theta, xdot, ydot, thetadot = x
28+
F1, F2 = u
29+
30+
# Constrain the inputs
31+
F2 = np.clip(F2, 0, 1.5 * m * g)
32+
F1 = np.clip(F1, -0.1 * F2, 0.1 * F2)
33+
34+
# Dynamics
35+
xddot = (F1 * cos(theta) - F2 * sin(theta) - c * xdot) / m
36+
yddot = (F1 * sin(theta) + F2 * cos(theta) - m * g - c * ydot) / m
37+
thddot = (r * F1) / J
38+
39+
return np.array([xdot, ydot, thetadot, xddot, yddot, thddot])
40+
41+
def pvtol_output(t, x, u, params):
42+
return x
43+
44+
# PVTOL flat system mappings
45+
def pvtol_flat_forward(states, inputs, params={}):
46+
# Get the parameter values
47+
m = params.get('m', 4.) # mass of aircraft
48+
J = params.get('J', 0.0475) # inertia around pitch axis
49+
r = params.get('r', 0.25) # distance to center of force
50+
g = params.get('g', 9.8) # gravitational constant
51+
c = params.get('c', 0.05) # damping factor (estimated)
52+
53+
# Make sure that c is zero
54+
if c != 0:
55+
warn("System is only approximately flat (c != 0)")
56+
57+
# Create a list of arrays to store the flat output and its derivatives
58+
zflag = [np.zeros(5), np.zeros(5)]
59+
60+
# Store states and inputs in variables to make things easier to read
61+
x, y, theta, xdot, ydot, thdot = states
62+
F1, F2 = inputs
63+
64+
# Use equations of motion for higher derivates
65+
x1ddot = (F1 * cos(theta) - F2 * sin(theta)) / m
66+
x2ddot = (F1 * sin(theta) + F2 * cos(theta) - m * g) / m
67+
thddot = (r * F1) / J
68+
69+
# Flat output is a point above the vertical axis
70+
zflag[0][0] = x - (J / (m * r)) * sin(theta)
71+
zflag[1][0] = y + (J / (m * r)) * cos(theta)
72+
73+
zflag[0][1] = xdot - (J / (m * r)) * cos(theta) * thdot
74+
zflag[1][1] = ydot - (J / (m * r)) * sin(theta) * thdot
75+
76+
zflag[0][2] = (F1 * cos(theta) - F2 * sin(theta)) / m \
77+
+ (J / (m * r)) * sin(theta) * thdot**2 \
78+
- (J / (m * r)) * cos(theta) * thddot
79+
zflag[1][2] = (F1 * sin(theta) + F2 * cos(theta) - m * g) / m \
80+
- (J / (m * r)) * cos(theta) * thdot**2 \
81+
- (J / (m * r)) * sin(theta) * thddot
82+
83+
# For the third derivative, assume F1, F2 are constant (also thddot)
84+
zflag[0][3] = (-F1 * sin(theta) - F2 * cos(theta)) * (thdot / m) \
85+
+ (J / (m * r)) * cos(theta) * thdot**3 \
86+
+ 3 * (J / (m * r)) * sin(theta) * thdot * thddot
87+
zflag[1][3] = (F1 * cos(theta) - F2 * sin(theta)) * (thdot / m) \
88+
+ (J / (m * r)) * sin(theta) * thdot**3 \
89+
- 3 * (J / (m * r)) * cos(theta) * thdot * thddot
90+
91+
# For the fourth derivative, assume F1, F2 are constant (also thddot)
92+
zflag[0][4] = (-F1 * sin(theta) - F2 * cos(theta)) * (thddot / m) \
93+
+ (-F1 * cos(theta) + F2 * sin(theta)) * (thdot**2 / m) \
94+
+ 6 * (J / (m * r)) * cos(theta) * thdot**2 * thddot \
95+
+ 3 * (J / (m * r)) * sin(theta) * thddot**2 \
96+
- (J / (m * r)) * sin(theta) * thdot**4
97+
zflag[1][4] = (F1 * cos(theta) - F2 * sin(theta)) * (thddot / m) \
98+
+ (-F1 * sin(theta) - F2 * cos(theta)) * (thdot**2 / m) \
99+
- 6 * (J / (m * r)) * sin(theta) * thdot**2 * thddot \
100+
- 3 * (J / (m * r)) * cos(theta) * thddot**2 \
101+
+ (J / (m * r)) * cos(theta) * thdot**4
102+
103+
return zflag
104+
105+
def pvtol_flat_reverse(zflag, params={}):
106+
# Get the parameter values
107+
m = params.get('m', 4.) # mass of aircraft
108+
J = params.get('J', 0.0475) # inertia around pitch axis
109+
r = params.get('r', 0.25) # distance to center of force
110+
g = params.get('g', 9.8) # gravitational constant
111+
c = params.get('c', 0.05) # damping factor (estimated)
112+
113+
# Create a vector to store the state and inputs
114+
x = np.zeros(6)
115+
u = np.zeros(2)
116+
117+
# Given the flat variables, solve for the state
118+
theta = np.arctan2(-zflag[0][2], zflag[1][2] + g)
119+
x = zflag[0][0] + (J / (m * r)) * sin(theta)
120+
y = zflag[1][0] - (J / (m * r)) * cos(theta)
121+
122+
# Solve for thdot using next derivative
123+
thdot = (zflag[0][3] * cos(theta) + zflag[1][3] * sin(theta)) \
124+
/ (zflag[0][2] * sin(theta) - (zflag[1][2] + g) * cos(theta))
125+
126+
# xdot and ydot can be found from first derivative of flat outputs
127+
xdot = zflag[0][1] + (J / (m * r)) * cos(theta) * thdot
128+
ydot = zflag[1][1] + (J / (m * r)) * sin(theta) * thdot
129+
130+
# Solve for the input forces
131+
F2 = m * ((zflag[1][2] + g) * cos(theta) - zflag[0][2] * sin(theta)
132+
+ (J / (m * r)) * thdot**2)
133+
F1 = (J / r) * \
134+
(zflag[0][4] * cos(theta) + zflag[1][4] * sin(theta)
135+
# - 2 * (zflag[0][3] * sin(theta) - zflag[1][3] * cos(theta)) * thdot \
136+
- 2 * zflag[0][3] * sin(theta) * thdot \
137+
+ 2 * zflag[1][3] * cos(theta) * thdot \
138+
# - (zflag[0][2] * cos(theta)
139+
# + (zflag[1][2] + g) * sin(theta)) * thdot**2) \
140+
- zflag[0][2] * cos(theta) * thdot**2
141+
- (zflag[1][2] + g) * sin(theta) * thdot**2) \
142+
/ (zflag[0][2] * sin(theta) - (zflag[1][2] + g) * cos(theta))
143+
144+
return np.array([x, y, theta, xdot, ydot, thdot]), np.array([F1, F2])
145+
146+
pvtol = fs.FlatSystem(
147+
pvtol_flat_forward, pvtol_flat_reverse, name='pvtol',
148+
updfcn=pvtol_update, outfcn=pvtol_output,
149+
states = [f'x{i}' for i in range(6)],
150+
inputs = ['F1', 'F2'],
151+
outputs = [f'x{i}' for i in range(6)],
152+
params = {
153+
'm': 4., # mass of aircraft
154+
'J': 0.0475, # inertia around pitch axis
155+
'r': 0.25, # distance to center of force
156+
'g': 9.8, # gravitational constant
157+
'c': 0.05, # damping factor (estimated)
158+
}
159+
)
160+
161+
#
162+
# PVTOL dynamics with wind
163+
#
164+
165+
def windy_update(t, x, u, params):
166+
# Get the input vector
167+
F1, F2, d = u
168+
169+
# Get the system response from the original dynamics
170+
xdot, ydot, thetadot, xddot, yddot, thddot = \
171+
pvtol_update(t, x, u[0:2], params)
172+
173+
# Now add the wind term
174+
m = params.get('m', 4.) # mass of aircraft
175+
xddot += d / m
176+
177+
return np.array([xdot, ydot, thetadot, xddot, yddot, thddot])
178+
179+
windy_pvtol = ct.NonlinearIOSystem(
180+
windy_update, pvtol_output, name="windy_pvtol",
181+
states = [f'x{i}' for i in range(6)],
182+
inputs = ['F1', 'F2', 'd'],
183+
outputs = [f'x{i}' for i in range(6)]
184+
)
185+
186+
#
187+
# PVTOL dynamics with noise and disturbances
188+
#
189+
190+
def noisy_update(t, x, u, params):
191+
# Get the inputs
192+
F1, F2, Dx, Dy, Nx, Ny, Nth = u
193+
194+
# Get the system response from the original dynamics
195+
xdot, ydot, thetadot, xddot, yddot, thddot = \
196+
pvtol_update(t, x, u[0:2], params)
197+
198+
# Get the parameter values we need
199+
m = params.get('m', 4.) # mass of aircraft
200+
J = params.get('J', 0.0475) # inertia around pitch axis
201+
202+
# Now add the disturbances
203+
xddot += Dx / m
204+
yddot += Dy / m
205+
206+
return np.array([xdot, ydot, thetadot, xddot, yddot, thddot])
207+
208+
def noisy_output(t, x, u, params):
209+
F1, F2, dx, Dy, Nx, Ny, Nth = u
210+
return x + np.array([Nx, Ny, Nth, 0, 0, 0])
211+
212+
noisy_pvtol = ct.NonlinearIOSystem(
213+
noisy_update, noisy_output, name="noisy_pvtol",
214+
states = [f'x{i}' for i in range(6)],
215+
inputs = ['F1', 'F2'] + ['Dx', 'Dy'] + ['Nx', 'Ny', 'Nth'],
216+
outputs = pvtol.state_labels
217+
)
218+
219+
# Add the linearitizations to the dynamics as additional methods
220+
def noisy_pvtol_A(x, u, params={}):
221+
# Get the parameter values we need
222+
m = params.get('m', 4.) # mass of aircraft
223+
J = params.get('J', 0.0475) # inertia around pitch axis
224+
c = params.get('c', 0.05) # damping factor (estimated)
225+
226+
# Get the angle and compute sine and cosine
227+
theta = x[[2]]
228+
cth, sth = cos(theta), sin(theta)
229+
230+
# Return the linearized dynamics matrix
231+
return np.array([
232+
[0, 0, 0, 1, 0, 0],
233+
[0, 0, 0, 0, 1, 0],
234+
[0, 0, 0, 0, 0, 1],
235+
[0, 0, (-u[0] * sth - u[1] * cth)/m, -c/m, 0, 0],
236+
[0, 0, ( u[0] * cth - u[1] * sth)/m, 0, -c/m, 0],
237+
[0, 0, 0, 0, 0, 0]
238+
])
239+
pvtol.A = noisy_pvtol_A
240+
241+
# Plot the trajectory in xy coordinates
242+
def plot_results(t, x, u):
243+
# Set the size of the figure
244+
plt.figure(figsize=(10, 6))
245+
246+
# Top plot: xy trajectory
247+
plt.subplot(2, 1, 1)
248+
plt.plot(x[0], x[1])
249+
plt.xlabel('x [m]')
250+
plt.ylabel('y [m]')
251+
plt.axis('equal')
252+
253+
# Time traces of the state and input
254+
plt.subplot(2, 4, 5)
255+
plt.plot(t, x[1])
256+
plt.xlabel('Time t [sec]')
257+
plt.ylabel('y [m]')
258+
259+
plt.subplot(2, 4, 6)
260+
plt.plot(t, x[2])
261+
plt.xlabel('Time t [sec]')
262+
plt.ylabel('theta [rad]')
263+
264+
plt.subplot(2, 4, 7)
265+
plt.plot(t, u[0])
266+
plt.xlabel('Time t [sec]')
267+
plt.ylabel('$F_1$ [N]')
268+
269+
plt.subplot(2, 4, 8)
270+
plt.plot(t, u[1])
271+
plt.xlabel('Time t [sec]')
272+
plt.ylabel('$F_2$ [N]')
273+
plt.tight_layout()
274+
275+
#
276+
# Additional functions for testing
277+
#
278+
279+
# Check flatness calculations
280+
def _pvtol_check_flat(test_points=None, verbose=False):
281+
if test_points is None:
282+
# If no test points, use internal set
283+
mg = 9.8 * 4
284+
test_points = [
285+
([0, 0, 0, 0, 0, 0], [0, mg]),
286+
([1, 0, 0, 0, 0, 0], [0, mg]),
287+
([0, 1, 0, 0, 0, 0], [0, mg]),
288+
([1, 1, 0.1, 0, 0, 0], [0, mg]),
289+
([0, 0, 0.1, 0, 0, 0], [0, mg]),
290+
([0, 0, 0, 1, 0, 0], [0, mg]),
291+
([0, 0, 0, 0, 1, 0], [0, mg]),
292+
([0, 0, 0, 0, 0, 0.1], [0, mg]),
293+
([0, 0, 0, 1, 1, 0.1], [0, mg]),
294+
([0, 0, 0, 0, 0, 0], [1, mg]),
295+
([0, 0, 0, 0, 0, 0], [0, mg + 1]),
296+
([0, 0, 0, 0, 0, 0.1], [1, mg]),
297+
([0.1, 0.2, 0.3, 0.4, 0.5, 0.6], [0.7, mg + 1]),
298+
]
299+
elif isinstance(test_points, tuple):
300+
# If we only got one test point, convert to a list
301+
test_points = [test_points]
302+
303+
for x, u in test_points:
304+
x, u = np.array(x), np.array(u)
305+
flag = pvtol_flat_forward(x, u)
306+
xc, uc = pvtol_flat_reverse(flag)
307+
print(f'({x}, {u}): ', end='')
308+
if verbose:
309+
print(f'\n flag: {flag}')
310+
print(f' check: ({xc}, {uc}): ', end='')
311+
if np.allclose(x, xc) and np.allclose(u, uc):
312+
print("OK")
313+
else:
314+
print("ERR")
315+

0 commit comments

Comments
 (0)