|
7017
|
1 ## Copyright (C) 1997, 2000, 2002, 2003, 2004, 2005, 2007 |
|
|
2 ## Jose Daniel Munoz Frias |
|
3432
|
3 ## |
|
|
4 ## This file is part of Octave. |
|
|
5 ## |
|
|
6 ## Octave is free software; you can redistribute it and/or modify it |
|
7016
|
7 ## under the terms of the GNU General Public License as published by |
|
|
8 ## the Free Software Foundation; either version 3 of the License, or (at |
|
|
9 ## your option) any later version. |
|
3432
|
10 ## |
|
7016
|
11 ## Octave is distributed in the hope that it will be useful, but |
|
|
12 ## WITHOUT ANY WARRANTY; without even the implied warranty of |
|
|
13 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
|
14 ## General Public License for more details. |
|
3432
|
15 ## |
|
|
16 ## You should have received a copy of the GNU General Public License |
|
7016
|
17 ## along with Octave; see the file COPYING. If not, see |
|
|
18 ## <http://www.gnu.org/licenses/>. |
|
3432
|
19 |
|
|
20 ## -*- texinfo -*- |
|
5016
|
21 ## @deftypefn {Function File} {@var{K} =} place (@var{sys}, @var{p}) |
|
|
22 ## Computes the matrix @var{K} such that if the state |
|
|
23 ## is feedback with gain @var{K}, then the eigenvalues of the closed loop |
|
|
24 ## system (i.e. @math{A-BK}) are those specified in the vector @var{p}. |
|
3432
|
25 ## |
|
|
26 ## Version: Beta (May-1997): If you have any comments, please let me know. |
|
|
27 ## (see the file place.m for my address) |
|
|
28 ## @end deftypefn |
|
|
29 |
|
|
30 ## Author: Jose Daniel Munoz Frias |
|
|
31 |
|
|
32 ## Universidad Pontificia Comillas |
|
|
33 ## ICAIdea |
|
|
34 ## Alberto Aguilera, 23 |
|
|
35 ## 28015 Madrid, Spain |
|
|
36 ## |
|
|
37 ## E-Mail: daniel@dea.icai.upco.es |
|
|
38 ## |
|
|
39 ## Phone: 34-1-5422800 Fax: 34-1-5596569 |
|
|
40 ## |
|
|
41 ## Algorithm taken from "The Control Handbook", IEEE press pp. 209-212 |
|
|
42 ## |
|
|
43 ## code adaped by A.S.Hodel (a.s.hodel@eng.auburn.edu) for use in controls |
|
|
44 ## toolbox |
|
|
45 |
|
|
46 function K = place (sys, P) |
|
|
47 |
|
7125
|
48 if (nargin != 2) |
|
|
49 print_usage (); |
|
|
50 endif |
|
|
51 |
|
5568
|
52 ## check arguments |
|
3432
|
53 |
|
7126
|
54 if (! isstruct (sys)) |
|
7125
|
55 error ("sys must be in system data structure format (see ss)"); |
|
5568
|
56 endif |
|
7125
|
57 sys = sysupdate (sys, "ss"); # make sure it has state space form up to date |
|
|
58 if (! is_controllable (sys)) |
|
|
59 error ("sys is not controllable"); |
|
|
60 elseif (min (size (P)) != 1) |
|
|
61 error ("P must be a vector") |
|
5568
|
62 else |
|
7125
|
63 P = P(:); # make P a column vector |
|
5568
|
64 endif |
|
|
65 ## system must be purely continuous or discrete |
|
7125
|
66 is_digital (sys); |
|
|
67 [n, nz, m, p] = sysdimensions (sys); |
|
5568
|
68 nx = n+nz; # already checked that it's not a mixed system. |
|
7126
|
69 if (m != 1) |
|
7125
|
70 error ("sys has %d inputs; need only 1", m); |
|
5568
|
71 endif |
|
3432
|
72 |
|
5568
|
73 ## takes the A and B matrix from the system representation |
|
7125
|
74 [A, B] = sys2ss (sys); |
|
|
75 sp = length (P); |
|
|
76 if (nx == 0) |
|
|
77 error ("place: A matrix is empty (0x0)"); |
|
|
78 elseif (nx != length (P)) |
|
|
79 error ("A=(%dx%d), P has %d entries", nx, nx, length (P)) |
|
5568
|
80 endif |
|
3432
|
81 |
|
5568
|
82 ## arguments appear to be compatible; let's give it a try! |
|
|
83 ## The second step is the calculation of the characteristic polynomial ofA |
|
7125
|
84 PC = poly (A); |
|
3432
|
85 |
|
5568
|
86 ## Third step: Calculate the transformation matrix T that transforms the state |
|
|
87 ## equation in the controllable canonical form. |
|
4460
|
88 |
|
5568
|
89 ## first we must calculate the controllability matrix M: |
|
7125
|
90 M = B; |
|
|
91 AA = A; |
|
5568
|
92 for n = 2:nx |
|
7125
|
93 M(:,n) = AA*B; |
|
|
94 AA = AA*A; |
|
5568
|
95 endfor |
|
3432
|
96 |
|
5568
|
97 ## second, construct the matrix W |
|
7125
|
98 PCO = PC(nx:-1:1); |
|
|
99 PC1 = PCO; # Matrix to shift and create W row by row |
|
3432
|
100 |
|
5568
|
101 for n = 1:nx |
|
|
102 W(n,:) = PC1; |
|
7125
|
103 PC1 = [PCO(n+1:nx), zeros(1,n)]; |
|
5568
|
104 endfor |
|
3432
|
105 |
|
7125
|
106 T = M*W; |
|
3432
|
107 |
|
5568
|
108 ## finaly the matrix K is calculated |
|
7125
|
109 PD = poly (P); # The desired characteristic polynomial |
|
5568
|
110 PD = PD(nx+1:-1:2); |
|
|
111 PC = PC(nx+1:-1:2); |
|
3432
|
112 |
|
5568
|
113 K = (PD-PC)/T; |
|
3432
|
114 |
|
5568
|
115 ## Check if the eigenvalues of (A-BK) are the same specified in P |
|
7125
|
116 Pcalc = eig (A-B*K); |
|
3432
|
117 |
|
7125
|
118 Pcalc = sortcom (Pcalc); |
|
|
119 P = sortcom (P); |
|
3432
|
120 |
|
7125
|
121 if (max ((abs(Pcalc)-abs(P))./abs(P) ) > 0.1) |
|
|
122 warning ("place: Pole placed at more than 10% relative error from specified"); |
|
5568
|
123 endif |
|
4460
|
124 |
|
3432
|
125 endfunction |
|
|
126 |
