home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / lqemsf < prev    next >
Encoding:
Text File  |  1995-11-15  |  2.7 KB  |  95 lines
  1. //----------------------------------------------------------------------------
  2. //
  3. // lqe
  4. //
  5. // Syntax: G=lqemsf(A,D,M,Q,R,CT,Reps,iterm)
  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 lqrmsf routine to compute the optimal estimator.
  32. //
  33. // The routine lqrmsf uses the matrix sign function to solve the Riccati
  34. // Equation to compute the estimator gains. The input Reps, is the tolerance
  35. // for convergence, and iterm is the maximum number of iterations allowed
  36. // in lqrmsf.
  37. //
  38. // Note: Three matrices are returned in a list.
  39. //
  40. //       G.l = Optimal Estimator Gain matrix
  41. //       G.p = Riccatti Eqn. Solution, which is the estimate error covariance
  42. //
  43. // Ref: (1) Skelton, R. "Dynamic Systems Control Linear System Analysis and
  44. //          Synthesis," John Wiley and Sons, 1988.
  45. //
  46. // Copyright (C), by Jeffrey B. Layton, 1994
  47. // Version JBL 940918
  48. //----------------------------------------------------------------------------
  49.  
  50. rfile lqrmsf
  51.  
  52. lqemsf = function(a,d,m,q,r,ct,Reps,iterm)
  53. {
  54.    local(nargs,Dum,eps,itermax)
  55.  
  56. // Count number of input arguments
  57.    nargs=0;
  58.    if (exist(a)) {nargs=nargs+1;}
  59.    if (exist(d)) {nargs=nargs+1;}
  60.    if (exist(m)) {nargs=nargs+1;}
  61.    if (exist(q)) {nargs=nargs+1;}
  62.    if (exist(r)) {nargs=nargs+1;}
  63.    if (exist(ct)) {nargs=nargs+1;}
  64.    if (exist(Reps)) {nargs=nargs+1;}
  65.    if (exist(iterm)) {nargs=nargs+1;}
  66.  
  67.    if ( nargs < 5) {
  68.        error("LQEMSF: Wrong number of input arguments.");
  69.    }
  70.  
  71. // Set defaults if variables are not input.
  72. // ========================================
  73.  
  74.    if (!exist(iterm)) {
  75.        itermax=1000;
  76.    else
  77.        itermax=iterm;
  78.    }
  79.  
  80.    if (!exist(Reps)) {
  81.        eps=1.0e-10;
  82.    else
  83.        eps=Reps
  84.    }
  85.  
  86. // Don't do any error checking, let lqr take care of that.
  87.    if ( exist(ct)) {
  88.         Dum=lqrmsf(a',m',d*q*d',r,d*ct,eps,itermax);
  89.    else
  90.         Dum=lqrmsf(a',m',d*q*d',r,eps,itermax);
  91.    }
  92.     
  93.    return << l=Dum.G'; p=Dum.P' >>
  94. };
  95.