Mercurial > hg > octave-avbm
changeset 3696:5a7174ebc684
[project @ 2000-07-17 19:38:53 by jwe]
author | jwe |
---|---|
date | Mon, 17 Jul 2000 19:39:47 +0000 |
parents | 64ca92e02a7e |
children | 8ce0d75eb4e3 |
files | scripts/ChangeLog scripts/control/base/dkalman.m scripts/control/base/dlqe.m scripts/control/base/dlqr.m |
diffstat | 4 files changed, 222 insertions(+), 31 deletions(-) [+] |
line wrap: on
line diff
--- a/scripts/ChangeLog +++ b/scripts/ChangeLog @@ -1,3 +1,9 @@ +2000-07-17 Gabriele Pannocchia <pannocchia@ing.unipi.it> + + * control/base/dkalman.m: New file. + * control/base/dlqe.m: Handle singular A matrix. + * control/base/dlqr.m: Likewise. + 2000-07-14 John W. Eaton <jwe@bevo.che.wisc.edu> * strings/strcmp.m: Return 0 instead of an error if row and column
new file mode 100644 --- /dev/null +++ b/scripts/control/base/dkalman.m @@ -0,0 +1,187 @@ +## Copyright (C) 2000 Gabriele Pannocchia +## +## 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 USA. + +## -*- texinfo -*- +## @deftypefn {Function File} {[@var{Lp}, @var{Lf}, @var{P}, @var{Z}] =} dkalman (@var{A}, @var{G}, @var{C}, @var{Qw}, @var{Rv}, @var{S}) +## Construct the linear quadratic estimator (Kalman predictor) for the +## discrete time system +## @iftex +## @tex +## $$ +## x_{k+1} = A x_k + B u_k + G w_k +## $$ +## $$ +## y_k = C x_k + D u_k + v_k +## $$ +## @end tex +## @end iftex +## @ifinfo +## +## @example +## x[k+1] = A x[k] + B u[k] + G w[k] +## y[k] = C x[k] + D u[k] + v[k] +## @end example +## +## @end ifinfo +## where @var{w}, @var{v} are zero-mean gaussian noise processes with +## respective intensities @code{@var{Qw} = cov (@var{w}, @var{w})} and +## @code{@var{Rv} = cov (@var{v}, @var{v})}. +## +## If specified, @var{S} is @code{cov (@var{w}, @var{v})}. Otherwise +## @code{cov (@var{w}, @var{v}) = 0}. +## +## The observer structure is +## @iftex +## @tex +## $x_{k+1|k} = A x_{k|k-1} + B u_k + L_p (y_k - C x_{k|k-1} - D u_k)$ +## $x_{k|k} = x_{k|k} + L_f (y_k - C x_{k|k-1} - D u_k)$ +## @end tex +## @end iftex +## @ifinfo +## +## @example +## x[k+1|k] = A x[k|k-1] + B u[k] + LP (y[k] - C x[k|k-1] - D u[k]) +## x[k|k] = x[k|k-1] + LF (y[k] - C x[k|k-1] - D u[k]) +## @end example +## @end ifinfo +## +## @noindent +## The following values are returned: +## +## @table @var +## @item Lp +## The predictor gain, +## @iftex +## @tex +## $(A - L_p C)$. +## @end tex +## @end iftex +## @ifinfo +## (@var{A} - @var{Lp} @var{C}) +## @end ifinfo +## is stable. +## +## @item Lf +## The filter gain. +## +## @item P +## The Riccati solution. +## @iftex +## @tex +## $P = E \{(x - x_{n|n-1})(x - x_{n|n-1})'\}$ +## @end tex +## @end iftex +## +## @ifinfo +## P = E [(x - x[n|n-1])(x - x[n|n-1])'] +## @end ifinfo +## +## @item Z +## The updated error covariance matrix. +## @iftex +## @tex +## $Z = E \{(x - x_{n|n})(x - x_{n|n})'\}$ +## @end tex +## @end iftex +## +## @ifinfo +## Z = E [(x - x[n|n])(x - x[n|n])'] +## @end ifinfo +## @end table +## @end deftypefn + +## Author: Gabriele Pannocchia <pannocchia@ing.unipi.it> +## Created: July 2000 + +function [Lp, Lf, P, Z] = dkalman (A, G, C, Qw, Rv, S) + + if (nargin != 5 && nargin != 6) + error ("dkalman: invalid number of arguments"); + endif + + ## Check A. + if ((n = is_square (A)) == 0) + error ("dkalman: requires 1st parameter(A) to be square"); + endif + + ## Check C. + [p, n1] = size (C); + if (n1 != n) + error ("dkalman: A,C not conformal"); + endif + + ## Check G. + [n1, nw] = size (G); + if (n1 != n) + error ("dkalman: A,G not conformal"); + endif + + ## Check Qw. + if ((nw1 = is_square (Qw)) == 0) + error ("dkalman: requires 4rd parameter(Qw) to be square"); + else + if (nw1 != nw) + error ("dkalman: G,Qw not conformal"); + endif + endif + + ## Check Rv. + if ((p1 = is_square (Rv)) == 0) + error ("dkalman: requires 5rd parameter(Rv) to be square"); + else + if (p1 != p) + error ("dkalman: C,Rv not conformal"); + endif + endif + + ## Check S if it is there + if (nargin == 6) + [nw1, p1] = size (S); + if (nw1 != nw || p1 != p) + error ("dkalman: S not conformal with Qw and Rv"); + else + Cov_aug = [Qw, S; S', Rv]; + if (! all (eig (Cov_aug) > 0)) + error ("dkalman: augmented noise covariance matrix must be positive definite"); + endif + endif + else + if (! all (eig (Qw) > 0) || ! all (eig (Rv) > 0)) + error ("dkalman: covariance matrices Qw,Rv must be positive definite"); + endif + S = zeros (nw, p); + endif + + ## Incorporate the cross term into A and Qw + As = A - G*S/Rv*C; + Qs = Qw - S/Rv*S'; + + ## Call dare to solve the Riccati eqn. + a = As'; + b = C'; + c = G*Qs*G'; + r = Rv; + p = dare (a, b, c, r); + + ## Output + Lp = (A*p*C'+G*S)/(Rv+C*p*C'); + Lf = (p*C')/(Rv+C*p*C'); + P = p; + Z = p - Lf*C*p; + +endfunction
--- a/scripts/control/base/dlqe.m +++ b/scripts/control/base/dlqe.m @@ -1,4 +1,4 @@ -## Copyright (C) 1993, 1994, 1995, 2000 Auburn University. All rights reserved. +## Copyright (C) 1993, 1994, 1995 Auburn University ## ## This file is part of Octave. ## @@ -26,7 +26,7 @@ ## x_{k+1} = A x_k + B u_k + G w_k ## $$ ## $$ -## y_k = C x_k + D u_k + w_k +## y_k = C x_k + D u_k + v_k ## $$ ## @end tex ## @end iftex @@ -34,7 +34,7 @@ ## ## @example ## x[k+1] = A x[k] + B u[k] + G w[k] -## y[k] = C x[k] + D u[k] + w[k] +## y[k] = C x[k] + D u[k] + v[k] ## @end example ## ## @end ifinfo @@ -49,14 +49,18 @@ ## @iftex ## @tex ## $$ -## z_{k+1} = A z_k + B u_k + L (y_k - C z_k - D u_k) +## z_{k|k} = z_{k|k-1} + l (y_k - C z_{k|k-1} - D u_k) +## $$ +## $$ +## z_{k+1|k} = A z_{k|k} + B u_k ## $$ ## @end tex ## @end iftex ## @ifinfo ## ## @example -## z[k+1] = A z[k] + B u[k] + L (y[k] - C z[k] - D u[k]) +## z[k|k] = z[k|k-1] + L (y[k] - C z[k|k-1] - D u[k]) +## z[k+1|k] = A z[k|k] + B u[k] ## @end example ## @end ifinfo ## @@ -65,17 +69,16 @@ ## ## @table @var ## @item l -## The observer gain. The estimator state matrix +## The observer gain, ## @iftex ## @tex -## $(A - LC)$ +## $(A - ALC)$. ## @end tex ## @end iftex ## @ifinfo -## (@var{a} - @var{l}@var{c}) +## (@var{a} - @var{a}@var{l}@var{c}). ## @end ifinfo -## is stable. NOTE: This differs from the MATLAB dlqe function, which -## returns L such that (A - A L C) is stable. +## is stable. ## ## @item m ## The Riccati equation solution. @@ -87,11 +90,11 @@ ## The closed loop poles of ## @iftex ## @tex -## $(A - LC)$. +## $(A - ALC)$. ## @end tex ## @end iftex ## @ifinfo -## (@var{a} - @var{l}@var{c}). +## (@var{a} - @var{a}@var{l}@var{c}). ## @end ifinfo ## @end table ## @end deftypefn @@ -100,6 +103,8 @@ ## Created: August 1993 ## Modified for discrete time by R. Bruce Tenison (btenison@eng.auburn.edu) ## October, 1993 +## Modified by Gabriele Pannocchia <pannocchia@ing.unipi.it> +## July 2000 function [l, m, p, e] = dlqe (a, g, c, sigw, sigv, s) @@ -110,17 +115,14 @@ ## The problem is dual to the regulator design, so transform to dlqr call. if (nargin == 5) - [k, p, e] = dlqr (a', c', g*sigw*g', sigv); - m = p; - l = k'; + [k, m, e] = dlqr (a', c', g*sigw*g', sigv); else - [k, p, e] = dlqr (a', c', g*sigw*g', sigv, g*s); - m = p; - l = k'; - a = a-g*t/sigv*c; - sigw = sigw-t/sigv; + [k, m, e] = dlqr (a', c', g*sigw*g', sigv, g*s); + warning ("dlqe: use dkalman when there is a cross-covariance term"); endif - p = a\(m-g*sigw*g')/a'; + l = m*c'/(c*m*c'+sigv); + p = m - m*c'/(c*m*c'+sigv)*c*m; endfunction +
--- a/scripts/control/base/dlqr.m +++ b/scripts/control/base/dlqr.m @@ -1,4 +1,4 @@ -## Copyright (C) 1993, 1994, 1995 Auburn University. All rights reserved. +## Copyright (C) 1993, 1994, 1995 Auburn University ## ## This file is part of Octave. ## @@ -95,19 +95,14 @@ ## (@var{a} - @var{b}@var{k}). ## @end ifinfo ## @end table -## @strong{References} -## @enumerate -## @item Anderson and Moore, Optimal Control: Linear Quadratic Methods, -## Prentice-Hall, 1990, pp. 56-58 -## @item Kuo, Digital Control Systems, Harcourt Brace Jovanovich, 1992, -## section 11-5-2. -## @end enumerate ## @end deftypefn ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> ## Created: August 1993 ## Converted to discrete time by R. B. Tenison ## (btenison@eng.auburn.edu) October 1993 +## Modified by Gabriele Pannocchia <pannocchia@ing.unipi.it> +## July 2000 function [k, p, e] = dlqr (a, b, q, r, s) @@ -154,13 +149,14 @@ endif ## Check that q, (r) are symmetric, positive (semi)definite - if (is_symmetric (q) && is_symmetric (r) ... + 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*a + r\s'; + k = (r+b'*p*b)\(b'*p*a + s'); e = eig (a - b*k); else error ("dlqr: q (r) must be symmetric positive (semi) definite"); endif endfunction +