|
904
|
1 # Copyright (C) 1993, 1994 John W. Eaton |
|
245
|
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, 675 Mass Ave, Cambridge, MA 02139, USA. |
|
|
18 |
|
201
|
19 function [l, m, p, e] = dlqe (a, g, c, sigw, sigv, zz) |
|
|
20 |
|
|
21 # Usage: [l, m, p, e] = dlqe (A, G, C, SigW, SigV {,Z}) |
|
|
22 # |
|
|
23 # Linear quadratic estimator (Kalman filter) design for the |
|
|
24 # discrete time system |
|
|
25 # |
|
|
26 # x[k+1] = A x[k] + B u[k] + G w[k] |
|
|
27 # y[k] = C x[k] + D u[k] + w[k] |
|
|
28 # |
|
|
29 # where w, v are zero-mean gaussian noise processes with respective |
|
|
30 # intensities SigW = cov (w, w) and SigV = cov (v, v). |
|
|
31 # |
|
|
32 # Z (if specified) is cov(w,v); otherwise cov(w,v) = 0. |
|
|
33 # |
|
|
34 # Observer structure is |
|
|
35 # z[k+1] = A z[k] + B u[k] + k(y[k] - C z[k] - D u[k]). |
|
|
36 # |
|
|
37 # Returns: |
|
|
38 # |
|
|
39 # l = observer gain, (A - A L C) is stable |
|
|
40 # m = Ricatti equation solution |
|
|
41 # p = the estimate error covariance after the measurement update |
|
|
42 # e = closed loop poles of (A - A L C) |
|
|
43 |
|
|
44 # Written by A. S. Hodel (scotte@eng.auburn.edu) August, 1993. |
|
|
45 # Modified for discrete time by R. Bruce Tenison (btenison@eng.auburn.edu) |
|
|
46 # October, 1993 |
|
|
47 |
|
|
48 if (nargin != 5 && nargin != 6) |
|
904
|
49 error ("dlqe: invalid number of arguments"); |
|
201
|
50 endif |
|
|
51 |
|
|
52 # The problem is dual to the regulator design, so transform to lqr |
|
|
53 # call. |
|
|
54 |
|
|
55 if (nargin == 5) |
|
|
56 [k, p, e] = dlqr (a', c', g*sigw*g', sigv); |
|
|
57 m = p'; |
|
|
58 l = (m*c')/(c*m*c'+sigv); |
|
|
59 else |
|
|
60 [k, p, e] = dlqr (a', c', g*sigw*g', sigv, g*zz); |
|
|
61 m = p'; |
|
1002
|
62 l = (m*c'+a\g)/(c*m*c'+sigv); |
|
201
|
63 a = a-g*t/sigv*c; |
|
1002
|
64 sigw = sigw-t/sigv; |
|
201
|
65 endif |
|
|
66 |
|
|
67 p = a\(m-g*sigw*g')/a'; |
|
|
68 |
|
|
69 endfunction |
