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
|
## Copyright (C) 1996 John W. Eaton
##
## This file is part of Octave.
##
## Octave is free software; you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 2, or (at your option)
## any later version.
##
## Octave is distributed in the hope that it will be useful, but
## WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
## General Public License for more details.
##
## You should have received a copy of the GNU General Public License
## along with Octave; see the file COPYING. If not, write to the Free
## Software Foundation, 59 Temple Place - Suite 330, Boston, MA
## 02111-1307, USA.
## Usage: [k, p, e] = dlqr (A, B, Q, R {,Z})
##
## Linear quadratic regulator design for the discrete time system
##
## x[k+1] = A x[k] + B u[k]
##
## to minimize the cost functional
##
## J = Sum { x' Q x + u' R u } Z omitted
##
## or
##
## J = Sum { x' Q x + u' R u +2 x' Z u} Z included
##
## Returns:
##
## k = state feedback gain, (A - B K) is stable
## p = solution of algebraic Riccati equation
## e = closed loop poles of (A - B K)
## Author: A. S. Hodel <scotte@eng.auburn.edu>
## R. B. Tenison <btenison@eng.auburn.edu>
## Created: August 1993
## Adapted-By: jwe
function [k, p, e] = dlqr (a, b, q, r, zz)
if (nargin != 4 && nargin != 5)
error ("dlqr: invalid number of arguments");
endif
## Check a.
if ((n = is_square (a)) == 0)
error ("dlqr: requires 1st parameter(a) to be square");
endif
## Check b.
[n1, m] = size (b);
if (n1 != n)
error ("dlqr: a,b not conformal");
endif
## Check q.
if ((n1 = is_square (q)) == 0 || n1 != n)
error ("dlqr: q must be square and conformal with a");
endif
## Check r.
if((m1 = is_square(r)) == 0 || m1 != m)
error ("dlqr: r must be square and conformal with column dimension of b");
endif
## Check if n is there.
if (nargin == 5)
[n1, m1] = size (zz);
if (n1 != n || m1 != m)
error ("dlqr: z must be identically dimensioned with b");
endif
## Incorporate cross term into a and q.
ao = a - (b/r)*zz';
qo = q - (zz/r)*zz';
else
zz = zeros (n, m);
ao = a;
qo = q;
endif
## Check that q, (r) are symmetric, positive (semi)definite
if (is_symmetric (q) && is_symmetric (r) ...
&& all (eig (q) >= 0) && all (eig (r) > 0))
p = dare (ao, b, qo, r);
k = (r+b'*p*b)\b'*p*ao + r\zz';
e = eig (a - b*k);
else
error ("dlqr: q (r) must be symmetric positive (semi) definite");
endif
endfunction
|
