octave_packages/control-2.3.52/dare.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}] =} dare (@var{a}, @var{b}, @var{q}, @var{r})
  20 ## @deftypefnx {Function File} {[@var{x}, @var{l}, @var{g}] =} dare (@var{a}, @var{b}, @var{q}, @var{r}, @var{s})
  21 ## @deftypefnx {Function File} {[@var{x}, @var{l}, @var{g}] =} dare (@var{a}, @var{b}, @var{q}, @var{r}, @var{[]}, @var{e})
  22 ## @deftypefnx {Function File} {[@var{x}, @var{l}, @var{g}] =} dare (@var{a}, @var{b}, @var{q}, @var{r}, @var{s}, @var{e})
  23 ## Solve discrete-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 discrete-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'XA - X - A'XB (B'XB + R)   B'XA + Q = 0
  56 ##
  57 ##                                 -1
  58 ## A'XA - X - (A'XB + S) (B'XB + R)   (B'XA + S') + Q = 0
  59 ##
  60 ##               -1
  61 ## G = (B'XB + R)   B'XA
  62 ##
  63 ##               -1
  64 ## G = (B'XB + R)   (B'XA + S')
  65 ##
  66 ## L = eig (A - B*G)
  67 ## @end group
  68 ## @end example
  69 ## @example
  70 ## @group
  71 ##                              -1
  72 ## A'XA - E'XE - A'XB (B'XB + R)   B'XA + Q = 0
  73 ##
  74 ##                                    -1
  75 ## A'XA - E'XE - (A'XB + S) (B'XB + R)   (B'XA + S') + Q = 0
  76 ##
  77 ##               -1
  78 ## G = (B'XB + R)   B'XA
  79 ##
  80 ##               -1
  81 ## G = (B'XB + R)   (B'XA + 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{care, lqr, dlqr, kalman}
  92 ## @end deftypefn
  93
  94 ## Author: Lukas Reichlin <lukas.reichlin@gmail.com>
  95 ## Created: October 2009
  96 ## Version: 0.5.1
  97
  98 function [x, l, g] = dare (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 ("dare: a, q, r must be real and square");
 108     error ("dare: %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 ("dare: a and b must have the same number of rows");
 114     error ("dare: %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 ("dare: b and r must have the same number of columns");
 120     error ("dare: %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 ("dare: s(%dx%d) must be real and identically dimensioned with b(%dx%d)",
 126     ##         rows (s), columns (s), rows (b), columns (b));
 127     error ("dare: %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 ("dare: a and e must have the same number of rows");
 133     error ("dare: %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, [], 1))
 139     ## error ("dare: (a, b) not stabilizable");
 140     error ("dare: (%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 ("dare: require [q, s; s.', r] >= 0");
 155     error ("dare: 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, true, false);
 163       g = (r + b.'*x*b) \ (b.'*x*a);        # gain matrix
 164     else
 165       [x, l] = slsb02od (a, b, q, r, s, true, true);
 166       g = (r + b.'*x*b) \ (b.'*x*a + s.');  # gain matrix
 167     endif
 168   else
 169     if (isempty (s))
 170       [x, l] = slsg02ad (a, e, b, q, r, b, true, false);
 171       g = (r + b.'*x*b) \ (b.'*x*a);        # gain matrix
 172     else
 173       [x, l] = slsg02ad (a, e, b, q, r, s, true, true);
 174       g = (r + b.'*x*b) \ (b.'*x*a + s.');  # gain matrix
 175     endif
 176   endif
 177
 178 endfunction
 179
 180
 181 %!shared x, l, g, xe, le, ge
 182 %! a = [ 0.4   1.7
 183 %!       0.9   3.8];
 184 %!
 185 %! b = [ 0.8
 186 %!       2.1];
 187 %!
 188 %! c = [ 1  -1];
 189 %!
 190 %! r = 3;
 191 %!
 192 %! [x, l, g] = dare (a, b, c.'*c, r);
 193 %!
 194 %! xe = [ 1.5354    1.2623
 195 %!        1.2623   10.5596];
 196 %!
 197 %! le = [-0.0022
 198 %!        0.2454];
 199 %!
 200 %! ge = [ 0.4092    1.7283];
 201 %!
 202 %!assert (x, xe, 1e-4);
 203 %!assert (sort (l), sort (le), 1e-4);
 204 %!assert (g, ge, 1e-4);
 205
 206 ## TODO: add more tests (nonempty s and/or e)