home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / DESTIM.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  2.0 KB  |  62 lines
  1. function [ae,be,ce,de] = destim(a,b,c,d,l,e,f)
  2. %DESTIM Form discrete Kalman estimator.
  3. %    [Ae,Be,Ce,De] = DESTIM(A,B,C,D,L) produces the Kalman estimator 
  4. %    based on the discrete system (A,B,C,D) with Kalman gain matrix L 
  5. %    assuming all the outputs of the system are sensor outputs.  The 
  6. %    resulting state-space estimator is
  7. %
  8. %        xBar[n+1] = [A-ALC] xBar[n] + [AL] y[n]
  9. %
  10. %         |yHat|   = |C-CLC| xBar[n] + |CL| y[n]
  11. %         |xHat|     |I-LC |           |L |
  12. %
  13. %    and has estimated sensors yHat and estimated states xHat as 
  14. %    outputs, and sensors y as inputs.  
  15. %
  16. %    [Ae,Be,Ce,De] = DESTIM(A,B,C,D,L,SENSORS,KNOWN) forms the Kalman 
  17. %    estimator using the sensors specified by SENSORS, and the 
  18. %    additional known inputs specified by KNOWN.  The resulting system
  19. %    has estimated sensors and states as outputs, and the known inputs
  20. %    and sensors as inputs.  The KNOWN inputs are non-stochastic inputs
  21. %    of the plant and are usually control inputs.
  22. %
  23. %     See also: REG,DCONTROL,ESTIM,LQR,DLQR,LQE and DLQE.
  24.  
  25. %    Clay M. Thompson 7-2-90
  26. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  27.  
  28. error(nargchk(5,7,nargin));
  29. if ~((nargin==5)|(nargin==7)), error('Wrong number of input arguments.'); end
  30.  
  31. error(abcdchk(a,b,c,d));
  32.  
  33. [nx,na] = size(a);
  34. [ny,nu] = size(d);
  35.  
  36. if (nargin==5)
  37.   sensors = [1:ny]; known = [];
  38. end
  39. if (nargin==7)
  40.   sensors = e; known = f;
  41. end
  42.  
  43. nsens = length(sensors); nknown = length(known);
  44.  
  45. % Check size of L with number of states and sensors
  46. [nl,ml] = size(l);
  47. if (ml~=nsens)
  48.   error('Number of sensors and size of L matrix don''t match.'); end
  49. if (nl~=nx)
  50.   error('The A and L matrices must have the same number of rows.'); end
  51.  
  52. inputs = [1:nsens] + nu; outputs = sensors + ny; states = [1:nx] + 2*ny;
  53.  
  54. % --- Form continuous Kalman estimator ---
  55. ae = a;
  56. be = [b,a*l];
  57. ce = [c;c;eye(nx)];
  58. de = [d, zeros(ny,nsens);d,c*l;zeros(nx,nu), l];
  59. % close sensor feedback loop
  60. [ae,be,ce,de] = cloop(ae,be,ce,de,sensors,-inputs);
  61. [ae,be,ce,de] = ssselect(ae,be,ce,de,[known,inputs],[outputs,states]);
  62.