octave_packages/control-2.3.52/care.m

   1 ## Copyright (C) 2009, 2010   Lukas F. Reichlin
   2 ##
   3 ## This file is part of LTI Syncope.
   4 ##
   5 ## LTI Syncope is free software: you can redistribute it and/or modify
   6 ## it under the terms of the GNU General Public License as published by
   7 ## the Free Software Foundation, either version 3 of the License, or
   8 ## (at your option) any later version.
   9 ##
  10 ## LTI Syncope is distributed in the hope that it will be useful,
  11 ## but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 ## GNU General Public License for more details.
  14 ##
  15 ## You should have received a copy of the GNU General Public License
  16 ## along with LTI Syncope.  If not, see <http://www.gnu.org/licenses/>.
  17
  18 ## -*- texinfo -*-
  19 ## @deftypefn {Function File} {[@var{x}, @var{l}, @var{g}] =} care (@var{a}, @var{b}, @var{q}, @var{r})
  20 ## @deftypefnx {Function File} {[@var{x}, @var{l}, @var{g}] =} care (@var{a}, @var{b}, @var{q}, @var{r}, @var{s})
  21 ## @deftypefnx {Function File} {[@var{x}, @var{l}, @var{g}] =} care (@var{a}, @var{b}, @var{q}, @var{r}, @var{[]}, @var{e})
  22 ## @deftypefnx {Function File} {[@var{x}, @var{l}, @var{g}] =} care (@var{a}, @var{b}, @var{q}, @var{r}, @var{s}, @var{e})
  23 ## Solve continuous-time algebraic Riccati equation (ARE).
  24 ##
  25 ## @strong{Inputs}
  26 ## @table @var
  27 ## @item a
  28 ## Real matrix (n-by-n).
  29 ## @item b
  30 ## Real matrix (n-by-m).
  31 ## @item q
  32 ## Real matrix (n-by-n).
  33 ## @item r
  34 ## Real matrix (m-by-m).
  35 ## @item s
  36 ## Optional real matrix (n-by-m).  If @var{s} is not specified, a zero matrix is assumed.
  37 ## @item e
  38 ## Optional descriptor matrix (n-by-n).  If @var{e} is not specified, an identity matrix is assumed.
  39 ## @end table
  40 ##
  41 ## @strong{Outputs}
  42 ## @table @var
  43 ## @item x
  44 ## Unique stabilizing solution of the continuous-time Riccati equation (n-by-n).
  45 ## @item l
  46 ## Closed-loop poles (n-by-1).
  47 ## @item g
  48 ## Corresponding gain matrix (m-by-n).
  49 ## @end table
  50 ##
  51 ## @strong{Equations}
  52 ## @example
  53 ## @group
  54 ##                -1
  55 ## A'X + XA - XB R  B'X + Q = 0
  56 ##
  57 ##                      -1
  58 ## A'X + XA - (XB + S) R  (B'X + S') + Q = 0
  59 ##
  60 ##      -1
  61 ## G = R  B'X
  62 ##
  63 ##      -1
  64 ## G = R  (B'X + S')
  65 ##
  66 ## L = eig (A - B*G)
  67 ## @end group
  68 ## @end example
  69 ## @example
  70 ## @group
  71 ##                     -1
  72 ## A'XE + E'XA - E'XB R   B'XE + Q = 0
  73 ##
  74 ##                           -1
  75 ## A'XE + E'XA - (E'XB + S) R   (B'XE + S') + Q = 0
  76 ##
  77 ##      -1
  78 ## G = R  B'XE
  79 ##
  80 ##      -1
  81 ## G = R  (B'XE + S)
  82 ##
  83 ## L = eig (A - B*G, E)
  84 ## @end group
  85 ## @end example
  86 ##
  87 ## @strong{Algorithm}@*
  88 ## Uses SLICOT SB02OD and SG02AD by courtesy of
  89 ## @uref{http://www.slicot.org, NICONET e.V.}
  90 ##
  91 ## @seealso{dare, lqr, dlqr, kalman}
  92 ## @end deftypefn
  93
  94 ## Author: Lukas Reichlin <lukas.reichlin@gmail.com>
  95 ## Created: November 2009
  96 ## Version: 0.5.1
  97
  98 function [x, l, g] = care (a, b, q, r, s = [], e = [])
  99
 100   ## TODO: extract feedback matrix g from SB02OD (and SG02AD)
 101
 102   if (nargin < 4 || nargin > 6)
 103     print_usage ();
 104   endif
 105
 106   if (! is_real_square_matrix (a, q, r))
 107     ## error ("care: a, q, r must be real and square");
 108     error ("care: %s, %s, %s must be real and square", \
 109             inputname (1), inputname (3), inputname (4));
 110   endif
 111
 112   if (! is_real_matrix (b) || rows (a) != rows (b))
 113     ## error ("care: a and b must have the same number of rows");
 114     error ("care: %s and %s must have the same number of rows", \
 115             inputname (1), inputname (2));
 116   endif
 117
 118   if (columns (r) != columns (b))
 119     ## error ("care: b and r must have the same number of columns");
 120     error ("care: %s and %s must have the same number of columns", \
 121             inputname (2), inputname (4));
 122   endif
 123
 124   if (! is_real_matrix (s) && ! size_equal (s, b))
 125     ## error ("care: s(%dx%d) must be real and identically dimensioned with b(%dx%d)",
 126     ##         rows (s), columns (s), rows (b), columns (b));
 127     error ("care: %s(%dx%d) must be real and identically dimensioned with %s(%dx%d)", \
 128             inputname (5), rows (s), columns (s), inputname (2), rows (b), columns (b));
 129   endif
 130
 131   if (! isempty (e) && (! is_real_square_matrix (e) || ! size_equal (e, a)))
 132     ## error ("care: a and e must have the same number of rows");
 133     error ("care: %s and %s must have the same number of rows", \
 134             inputname (1), inputname (6));
 135   endif
 136
 137   ## check stabilizability
 138   if (! isstabilizable (a, b, e, [], 0))
 139     ## error ("care: (a, b) not stabilizable");
 140     error ("care: (%s, %s) not stabilizable", \
 141             inputname (1), inputname (2));
 142   endif
 143
 144   ## check positive semi-definiteness
 145   if (isempty (s))
 146     t = zeros (size (b));
 147   else
 148     t = s;
 149   endif
 150
 151   m = [q, t; t.', r];
 152
 153   if (isdefinite (m) < 0)
 154     ## error ("care: require [q, s; s.', r] >= 0");
 155     error ("care: require [%s, %s; %s.', %s] >= 0", \
 156             inputname (3), inputname (5), inputname (5), inputname (4));
 157   endif
 158
 159   ## solve the riccati equation
 160   if (isempty (e))
 161     if (isempty (s))
 162       [x, l] = slsb02od (a, b, q, r, b, false, false);
 163       g = r \ (b.'*x);          # gain matrix
 164     else
 165       [x, l] = slsb02od (a, b, q, r, s, false, true);
 166       g = r \ (b.'*x + s.');    # gain matrix
 167     endif
 168   else
 169     if (isempty (s))
 170       [x, l] = slsg02ad (a, e, b, q, r, b, false, false);
 171       g = r \ (b.'*x*e);        # gain matrix
 172     else
 173       [x, l] = slsg02ad (a, e, b, q, r, s, false, true);
 174       g = r \ (b.'*x*e + s.');  # gain matrix
 175     endif
 176   endif
 177
 178 endfunction
 179
 180
 181 %!shared x, l, g, xe, le, ge
 182 %! a = [-3   2
 183 %!       1   1];
 184 %!
 185 %! b = [ 0
 186 %!       1];
 187 %!
 188 %! c = [ 1  -1];
 189 %!
 190 %! r = 3;
 191 %!
 192 %! [x, l, g] = care (a, b, c.'*c, r);
 193 %!
 194 %! xe = [ 0.5895    1.8216
 195 %!        1.8216    8.8188];
 196 %!
 197 %! le = [-3.5026
 198 %!       -1.4370];
 199 %!
 200 %! ge = [ 0.6072    2.9396];
 201 %!
 202 %!assert (x, xe, 1e-4);
 203 %!assert (l, le, 1e-4);
 204 %!assert (g, ge, 1e-4);
 205
 206 %!shared x, l, g, xe, le, ge
 207 %! a = [ 0.0  1.0
 208 %!       0.0  0.0];
 209 %!
 210 %! b = [ 0.0
 211 %!       1.0];
 212 %!
 213 %! c = [ 1.0  0.0
 214 %!       0.0  1.0
 215 %!       0.0  0.0];
 216 %!
 217 %! d = [ 0.0
 218 %!       0.0
 219 %!       1.0];
 220 %!
 221 %! [x, l, g] = care (a, b, c.'*c, d.'*d);
 222 %!
 223 %! xe = [ 1.7321   1.0000
 224 %!        1.0000   1.7321];
 225 %!
 226 %! le = [-0.8660 + 0.5000i
 227 %!       -0.8660 - 0.5000i];
 228 %!
 229 %! ge = [ 1.0000   1.7321];
 230 %!
 231 %!assert (x, xe, 1e-4);
 232 %!assert (l, le, 1e-4);
 233 %!assert (g, ge, 1e-4);
 234
 235 %!shared x, xe
 236 %! a = [ 0.0  1.0
 237 %!       0.0  0.0 ];
 238 %!
 239 %! e = [ 1.0  0.0
 240 %!       0.0  1.0 ];
 241 %!
 242 %! b = [ 0.0
 243 %!       1.0 ];
 244 %!
 245 %! c = [ 1.0  0.0
 246 %!       0.0  1.0
 247 %!       0.0  0.0 ];
 248 %!
 249 %! d = [ 0.0
 250 %!       0.0
 251 %!       1.0 ];
 252 %!
 253 %! x = care (a, b, c.'*c, d.'*d, [], e);
 254 %!
 255 %! xe = [ 1.7321   1.0000
 256 %!        1.0000   1.7321 ];
 257 %!
 258 %!assert (x, xe, 1e-4);