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