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