home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / LQED.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  2.3 KB  |  69 lines
  1. function [l,m,p,e] = lqed(a,g,c,q,r,Ts)
  2. %LQED   Discrete linear quadratic estimator design from continuous 
  3. %       cost function.
  4. %    [L,M,P,E] = LQED(A,G,C,Q,R,Ts) calculates the Kalman gain matrix L
  5. %    that minimizes the discrete estimation error equivalent to the 
  6. %    estimation error from the continuous system:
  7. %             .
  8. %             x = Ax + Bu + Gw
  9. %                y = Cx + Du +  v
  10. %
  11. %    with process and measurement noise:
  12. %        E{w} = E{v} = 0,  E{ww'} = Q,  E{vv'} = R,  E{wv'} = 0
  13. %
  14. %    Also returned is the discrete Riccati solution M, the estimate
  15. %    error covariance after the measurement update P, and the discrete
  16. %    closed loop loop eigenvalues E = EIG(Ad-Ad*L*Cd).
  17. %
  18. %    The gain matrix is determined by discretizing the continuous plant
  19. %    (A,B,C,D) and continuous covariance matrices (Q,R) using the 
  20. %    sample time Ts and the zero order hold approximation. The gain 
  21. %    matrix is then calculated using DLQE.
  22. %
  23. %    See also: C2D, LQRD, DLQE, and LQE.
  24.  
  25. %    Clay M. Thompson 7-18-90
  26. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  27.  
  28. % Reference: This routine is based on the routine DISRW.M by Franklin, 
  29. % Powell and Workman and is described on pp. 454-455 of "Digital Control
  30. % of Dynamic Systems".
  31.  
  32. error(nargchk(6,7,nargin));
  33. error(abcdchk(a,g,c));
  34. [nx,nu] = size(g);
  35. [ny,nx] = size(c);
  36.  
  37. [nq,mq] = size(q);
  38. if (mq ~= nq) | (nu ~= mq), error('G and Q must be consistent.'); end
  39. [nr,mr] = size(r);
  40. if (mr ~= nr) | (ny ~= mr), error('C and R must be consistent.'); end
  41.  
  42. % Check if q is positive semi-definite and symmetric
  43. if any(eig(q) < -eps) | (norm(q'-q,1)/norm(q,1) > eps)
  44.   disp('Warning: Q is not symmetric and positive semi-definite');
  45. end
  46. % Check if r is positive definite and symmetric
  47. if any(eig(r) <= -eps) | (norm(r'-r,1)/norm(r,1) > eps)
  48.   disp('Warning: R is not symmetric and positive definite');
  49. end
  50.  
  51. % Discretize the state-space system.
  52. [ad,gd] = c2d(a,g,Ts);
  53.  
  54. % --- Compute discrete equivalent of continuous noise ---
  55. Za = zeros(nx); 
  56. M = [ -a  g*q*g'
  57.       Za   a'   ];
  58. phi = expm(M*Ts);
  59. phi12 = phi(1:nx,nx+1:2*nx);
  60. phi22 = phi(nx+1:2*nx,nx+1:2*nx);
  61. Qd = phi22'*phi12;
  62. Qd = (Qd+Qd')/2;        % Make sure Qd is symmetric
  63.  
  64. Rd = r/Ts;
  65.  
  66. % Design the gain matrix using the discrete plant and discrete cost function
  67. [l,m,p,e] = dlqe(ad,eye(nx),c,Qd,Rd);
  68.  
  69.