octave_packages/control-2.3.52/kalman.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{est}, @var{g}, @var{x}] =} kalman (@var{sys}, @var{q}, @var{r})
  20 ## @deftypefnx {Function File} {[@var{est}, @var{g}, @var{x}] =} kalman (@var{sys}, @var{q}, @var{r}, @var{s})
  21 ## @deftypefnx {Function File} {[@var{est}, @var{g}, @var{x}] =} kalman (@var{sys}, @var{q}, @var{r}, @var{[]}, @var{sensors}, @var{known})
  22 ## @deftypefnx {Function File} {[@var{est}, @var{g}, @var{x}] =} kalman (@var{sys}, @var{q}, @var{r}, @var{s}, @var{sensors}, @var{known})
  23 ## Design Kalman estimator for LTI systems.
  24 ##
  25 ## @strong{Inputs}
  26 ## @table @var
  27 ## @item sys
  28 ## Nominal plant model.
  29 ## @item q
  30 ## Covariance of white process noise.
  31 ## @item r
  32 ## Covariance of white measurement noise.
  33 ## @item s
  34 ## Optional cross term covariance.  Default value is 0.
  35 ## @item sensors
  36 ## Indices of measured output signals y from @var{sys}.  If omitted, all outputs are measured.
  37 ## @item known
  38 ## Indices of known input signals u (deterministic) to @var{sys}.  All other inputs to @var{sys}
  39 ## are assumed stochastic.  If argument @var{known} is omitted, no inputs u are known.
  40 ## @end table
  41 ##
  42 ## @strong{Outputs}
  43 ## @table @var
  44 ## @item est
  45 ## State-space model of the Kalman estimator.
  46 ## @item g
  47 ## Estimator gain.
  48 ## @item x
  49 ## Solution of the Riccati equation.
  50 ## @end table
  51 ##
  52 ## @strong{Block Diagram}
  53 ## @example
  54 ## @group
  55 ##                                  u  +-------+         ^
  56 ##       +---------------------------->|       |-------> y
  57 ##       |    +-------+     +       y  |  est  |         ^
  58 ## u ----+--->|       |----->(+)------>|       |-------> x
  59 ##            |  sys  |       ^ +      +-------+
  60 ## w -------->|       |       |
  61 ##            +-------+       | v
  62 ##
  63 ## Q = cov (w, w')     R = cov (v, v')     S = cov (w, v')
  64 ## @end group
  65 ## @end example
  66 ##
  67 ## @seealso{care, dare, estim, lqr}
  68 ## @end deftypefn
  69
  70 ## Author: Lukas Reichlin <lukas.reichlin@gmail.com>
  71 ## Created: November 2009
  72 ## Version: 0.3
  73
  74 function [est, k, x] = kalman (sys, q, r, s = [], sensors = [], deterministic = [])
  75
  76   ## TODO: type "current" for discrete-time systems
  77
  78   if (nargin < 3 || nargin > 6 || ! isa (sys, "lti"))
  79     print_usage ();
  80   endif
  81
  82   [a, b, c, d, e] = dssdata (sys, []);
  83
  84   if (isempty (sensors))
  85     sensors = 1 : rows (c);
  86   endif
  87
  88   stochastic = setdiff (1 : columns (b), deterministic);
  89
  90   c = c(sensors, :);
  91   g = b(:, stochastic);
  92   h = d(sensors, stochastic);
  93
  94   if (isempty (s))
  95     rbar = r + h*q*h.';
  96     sbar = g * q*h.';
  97   else
  98     rbar = r + h*q*h.'+ h*s + s.'*h.';
  99     sbar = g * (q*h.' + s);
 100   endif
 101
 102   if (isct (sys))
 103     [x, l, k] = care (a.', c.', g*q*g.', rbar, sbar, e.');
 104   else
 105     [x, l, k] = dare (a.', c.', g*q*g.', rbar, sbar, e.');
 106   endif
 107
 108   k = k.';
 109
 110   est = estim (sys, k, sensors, deterministic);
 111
 112 endfunction
 113
 114
 115 %!shared m, m_exp, g, g_exp, x, x_exp
 116 %! sys = ss (-2, 1, 1, 3);
 117 %! [est, g, x] = kalman (sys, 1, 1, 1);
 118 %! [a, b, c, d] = ssdata (est);
 119 %! m = [a, b; c, d];
 120 %! m_exp = [-2.25, 0.25; 1, 0; 1, 0];
 121 %! g_exp = 0.25;
 122 %! x_exp = 0;
 123 %!assert (m, m_exp, 1e-2);
 124 %!assert (g, g_exp, 1e-2);
 125 %!assert (x, x_exp, 1e-2);