Repository navigation
Expand file tree
/
Copy pathpvtol-nested-ss.py
More file actions
147 lines (114 loc) · 3.64 KB
/
Copy pathpvtol-nested-ss.py
File metadata and controls
147 lines (114 loc) · 3.64 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
# 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
import numpy as np
import math
import control as ct
# 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 = ct.tf([r], [J, 0, 0]) # inner loop (roll)
Po = ct.tf([1], [m, c, 0]) # outer loop (position)
# Use state space versions
Pi = ct.tf2ss(Pi)
Po = ct.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*ct.tf([1, a], [1, b]) # lead compensator
# Convert to statespace
Ci = ct.tf2ss(Ci)
# Compute the loop transfer function for the inner loop
Li = Pi*Ci
# Bode plot for the open loop process
plt.figure(1)
ct.bode(Pi)
# Bode plot for the loop transfer function, with margins
plt.figure(2)
ct.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 = ct.feedback(1, Li)
Ti = Li*Si
# Check to make sure that the specification is met
plt.figure(3)
ct.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 = ct.parallel(ct.feedback(Ci, Pi), -m*g*ct.feedback(Ci*Pi, 1))
plt.figure(4)
plt.clf()
ct.bode(Hi)
# Now design the lateral control system
a, b, K = 0.02, 5, 2
Co = -K*ct.tf([1, 0.3], [1, 10]) # another lead compensator
# Convert to statespace
Co = ct.tf2ss(Co)
# Compute the loop transfer function for the outer loop
Lo = -m*g*Po*Co
plt.figure(5)
ct.bode(Lo, display_margins=True) # margin(Lo)
# Finally compute the real outer-loop loop gain + responses
L = Co*Hi*Po
S = ct.feedback(1, L)
T = ct.feedback(L, 1)
# Compute stability margins
#! Not yet implemented
# (gm, pm, wgc, wpc) = margin(L);
plt.figure(6)
plt.clf()
out = ct.bode(L, np.logspace(-4, 3), initial_phase=-math.pi/2)
axs = ct.get_plot_axes(out)
# Add crossover line to magnitude plot
axs[0, 0].semilogx([1e-4, 1e3], 20*np.log10([1, 1]), 'k-')
#
# Nyquist plot for complete design
#
plt.figure(7)
ct.nyquist(L)
# 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 = ct.step_response(T, np.linspace(1, 20))
plt.plot(Tvec.T, Yvec.T)
Yvec, Tvec = ct.step_response(Co*S, np.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)
# plt.suptitle("This figure intentionally blank")
# Gang of Four
plt.figure(11)
plt.clf()
ct.gangof4(Hi*Po, Co, np.linspace(-2, 3))
if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
plt.show()