Menu
▾
▴
[python-control-discuss] SF.net SVN: python-control:[23] trunk
|
From: <mur...@us...> - 2010-06-07 05:45:51
|
Revision: 23
http://python-control.svn.sourceforge.net/python-control/?rev=23&view=rev
Author: murrayrm
Date: 2010-06-07 05:45:44 +0000 (Mon, 07 Jun 2010)
Log Message:
-----------
Working LQR example + lqr() fixes
* Fixed bugs in lqr related to matrix multiplication (* vs dot())
* Added PVTOL LQR example (based on AM08 example)
* Version 0.3c ready to be released...
Modified Paths:
--------------
trunk/ChangeLog
trunk/examples/pvtol-lqr.py
trunk/src/statefbk.py
Modified: trunk/ChangeLog
===================================================================
--- trunk/ChangeLog 2010-06-06 21:24:07 UTC (rev 22)
+++ trunk/ChangeLog 2010-06-07 05:45:44 UTC (rev 23)
@@ -1,5 +1,7 @@
2010-06-06 Richard Murray <mu...@su...>
+ * examples/pvtol-lqr.py: Added example to test out LQR routines
+
* src/matlab.py (bode): created a wrapper that allows MATLAB style
arguments for bode (eg, bode(sys1, sys2))
Modified: trunk/examples/pvtol-lqr.py
===================================================================
--- trunk/examples/pvtol-lqr.py 2010-06-06 21:24:07 UTC (rev 22)
+++ trunk/examples/pvtol-lqr.py 2010-06-07 05:45:44 UTC (rev 23)
@@ -1,192 +1,195 @@
-% pvtol_lqr.m - LQR design for vectored thrust aircraft
-% RMM, 14 Jan 03
+# pvtol_lqr.m - LQR design for vectored thrust aircraft
+# RMM, 14 Jan 03
+#
+# This file works through an LQR based design problem, using the
+# planar vertical takeoff and landing (PVTOL) aircraft example from
+# Astrom and Mruray, Chapter 5. It is intended to demonstrate the
+# basic functionality of the python-control package.
+#
-aminit;
+from numpy import * # Grab all of the NumPy functions
+from matplotlib.pyplot import * # Grab MATLAB plotting functions
+from control.matlab import * # MATLAB-like functions
-%%
-%% System dynamics
-%%
-%% These are the dynamics for the PVTOL system, written in state space
-%% form.
-%%
+#
+# System dynamics
+#
+# These are the dynamics for the PVTOL system, written in state space
+# form.
+#
-pvtol_params; % System parameters
+# 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)
-% System matrices (entire plant: 2 input, 2 output)
-xe = [0 0 0 0 0 0]; ue = [0 m*g];
-[A, B, C, D] = pvtol_linearize(xe, ue);
+# State space dynamics
+xe = [0, 0, 0, 0, 0, 0]; # equilibrium point of interest
+ue = [0, m*g]; # (note these are lists, not matrices)
-%%
-%% Construct inputs and outputs corresponding to steps in xy position
-%%
-%% The vectors xd and yd correspond to the states that are the desired
-%% equilibrium states for the system. The matrices Cx and Cy are the
-%% corresponding outputs.
-%%
-%% The way these vectors are used is to compute the closed loop system
-%% dynamics as
-%%
-%% xdot = Ax + B u => xdot = (A-BK)x + K xd
-%% u = -K(x - xd) y = Cx
-%%
-%% The closed loop dynamics can be simulated using the "step" command,
-%% with K*xd as the input vector (assumes that the "input" is unit size,
-%% so that xd corresponds to the desired steady state.
-%%
+# Dynamics matrix (use matrix type so that * works for multiplication)
+A = matrix(
+ [[ 0, 0, 0, 1, 0, 0],
+ [ 0, 0, 0, 0, 1, 0],
+ [ 0, 0, 0, 0, 0, 1],
+ [ 0, 0, (-ue[0]*sin(xe[2]) - ue[1]*cos(xe[2]))/m, -c/m, 0, 0],
+ [ 0, 0, (ue[0]*cos(xe[2]) - ue[1]*sin(xe[2]))/m, 0, -c/m, 0],
+ [ 0, 0, 0, 0, 0, 0 ]])
-xd = [1; 0; 0; 0; 0; 0]; Cx = [1 0 0 0 0 0];
-yd = [0; 1; 0; 0; 0; 0]; Cy = [0 1 0 0 0 0];
+# Input matrix
+B = matrix(
+ [[0, 0], [0, 0], [0, 0],
+ [cos(xe[2])/m, -sin(xe[2])/m],
+ [sin(xe[2])/m, cos(xe[2])/m],
+ [r/J, 0]])
-%%
-%% LQR design
-%%
+# Output matrix
+C = matrix([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
+D = matrix([[0, 0], [0, 0]])
-% Start with a diagonal weighting
-Qx1 = diag([1, 1, 1, 1, 1, 1]);
-Qu1a = diag([1, 1]);
-K1a = lqr(A, B, Qx1, Qu1a);
+#
+# Construct inputs and outputs corresponding to steps in xy position
+#
+# The vectors xd and yd correspond to the states that are the desired
+# equilibrium states for the system. The matrices Cx and Cy are the
+# corresponding outputs.
+#
+# The way these vectors are used is to compute the closed loop system
+# dynamics as
+#
+# xdot = Ax + B u => xdot = (A-BK)x + K xd
+# u = -K(x - xd) y = Cx
+#
+# The closed loop dynamics can be simulated using the "step" command,
+# with K*xd as the input vector (assumes that the "input" is unit size,
+# so that xd corresponds to the desired steady state.
+#
-% Close the loop: xdot = Ax + B K (x-xd)
-H1a = ss(A-B*K1a, B*K1a*[xd, yd], [Cx; Cy], 0);
-[Y, T] = step(H1a, 10);
+xd = matrix([[1], [0], [0], [0], [0], [0]]);
+yd = matrix([[0], [1], [0], [0], [0], [0]]);
-figure(1); clf; subplot(321);
- plot(T, Y(:,1, 1), '-', T, Y(:,2, 2), '--', ...
- 'Linewidth', AM_data_linewidth); hold on;
- plot([0 10], [1 1], 'k-', 'Linewidth', AM_ref_linewidth); hold on;
+#
+# Extract the relevant dynamics for use with SISO library
+#
+# The current python-control library only supports SISO transfer
+# functions, so we have to modify some parts of the original MATLAB
+# code to extract out SISO systems. To do this, we define the 'lat' and
+# 'alt' index vectors to consist of the states that are are relevant
+# to the lateral (x) and vertical (y) dynamics.
+#
- amaxis([0, 10, -0.1, 1.4]);
- xlabel('time'); ylabel('position');
+# Indices for the parts of the state that we want
+lat = (0,2,3,5);
+alt = (1,4);
- lgh = legend('x', 'y', 'Location', 'southeast');
- legend(lgh, 'boxoff');
- amprint('pvtol-lqrstep1.eps');
+# Decoupled dynamics
+Ax = (A[lat, :])[:, lat]; #! not sure why I have to do it this way
+Bx = B[lat, 0]; Cx = C[0, lat]; Dx = D[0, 0];
-% Look at different input weightings
-Qu1a = diag([1, 1]); K1a = lqr(A, B, Qx1, Qu1a);
-H1ax = ss(A-B*K1a,B(:,1)*K1a(1,:)*xd,Cx,0);
+Ay = (A[alt, :])[:, alt]; #! not sure why I have to do it this way
+By = B[alt, 1]; Cy = C[1, alt]; Dy = D[1, 1];
-Qu1b = 40^2*diag([1, 1]); K1b = lqr(A, B, Qx1, Qu1b);
-H1bx = ss(A-B*K1b,B(:,1)*K1b(1,:)*xd,Cx,0);
+# Label the plot
+clf();
+suptitle("LQR controllers for vectored thrust aircraft (pvtol-lqr)")
-Qu1c = 200^2*diag([1, 1]); K1c = lqr(A, B, Qx1, Qu1c);
-H1cx = ss(A-B*K1c,B(:,1)*K1c(1,:)*xd,Cx,0);
+#
+# LQR design
+#
-[Y1, T1] = step(H1ax, 10);
-[Y2, T2] = step(H1bx, 10);
-[Y3, T3] = step(H1cx, 10);
+# Start with a diagonal weighting
+Qx1 = diag([1, 1, 1, 1, 1, 1]);
+Qu1a = diag([1, 1]);
+(K, X, E) = lqr(A, B, Qx1, Qu1a); K1a = matrix(K);
-figure(2); clf; subplot(321);
- plot(T1, Y1, 'b-', 'Linewidth', AM_data_linewidth); hold on;
- plot(T2, Y2, 'b-', 'Linewidth', AM_data_linewidth); hold on;
- plot(T3, Y3, 'b-', 'Linewidth', AM_data_linewidth); hold on;
- plot([0 10], [1 1], 'k-', 'Linewidth', AM_ref_linewidth); hold on;
+# Close the loop: xdot = Ax - B K (x-xd)
+# Note: python-control requires we do this 1 input at a time
+# H1a = ss(A-B*K1a, B*K1a*concatenate((xd, yd), axis=1), C, D);
+# (T, Y) = step(H1a, T=linspace(0,10,100));
- amaxis([0, 10, -0.1, 1.4]);
- xlabel('time'); ylabel('position');
+# Step response for the first input
+H1ax = ss(Ax - Bx*K1a[0,lat], Bx*K1a[0,lat]*xd[lat,:], Cx, Dx);
+(Tx, Yx) = step(H1ax, T=linspace(0,10,100));
+print Tx.shape
+print Yx.shape
- arcarrow([1.3 0.8], [5 0.45], -6);
- text(5.3, 0.4, 'rho');
+# Step response for the second input
+H1ay = ss(Ay - By*K1a[1,alt], By*K1a[1,alt]*yd[alt,:], Cy, Dy);
+(Ty, Yy) = step(H1ay, T=linspace(0,10,100));
- amprint('pvtol-lqrstep2.eps');
+subplot(221); title("Identity weights")
+# plot(T, Y[:,1, 1], '-', T, Y[:,2, 2], '--'); hold(True);
+plot(Tx, Yx[0,:].T, '-', Ty, Yy[0,:].T, '--'); hold(True);
+plot([0, 10], [1, 1], 'k-'); hold(True);
-% Output weighting - change Qx to use outputs
-Qx2 = [Cx; Cy]' * [Cx; Cy];
-Qu2 = 0.1 * diag([1, 1]);
-K2 = lqr(A, B, Qx2, Qu2);
+axis([0, 10, -0.1, 1.4]);
+ylabel('position');
+legend(('x', 'y'), loc='lower right');
-H2x = ss(A-B*K2,B(:,1)*K2(1,:)*xd,Cx,0);
-H2y = ss(A-B*K2,B(:,2)*K2(2,:)*yd,Cy,0);
+# Look at different input weightings
+Qu1a = diag([1, 1]); (K1a, X, E) = lqr(A, B, Qx1, Qu1a);
+H1ax = ss(Ax - Bx*K1a[0,lat], Bx*K1a[0,lat]*xd[lat,:], Cx, Dx);
-figure(3); step(H2x, H2y, 10);
-legend('x', 'y');
+Qu1b = (40**2)*diag([1, 1]); (K1b, X, E) = lqr(A, B, Qx1, Qu1b);
+H1bx = ss(Ax - Bx*K1b[0,lat], Bx*K1b[0,lat]*xd[lat,:],Cx, Dx);
-%%
-%% Physically motivated weighting
-%%
-%% Shoot for 1 cm error in x, 10 cm error in y. Try to keep the angle
-%% less than 5 degrees in making the adjustments. Penalize side forces
-%% due to loss in efficiency.
-%%
+Qu1c = (200**2)*diag([1, 1]); (K1c, X, E) = lqr(A, B, Qx1, Qu1c);
+H1cx = ss(Ax - Bx*K1c[0,lat], Bx*K1c[0,lat]*xd[lat,:],Cx, Dx);
-Qx3 = diag([100, 10, 2*pi/5, 0, 0, 0]);
-Qu3 = 0.1 * diag([1, 10]);
-K3 = lqr(A, B, Qx3, Qu3);
+[T1, Y1] = step(H1ax, T=linspace(0,10,100));
+[T2, Y2] = step(H1bx, T=linspace(0,10,100));
+[T3, Y3] = step(H1cx, T=linspace(0,10,100));
-H3x = ss(A-B*K3,B(:,1)*K3(1,:)*xd,Cx,0);
-H3y = ss(A-B*K3,B(:,2)*K3(2,:)*yd,Cy,0);
-figure(4); clf; subplot(221);
-step(H3x, H3y, 10);
-legend('x', 'y');
+subplot(222); title("Effect of input weights")
+plot(T1, Y1[0,:].T, 'b-'); hold(True);
+plot(T2, Y2[0,:].T, 'b-'); hold(True);
+plot(T3, Y3[0,:].T, 'b-'); hold(True);
+plot([0 ,10], [1, 1], 'k-'); hold(True);
-%%
-%% Velocity control
-%%
-%% In this example, we modify the system so that we control the
-%% velocity of the system in the x direction. We ignore the
-%% dynamics in the vertical (y) direction. These dynamics demonstrate
-%% the role of the feedforward system since the equilibrium point
-%% corresponding to vd neq 0 requires a nonzero input.
-%%
-%% For this example, we use a control law u = -K(x-xd) + ud and convert
-%% this to the form u = -K x + N r, where r is the reference input and
-%% N is computed as described in class.
-%%
+axis([0, 10, -0.1, 1.4]);
-% Extract system dynamics: theta, xdot, thdot
-Av = A([3 4 6], [3 4 6]);
-Bv = B([3 4 6], 1);
-Cv = [0 1 0]; % choose vx as output
-Dv = 0;
+# arcarrow([1.3, 0.8], [5, 0.45], -6);
+text(5.3, 0.4, 'rho');
-% Design the feedback term using LQR
-Qxv = diag([2*pi/5, 10, 0]);
-Quv = 0.1;
-Kv = lqr(Av, Bv, Qxv, Quv);
+# Output weighting - change Qx to use outputs
+Qx2 = C.T * C;
+Qu2 = 0.1 * diag([1, 1]);
+(K, X, E) = lqr(A, B, Qx2, Qu2); K2 = matrix(K)
-% Design the feedforward term by solve for eq pt in terms of reference r
-T = [Av Bv; Cv Dv]; % system matrix
-Nxu = T \ [0; 0; 0; 1]; % compute [Nx; Nu]
-Nx = Nxu(1:3); Nu = Nxu(4); % extract Nx and Nu
-N = Nu + Kv*Nx; % compute feedforward term
+H2x = ss(Ax - Bx*K2[0,lat], Bx*K2[0,lat]*xd[lat,:], Cx, Dx);
+H2y = ss(Ay - By*K2[1,alt], By*K2[1,alt]*yd[alt,:], Cy, Dy);
-%%
-%% Design #1: no feedforward input, ud
-%%
+subplot(223); title("Output weighting")
+[T2x, Y2x] = step(H2x, T=linspace(0,10,100));
+[T2y, Y2y] = step(H2y, T=linspace(0,10,100));
+plot(T2x, Y2x[0,:].T, T2y, Y2y[0,:].T)
+ylabel('position');
+xlabel('time'); ylabel('position');
+legend(('x', 'y'), loc='lower right');
-Nv1 = [0; 1; 0];
-Hv1 = ss(Av-Bv*Kv, Bv*Kv*Nx, Cv, 0);
-step(Hv1, 10);
+#
+# Physically motivated weighting
+#
+# Shoot for 1 cm error in x, 10 cm error in y. Try to keep the angle
+# less than 5 degrees in making the adjustments. Penalize side forces
+# due to loss in efficiency.
+#
-%%
-%% Design #2: compute feedforward gain corresponding to equilibrium point
-%%
+Qx3 = diag([100, 10, 2*pi/5, 0, 0, 0]);
+Qu3 = 0.1 * diag([1, 10]);
+(K, X, E) = lqr(A, B, Qx3, Qu3); K3 = matrix(K);
-Hv2 = ss(Av-Bv*Kv, Bv*N, Cv, 0);
-step(Hv2, 10);
+H3x = ss(Ax - Bx*K3[0,lat], Bx*K3[0,lat]*xd[lat,:], Cx, Dx);
+H3y = ss(Ay - By*K3[1,alt], By*K3[1,alt]*yd[alt,:], Cy, Dy);
+subplot(224)
+# step(H3x, H3y, 10);
+[T3x, Y3x] = step(H3x, T=linspace(0,10,100));
+[T3y, Y3y] = step(H3y, T=linspace(0,10,100));
+plot(T3x, Y3x[0,:].T, T3y, Y3y[0,:].T)
+title("Physically motivated weights")
+xlabel('time');
+legend(('x', 'y'), loc='lower right');
-%%
-%% Design #3: integral action
-%%
-%% Add a new state to the system that is given by xidot = v - vd. We
-%% construct the control law by computing an LQR gain for the augmented
-%% system.
-%%
-
-Ai = [Av, [0; 0; 0]; [Cv, 0]];
-Bi = [Bv; 0];
-Ci = [Cv, 0];
-Di = Dv;
-
-% Design the feedback term, including weight on integrator error
-Qxi = diag([2*pi/5, 10, 0, 10]);
-Qui = 0.1;
-Ki = lqr(Ai, Bi, Qxi, Qui);
-
-% Desired state (augmented)
-xid = [0; 1; 0; 0];
-
-% Construct the closed loop system (including integrator)
-Hi = ss(Ai-Bi*Ki,Bi*Ki*xid - [0; 0; 0; Ci*xid],Ci,0);
-step(Hi, 10);
-
+show()
Modified: trunk/src/statefbk.py
===================================================================
--- trunk/src/statefbk.py 2010-06-06 21:24:07 UTC (rev 22)
+++ trunk/src/statefbk.py 2010-06-07 05:45:44 UTC (rev 23)
@@ -41,6 +41,7 @@
# External packages and modules
import numpy as np
+import ctrlutil
from control.exception import *
# Pole placement
@@ -139,8 +140,7 @@
if (len(args) < 4):
raise ControlArgument("not enough input arguments")
- elif (getattr(args[0], 'A', None) and
- getattr(args[0], 'B', None)):
+ elif (ctrlutil.issys(args[0])):
# We were passed a system as the first argument; extract A and B
#! TODO: really just need to check for A and B attributes
A = np.array(args[0].A, ndmin=2, dtype=float);
@@ -179,7 +179,7 @@
X,rcond,w,S,U = sb02md(nstates, A_b, G, Q_b, 'C')
# Now compute the return value
- K = np.linalg.inv(R) * (np.transpose(B) * X + np.transpose(N));
+ K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T));
S = X;
E = w[0:nstates];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
