# LQR control design for vertical takeoff and landing aircraft¶

This script demonstrates the use of the python-control package for analysis and design of a controller for a vectored thrust aircraft model that is used as a running example through the text Feedback Systems by Astrom and Murray. This example makes use of MATLAB compatible commands.

## Code¶

  1# pvtol_lqr.m - LQR design for vectored thrust aircraft
2# RMM, 14 Jan 03
3#
4# This file works through an LQR based design problem, using the
5# planar vertical takeoff and landing (PVTOL) aircraft example from
6# Astrom and Murray, Chapter 5.  It is intended to demonstrate the
7# basic functionality of the python-control package.
8#
9
10import os
11import numpy as np
12import matplotlib.pyplot as plt  # MATLAB-like plotting functions
13import control as ct
14
15#
16# System dynamics
17#
18# These are the dynamics for the PVTOL system, written in state space
19# form.
20#
21
22# System parameters
23m = 4       # mass of aircraft
24J = 0.0475  # inertia around pitch axis
25r = 0.25    # distance to center of force
26g = 9.8     # gravitational constant
27c = 0.05    # damping factor (estimated)
28
29# State space dynamics
30xe = [0, 0, 0, 0, 0, 0]  # equilibrium point of interest
31ue = [0, m * g]  # (note these are lists, not matrices)
32
33# TODO: The following objects need converting from np.matrix to np.array
34# This will involve re-working the subsequent equations as the shapes
35# See below.
36
37A = np.array(
38    [[0, 0, 0, 1, 0, 0],
39     [0, 0, 0, 0, 1, 0],
40     [0, 0, 0, 0, 0, 1],
41     [0, 0, (-ue*np.sin(xe) - ue*np.cos(xe))/m, -c/m, 0, 0],
42     [0, 0, (ue*np.cos(xe) - ue*np.sin(xe))/m, 0, -c/m, 0],
43     [0, 0, 0, 0, 0, 0]]
44)
45
46# Input matrix
47B = np.array(
48    [[0, 0], [0, 0], [0, 0],
49     [np.cos(xe)/m, -np.sin(xe)/m],
50     [np.sin(xe)/m, np.cos(xe)/m],
51     [r/J, 0]]
52)
53
54# Output matrix
55C = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
56D = np.array([[0, 0], [0, 0]])
57
58#
59# Construct inputs and outputs corresponding to steps in xy position
60#
61# The vectors xd and yd correspond to the states that are the desired
62# equilibrium states for the system.  The matrices Cx and Cy are the
63# corresponding outputs.
64#
65# The way these vectors are used is to compute the closed loop system
66# dynamics as
67#
68#   xdot = Ax + B u  =>  xdot = (A-BK)x + K xd
69#      u = -K(x - xd)       y = Cx
70#
71# The closed loop dynamics can be simulated using the "step" command,
72# with K*xd as the input vector (assumes that the "input" is unit size,
73# so that xd corresponds to the desired steady state.
74#
75
76xd = np.array([, , , , , ])
77yd = np.array([, , , , , ])
78
79#
80# Extract the relevant dynamics for use with SISO library
81#
82# The current python-control library only supports SISO transfer
83# functions, so we have to modify some parts of the original MATLAB
84# code to extract out SISO systems.  To do this, we define the 'lat' and
85# 'alt' index vectors to consist of the states that are are relevant
86# to the lateral (x) and vertical (y) dynamics.
87#
88
89# Indices for the parts of the state that we want
90lat = (0, 2, 3, 5)
91alt = (1, 4)
92
93# Decoupled dynamics
94Ax = A[np.ix_(lat, lat)]
95Bx = B[np.ix_(lat, )]
96Cx = C[np.ix_(, lat)]
97Dx = D[np.ix_(, )]
98
99Ay = A[np.ix_(alt, alt)]
100By = B[np.ix_(alt, )]
101Cy = C[np.ix_(, alt)]
102Dy = D[np.ix_(, )]
103
104# Label the plot
105plt.clf()
106plt.suptitle("LQR controllers for vectored thrust aircraft (pvtol-lqr)")
107
108#
109# LQR design
110#
111
113Qx1 = np.diag([1, 1, 1, 1, 1, 1])
114Qu1a = np.diag([1, 1])
115K1a, X, E = ct.lqr(A, B, Qx1, Qu1a)
116
117# Close the loop: xdot = Ax - B K (x-xd)
118#
119# Note: python-control requires we do this 1 input at a time
120# H1a = ss(A-B*K1a, B*K1a*concatenate((xd, yd), axis=1), C, D);
121# (T, Y) = step_response(H1a, T=np.linspace(0,10,100));
122#
123
124# Step response for the first input
125H1ax = ct.ss(Ax - Bx @ K1a[np.ix_(, lat)],
126             Bx @ K1a[np.ix_(, lat)] @ xd[lat, :], Cx, Dx)
127Tx, Yx = ct.step_response(H1ax, T=np.linspace(0, 10, 100))
128
129# Step response for the second input
130H1ay = ct.ss(Ay - By @ K1a[np.ix_(, alt)],
131             By @ K1a[np.ix_(, alt)] @ yd[alt, :], Cy, Dy)
132Ty, Yy = ct.step_response(H1ay, T=np.linspace(0, 10, 100))
133
134plt.subplot(221)
135plt.title("Identity weights")
136# plt.plot(T, Y[:,1, 1], '-', T, Y[:,2, 2], '--')
137plt.plot(Tx.T, Yx.T, '-', Ty.T, Yy.T, '--')
138plt.plot([0, 10], [1, 1], 'k-')
139
140plt.axis([0, 10, -0.1, 1.4])
141plt.ylabel('position')
142plt.legend(('x', 'y'), loc='lower right')
143
144# Look at different input weightings
145Qu1a = np.diag([1, 1])
146K1a, X, E = ct.lqr(A, B, Qx1, Qu1a)
147H1ax = ct.ss(Ax - Bx @ K1a[np.ix_(, lat)],
148             Bx @ K1a[np.ix_(, lat)] @ xd[lat, :], Cx, Dx)
149
150Qu1b = (40 ** 2)*np.diag([1, 1])
151K1b, X, E = ct.lqr(A, B, Qx1, Qu1b)
152H1bx = ct.ss(Ax - Bx @ K1b[np.ix_(, lat)],
153             Bx @ K1b[np.ix_(, lat)] @ xd[lat, :], Cx, Dx)
154
155Qu1c = (200 ** 2)*np.diag([1, 1])
156K1c, X, E = ct.lqr(A, B, Qx1, Qu1c)
157H1cx = ct.ss(Ax - Bx @ K1c[np.ix_(, lat)],
158             Bx @ K1c[np.ix_(, lat)] @ xd[lat, :], Cx, Dx)
159
160T1, Y1 = ct.step_response(H1ax, T=np.linspace(0, 10, 100))
161T2, Y2 = ct.step_response(H1bx, T=np.linspace(0, 10, 100))
162T3, Y3 = ct.step_response(H1cx, T=np.linspace(0, 10, 100))
163
164plt.subplot(222)
165plt.title("Effect of input weights")
166plt.plot(T1.T, Y1.T, 'b-')
167plt.plot(T2.T, Y2.T, 'b-')
168plt.plot(T3.T, Y3.T, 'b-')
169plt.plot([0, 10], [1, 1], 'k-')
170
171plt.axis([0, 10, -0.1, 1.4])
172
173# arcarrow([1.3, 0.8], [5, 0.45], -6)
174plt.text(5.3, 0.4, r'$\rho$')
175
176# Output weighting - change Qx to use outputs
177Qx2 = C.T @ C
178Qu2 = 0.1 * np.diag([1, 1])
179K2, X, E = ct.lqr(A, B, Qx2, Qu2)
180
181H2x = ct.ss(Ax - Bx @ K2[np.ix_(, lat)],
182            Bx @ K2[np.ix_(, lat)] @ xd[lat, :], Cx, Dx)
183H2y = ct.ss(Ay - By @ K2[np.ix_(, alt)],
184            By @ K2[np.ix_(, alt)] @ yd[alt, :], Cy, Dy)
185
186plt.subplot(223)
187plt.title("Output weighting")
188T2x, Y2x = ct.step_response(H2x, T=np.linspace(0, 10, 100))
189T2y, Y2y = ct.step_response(H2y, T=np.linspace(0, 10, 100))
190plt.plot(T2x.T, Y2x.T, T2y.T, Y2y.T)
191plt.ylabel('position')
192plt.xlabel('time')
193plt.ylabel('position')
194plt.legend(('x', 'y'), loc='lower right')
195
196#
197# Physically motivated weighting
198#
199# Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle
200# less than 5 degrees in making the adjustments.  Penalize side forces
201# due to loss in efficiency.
202#
203
204Qx3 = np.diag([100, 10, 2*np.pi/5, 0, 0, 0])
205Qu3 = 0.1*np.diag([1, 10])
206K3, X, E = ct.lqr(A, B, Qx3, Qu3)
207
208H3x = ct.ss(Ax - Bx @ K3[np.ix_(, lat)],
209            Bx @ K3[np.ix_(, lat)] @ xd[lat, :], Cx, Dx)
210H3y = ct.ss(Ay - By @ K3[np.ix_(, alt)],
211            By @ K3[np.ix_(, alt)] @ yd[alt, :], Cy, Dy)
212plt.subplot(224)
213# step_response(H3x, H3y, 10)
214T3x, Y3x = ct.step_response(H3x, T=np.linspace(0, 10, 100))
215T3y, Y3y = ct.step_response(H3y, T=np.linspace(0, 10, 100))
216plt.plot(T3x.T, Y3x.T, T3y.T, Y3y.T)
217plt.title("Physically motivated weights")
218plt.xlabel('time')
219plt.legend(('x', 'y'), loc='lower right')
220plt.tight_layout()
221
222if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
223    plt.show()