home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / DLQE.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  1.9 KB  |  51 lines
  1. function [l,m,p,e] = dlqe(a,g,c,q,r,t)
  2. %DLQE    Discrete linear quadratic estimator design for the system:
  3. %        x[n+1] = Ax[n] + Bu[n] + Gw[n]      {State equation}
  4. %        z[n]   = Cx[n] + Du[n] +  v[n]      {Measurements}
  5. %    with process noise and measurement noise covariances:
  6. %        E{w} = E{v} = 0,  E{ww'} = Q,  E{vv'} = R,  E{wv'} = 0
  7. %    L = DLQE(A,G,C,Q,R) returns the gain matrix L such that the 
  8. %    discrete, stationary Kalman filter with time and observation 
  9. %    update equations:
  10. %       _         *               *      _                _
  11. %       x[n+1] = Ax[n] + Bu[n]    x[n] = x[n] + L(z[n] - Cx[n] - Du[n])
  12. %    produces an LQG optimal estimate of x.  The estimator can be 
  13. %    formed using DESTIM.
  14. %
  15. %    [L,M,P,E] = DLQE(A,G,C,Q,R) returns the gain matrix L, the Riccati
  16. %    equation solution M, the estimate error covariance after the 
  17. %    measurement update:          *    *
  18. %                             P = E{[x-x][x-x]'}
  19. %    and the closed-loop eigenvalues of the estimator, E=EIG(A-A*L*C).
  20. %
  21. %    [L,M,P,E] = DLQE(A,G,C,Q,R,N) solves the discrete est. problem
  22. %    when the process and sensor noise is correlated: E{wv'} = N.
  23. %
  24. %    See also: DLQEW, LQED and DESTIM.
  25.  
  26. %    J.N. Little 4-21-85
  27. %    Revised Clay M. Thompson  7-16-90
  28. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  29.  
  30. error(nargchk(5,6,nargin));
  31.  
  32. % Use DLQR to calculate estimator gains using duality
  33. if nargin==5
  34.   [k,s,e] = dlqr(a',c',g*q*g',r);
  35.   m = s';
  36.   l = (m*c')/(c*m*c' + r);
  37. else
  38.   [k,s,e] = dlqr(a',c',g*q*g',r,g*t);
  39.   m = s';
  40.   % Because of the notation used for discrete Kalman filters, the Kalman
  41.   % gain matrix shows up in the observation update equation.  When designing
  42.   % with cross terms, this requires that the A matrix be inverted when 
  43.   % computing the L matrix.
  44.   if rcond(a)<eps, disp('Warning: The A matrix must be non-singular for Kalman gain calculation.'); end
  45.   l = (m*c'+a\g*t)/(c*m*c' + r);
  46.   a = a-g*t/r*c;
  47.   q = q-t/r*t';
  48. end  
  49. p = a\(m-g*q*g')/a';
  50.  
  51.