home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / lqe < prev    next >
Encoding:
Text File  |  1995-11-15  |  2.1 KB  |  74 lines
  1. //----------------------------------------------------------------------------
  2. //
  3. // lqe
  4. //
  5. // Syntax: G=lqe(A,D,M,Q,R,CT)
  6. //
  7. // This routine computes the Linear Quadratic Estimator Design for continuous
  8. // systems. The system is given by the following differential equations:
  9. //      .
  10. //      x = Ax + Bu + Dw
  11. //      z = Mx + Gu + v
  12. //
  13. // where z is the vector of sensor measurements, w is the vector if process
  14. // noise, and v is the vector of sensor noise. The system has the following
  15. // process noise and measurement noise covariances:
  16. //
  17. //           E(w) = E(v) = 0
  18. //           E(ww') = Q
  19. //           E(vv') = R
  20. //           E(wv') = 0
  21. //
  22. // where E is the expectation operator. The gain matrix L is designed such
  23. // that the stationary Kalman filter:
  24. //     .
  25. //     x = Ax + Bu + L(z - Mx - Gu)
  26. //
  27. // produces an LQG optimal estimate of x. If CT exists then the process and
  28. // sensor noise are correlated: E(wv') = CT.
  29. //
  30. // This routine takes advantage of the dual nature of the optimal estimator,
  31. // by using the lqr routine to compute the optimal estimator.
  32. //
  33. // Note: Three matrices are returned in a list.
  34. //
  35. //       G.l = Optimal Estimator Gain matrix
  36. //       G.p = Riccatti Eqn. Solution, which is the estimate error covariance
  37. //       G.e = Closed Loop eigenvalues of the estimator  eig(A-L*M)
  38. //
  39. // Ref: (1) Skelton, R. "Dynamic Systems Control Linear System Analysis and
  40. //          Synthesis," John Wiley and Sons, 1988.
  41. //
  42. // Copyright (C), by Jeffrey B. Layton, 1994
  43. // Version JBL 940918
  44. //----------------------------------------------------------------------------
  45.  
  46. rfile lqr
  47.  
  48. lqe = function(a,d,m,q,r,ct)
  49. {
  50.    local(nargs,d)
  51.  
  52. // Count number of input arguments
  53.    nargs=0;
  54.    if (exist(a)) {nargs=nargs+1;}
  55.    if (exist(d)) {nargs=nargs+1;}
  56.    if (exist(m)) {nargs=nargs+1;}
  57.    if (exist(q)) {nargs=nargs+1;}
  58.    if (exist(r)) {nargs=nargs+1;}
  59.    if (exist(ct)) {nargs=nargs+1;}
  60.  
  61.    if ( nargs < 5) {
  62.        error("LQE: Wrong number of input arguments.");
  63.    }
  64.  
  65. // Don't do any error checking, let lqr take care of that.
  66.    if ( exist(ct)) {
  67.         d=lqr(a',m',d*q*d',r,d*ct);
  68.    else
  69.         d=lqr(a',m',d*q*d',r);
  70.    }
  71.     
  72.    return << l=d.k'; p=d.p'; e=d.e >>
  73. };
  74.