|
3696
|
1 ## Copyright (C) 1993, 1994, 1995 Auburn University |
|
3431
|
2 ## |
|
|
3 ## This file is part of Octave. |
|
|
4 ## |
|
|
5 ## Octave is free software; you can redistribute it and/or modify it |
|
|
6 ## under the terms of the GNU General Public License as published by the |
|
|
7 ## Free Software Foundation; either version 2, or (at your option) any |
|
|
8 ## later version. |
|
|
9 ## |
|
|
10 ## Octave is distributed in the hope that it will be useful, but WITHOUT |
|
|
11 ## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
|
|
12 ## FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License |
|
|
13 ## for more details. |
|
|
14 ## |
|
|
15 ## You should have received a copy of the GNU General Public License |
|
|
16 ## along with Octave; see the file COPYING. If not, write to the Free |
|
|
17 ## Software Foundation, 59 Temple Place, Suite 330, Boston, MA 02111 USA. |
|
|
18 |
|
|
19 ## -*- texinfo -*- |
|
|
20 ## @deftypefn {Function File} {[@var{l}, @var{m}, @var{p}, @var{e}] =} dlqe (@var{a}, @var{g}, @var{c}, @var{sigw}, @var{sigv}, @var{z}) |
|
|
21 ## Construct the linear quadratic estimator (Kalman filter) for the |
|
|
22 ## discrete time system |
|
|
23 ## @iftex |
|
|
24 ## @tex |
|
|
25 ## $$ |
|
|
26 ## x_{k+1} = A x_k + B u_k + G w_k |
|
|
27 ## $$ |
|
|
28 ## $$ |
|
3696
|
29 ## y_k = C x_k + D u_k + v_k |
|
3431
|
30 ## $$ |
|
|
31 ## @end tex |
|
|
32 ## @end iftex |
|
|
33 ## @ifinfo |
|
|
34 ## |
|
|
35 ## @example |
|
|
36 ## x[k+1] = A x[k] + B u[k] + G w[k] |
|
3696
|
37 ## y[k] = C x[k] + D u[k] + v[k] |
|
3431
|
38 ## @end example |
|
|
39 ## |
|
|
40 ## @end ifinfo |
|
|
41 ## where @var{w}, @var{v} are zero-mean gaussian noise processes with |
|
|
42 ## respective intensities @code{@var{sigw} = cov (@var{w}, @var{w})} and |
|
|
43 ## @code{@var{sigv} = cov (@var{v}, @var{v})}. |
|
|
44 ## |
|
|
45 ## If specified, @var{z} is @code{cov (@var{w}, @var{v})}. Otherwise |
|
|
46 ## @code{cov (@var{w}, @var{v}) = 0}. |
|
|
47 ## |
|
|
48 ## The observer structure is |
|
|
49 ## @iftex |
|
|
50 ## @tex |
|
|
51 ## $$ |
|
3696
|
52 ## z_{k|k} = z_{k|k-1} + l (y_k - C z_{k|k-1} - D u_k) |
|
|
53 ## $$ |
|
|
54 ## $$ |
|
|
55 ## z_{k+1|k} = A z_{k|k} + B u_k |
|
3431
|
56 ## $$ |
|
|
57 ## @end tex |
|
|
58 ## @end iftex |
|
|
59 ## @ifinfo |
|
|
60 ## |
|
|
61 ## @example |
|
3696
|
62 ## z[k|k] = z[k|k-1] + L (y[k] - C z[k|k-1] - D u[k]) |
|
|
63 ## z[k+1|k] = A z[k|k] + B u[k] |
|
3431
|
64 ## @end example |
|
|
65 ## @end ifinfo |
|
|
66 ## |
|
|
67 ## @noindent |
|
|
68 ## The following values are returned: |
|
|
69 ## |
|
|
70 ## @table @var |
|
|
71 ## @item l |
|
3696
|
72 ## The observer gain, |
|
3431
|
73 ## @iftex |
|
|
74 ## @tex |
|
3696
|
75 ## $(A - ALC)$. |
|
3431
|
76 ## @end tex |
|
|
77 ## @end iftex |
|
|
78 ## @ifinfo |
|
3696
|
79 ## (@var{a} - @var{a}@var{l}@var{c}). |
|
3431
|
80 ## @end ifinfo |
|
3696
|
81 ## is stable. |
|
3431
|
82 ## |
|
|
83 ## @item m |
|
|
84 ## The Riccati equation solution. |
|
|
85 ## |
|
|
86 ## @item p |
|
|
87 ## The estimate error covariance after the measurement update. |
|
|
88 ## |
|
|
89 ## @item e |
|
|
90 ## The closed loop poles of |
|
|
91 ## @iftex |
|
|
92 ## @tex |
|
3696
|
93 ## $(A - ALC)$. |
|
3431
|
94 ## @end tex |
|
|
95 ## @end iftex |
|
|
96 ## @ifinfo |
|
3696
|
97 ## (@var{a} - @var{a}@var{l}@var{c}). |
|
3431
|
98 ## @end ifinfo |
|
|
99 ## @end table |
|
|
100 ## @end deftypefn |
|
|
101 |
|
|
102 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
103 ## Created: August 1993 |
|
|
104 ## Modified for discrete time by R. Bruce Tenison (btenison@eng.auburn.edu) |
|
|
105 ## October, 1993 |
|
3696
|
106 ## Modified by Gabriele Pannocchia <pannocchia@ing.unipi.it> |
|
|
107 ## July 2000 |
|
3431
|
108 |
|
|
109 function [l, m, p, e] = dlqe (a, g, c, sigw, sigv, s) |
|
|
110 |
|
|
111 if (nargin != 5 && nargin != 6) |
|
|
112 error ("dlqe: invalid number of arguments"); |
|
|
113 endif |
|
|
114 |
|
|
115 ## The problem is dual to the regulator design, so transform to dlqr call. |
|
|
116 |
|
|
117 if (nargin == 5) |
|
3696
|
118 [k, m, e] = dlqr (a', c', g*sigw*g', sigv); |
|
3431
|
119 else |
|
3696
|
120 [k, m, e] = dlqr (a', c', g*sigw*g', sigv, g*s); |
|
|
121 warning ("dlqe: use dkalman when there is a cross-covariance term"); |
|
3431
|
122 endif |
|
|
123 |
|
3696
|
124 l = m*c'/(c*m*c'+sigv); |
|
|
125 p = m - m*c'/(c*m*c'+sigv)*c*m; |
|
3431
|
126 |
|
|
127 endfunction |
|
3696
|
128 |
