-
Notifications
You must be signed in to change notification settings - Fork 458
Expand file tree
/
Copy pathpvtol-nested-ss.py
More file actions
177 lines (140 loc) · 4.33 KB
/
Copy pathpvtol-nested-ss.py
File metadata and controls
177 lines (140 loc) · 4.33 KB
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
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
# pvtol-nested.py - inner/outer design for vectored thrust aircraft
# RMM, 5 Sep 09
#
# This file works through a fairly complicated control design and
# analysis, corresponding to the planar vertical takeoff and landing
# (PVTOL) aircraft in Astrom and Mruray, Chapter 11. It is intended
# to demonstrate the basic functionality of the python-control
# package.
#
import os
import matplotlib.pyplot as plt # MATLAB plotting functions
from control.matlab import * # MATLAB-like functions
import numpy as np
# System parameters
m = 4 # mass of aircraft
J = 0.0475 # inertia around pitch axis
r = 0.25 # distance to center of force
g = 9.8 # gravitational constant
c = 0.05 # damping factor (estimated)
# Transfer functions for dynamics
Pi = tf([r], [J, 0, 0]) # inner loop (roll)
Po = tf([1], [m, c, 0]) # outer loop (position)
# Use state space versions
Pi = tf2ss(Pi)
Po = tf2ss(Po)
#
# Inner loop control design
#
# This is the controller for the pitch dynamics. Goal is to have
# fast response for the pitch dynamics so that we can use this as a
# control for the lateral dynamics
#
# Design a simple lead controller for the system
k, a, b = 200, 2, 50
Ci = k*tf([1, a], [1, b]) # lead compensator
# Convert to statespace
Ci = tf2ss(Ci)
# Compute the loop transfer function for the inner loop
Li = Pi*Ci
# Bode plot for the open loop process
plt.figure(1)
bode(Pi)
# Bode plot for the loop transfer function, with margins
plt.figure(2)
bode(Li)
# Compute out the gain and phase margins
#! Not implemented
# (gm, pm, wcg, wcp) = margin(Li);
# Compute the sensitivity and complementary sensitivity functions
Si = feedback(1, Li)
Ti = Li*Si
# Check to make sure that the specification is met
plt.figure(3)
gangof4(Pi, Ci)
# Compute out the actual transfer function from u1 to v1 (see L8.2 notes)
# Hi = Ci*(1-m*g*Pi)/(1+Ci*Pi);
Hi = parallel(feedback(Ci, Pi), -m*g*feedback(Ci*Pi, 1))
plt.figure(4)
plt.clf()
plt.subplot(221)
bode(Hi)
# Now design the lateral control system
a, b, K = 0.02, 5, 2
Co = -K*tf([1, 0.3], [1, 10]) # another lead compensator
# Convert to statespace
Co = tf2ss(Co)
# Compute the loop transfer function for the outer loop
Lo = -m*g*Po*Co
plt.figure(5)
bode(Lo) # margin(Lo)
# Finally compute the real outer-loop loop gain + responses
L = Co*Hi*Po
S = feedback(1, L)
T = feedback(L, 1)
# Compute stability margins
#! Not yet implemented
# (gm, pm, wgc, wpc) = margin(L);
plt.figure(6)
plt.clf()
bode(L, logspace(-4, 3))
# Add crossover line to magnitude plot
for ax in plt.gcf().axes:
if ax.get_label() == 'control-bode-magnitude':
break
ax.semilogx([1e-4, 1e3], 20*np.log10([1, 1]), 'k-')
# Re-plot phase starting at -90 degrees
mag, phase, w = freqresp(L, logspace(-4, 3))
phase = phase - 360
for ax in plt.gcf().axes:
if ax.get_label() == 'control-bode-phase':
break
ax.semilogx([1e-4, 1e3], [-180, -180], 'k-')
ax.semilogx(w, np.squeeze(phase), 'b-')
ax.axis([1e-4, 1e3, -360, 0])
plt.xlabel('Frequency [deg]')
plt.ylabel('Phase [deg]')
# plt.set(gca, 'YTick', [-360, -270, -180, -90, 0])
# plt.set(gca, 'XTick', [10^-4, 10^-2, 1, 100])
#
# Nyquist plot for complete design
#
plt.figure(7)
plt.clf()
plt.axis([-700, 5300, -3000, 3000])
nyquist(L, (0.0001, 1000))
plt.axis([-700, 5300, -3000, 3000])
# Add a box in the region we are going to expand
plt.plot([-400, -400, 200, 200, -400], [-100, 100, 100, -100, -100], 'r-')
# Expanded region
plt.figure(8)
plt.clf()
plt.subplot(231)
plt.axis([-10, 5, -20, 20])
nyquist(L)
plt.axis([-10, 5, -20, 20])
# set up the color
color = 'b'
# Add arrows to the plot
# H1 = L.evalfr(0.4); H2 = L.evalfr(0.41);
# arrow([real(H1), imag(H1)], [real(H2), imag(H2)], AM_normal_arrowsize, \
# 'EdgeColor', color, 'FaceColor', color);
# H1 = freqresp(L, 0.35); H2 = freqresp(L, 0.36);
# arrow([real(H2), -imag(H2)], [real(H1), -imag(H1)], AM_normal_arrowsize, \
# 'EdgeColor', color, 'FaceColor', color);
plt.figure(9)
Yvec, Tvec = step(T, linspace(1, 20))
plt.plot(Tvec.T, Yvec.T)
Yvec, Tvec = step(Co*S, linspace(1, 20))
plt.plot(Tvec.T, Yvec.T)
#TODO: PZmap for statespace systems has not yet been implemented.
plt.figure(10)
plt.clf()
# P, Z = pzmap(T, Plot=True)
# print("Closed loop poles and zeros: ", P, Z)
# Gang of Four
plt.figure(11)
plt.clf()
gangof4(Hi*Po, Co, linspace(-2, 3))
if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
plt.show()