diff scripts/control/base/dlqe.m @ 3431:99ab64f4a09d

[project @ 2000-01-14 03:53:03 by jwe]
author jwe
date Fri, 14 Jan 2000 04:12:41 +0000
parents
children 9debe1be75a5
line wrap: on
line diff
new file mode 100644
--- /dev/null
+++ b/scripts/control/base/dlqe.m
@@ -0,0 +1,125 @@
+## Copyright (C) 1993, 1994, 1995 Auburn University.  All rights reserved.
+##
+## 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{l}, @var{m}, @var{p}, @var{e}] =} dlqe (@var{a}, @var{g}, @var{c}, @var{sigw}, @var{sigv}, @var{z})
+## Construct the linear quadratic estimator (Kalman filter) 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 + w_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] + w[k]
+## @end example
+##
+## @end ifinfo
+## where @var{w}, @var{v} are zero-mean gaussian noise processes with
+## respective intensities @code{@var{sigw} = cov (@var{w}, @var{w})} and
+## @code{@var{sigv} = cov (@var{v}, @var{v})}.
+##
+## If specified, @var{z} is @code{cov (@var{w}, @var{v})}.  Otherwise
+## @code{cov (@var{w}, @var{v}) = 0}.
+##
+## The observer structure is
+## @iftex
+## @tex
+## $$
+##  z_{k+1} = A z_k + B u_k + k (y_k - C z_k - D u_k)
+## $$
+## @end tex
+## @end iftex
+## @ifinfo
+##
+## @example
+## z[k+1] = A z[k] + B u[k] + k (y[k] - C z[k] - D u[k])
+## @end example
+## @end ifinfo
+##
+## @noindent
+## The following values are returned:
+##
+## @table @var
+## @item l
+## The observer gain,
+## @iftex
+## @tex
+## $(A - ALC)$.
+## @end tex
+## @end iftex
+## @ifinfo
+## (@var{a} - @var{a}@var{l}@var{c}).
+## @end ifinfo
+## is stable.
+##
+## @item m
+## The Riccati equation solution.
+##
+## @item p
+## The estimate error covariance after the measurement update.
+##
+## @item e
+## The closed loop poles of
+## @iftex
+## @tex
+## $(A - ALC)$.
+## @end tex
+## @end iftex
+## @ifinfo
+## (@var{a} - @var{a}@var{l}@var{c}).
+## @end ifinfo
+## @end table
+## @end deftypefn
+
+## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu>
+## Created: August 1993
+## Modified for discrete time by R. Bruce Tenison (btenison@eng.auburn.edu)
+## October, 1993
+
+function [l, m, p, e] = dlqe (a, g, c, sigw, sigv, s)
+
+  if (nargin != 5 && nargin != 6)
+    error ("dlqe: invalid number of arguments");
+  endif
+
+  ## 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';
+  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;
+  endif
+
+  p = a\(m-g*sigw*g')/a';
+
+endfunction