home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / glqe < prev    next >
Encoding:
Text File  |  1995-11-15  |  2.5 KB  |  82 lines
  1. //----------------------------------------------------------------------------
  2. //
  3. // glqe
  4. //
  5. // Syntax: G=glqe(A,D,M,Q,R,CT)
  6. //
  7. // This routine comptues the gradient of the Linear Quadratic Estimator with
  8. // respect to a scalar parameters 'p' for continuous systems. The system is
  9. // given by the following differential equations:
  10. //      .
  11. //      x = Ax + Bu + Dw
  12. //      z = Mx + Gu + v
  13. //
  14. // where z is the vector of sensor measurements, w is the vector if process
  15. // noise, and v is the vector of sensor noise. The system has the following
  16. // process noise and measurement noise covariances:
  17. //
  18. //           E(w) = E(v) = 0
  19. //           E(ww') = Q
  20. //           E(vv') = R
  21. //           E(wv') = 0
  22. //
  23. // where E is the expectation operator. The routine returns the gradient of
  24. // the estimator gain, dl, as well as the gradient of the Riccatti solution.
  25. //
  26. // This routine takes advantage of the dual nature of the optimal estimator,
  27. // by using the lqr routine to compute the optimal estimator.
  28. //
  29. // Note: Two matrices are returned in a list.
  30. //
  31. //       G.dl = Gradient of the Optimal Estimator Gain matrix
  32. //       G.dp = Gradient of the Riccatti Eqn. SOlution.
  33. //
  34. // Ref: (1) Skelton, R. "Dynamic Systems Control Linear System Analysis and
  35. //          Synthesis," John Wiley and Sons, 1988.
  36. //
  37. // Copyright (C), by Jeffrey B. Layton, 1994
  38. // Version JBL 940918
  39. //----------------------------------------------------------------------------
  40.  
  41. rfile glqr
  42.  
  43. glqe = function(a,da,d,dd,m,dm,q,dq,r,dr,ct,dct)
  44. {
  45.    local(d,dalocal,dblocal,dqlocal,drlocal,dctlocal,nargs,Dum)
  46.  
  47. // Count number of inpute arguments
  48.    nargs=0;
  49.    if (exist(a)) {nargs=nargs+1;}
  50.    if (exist(da)) {nargs=nargs+1;}
  51.    if (exist(d)) {nargs=nargs+1;}
  52.    if (exist(dd)) {nargs=nargs+1;}
  53.    if (exist(m)) {nargs=nargs+1;}
  54.    if (exist(dm)) {nargs=nargs+1;}
  55.    if (exist(q)) {nargs=nargs+1;}
  56.    if (exist(dq)) {nargs=nargs+1;}
  57.    if (exist(r)) {nargs=nargs+1;}
  58.    if (exist(dr)) {nargs=nargs+1;}
  59.    if (exist(ct)) {nargs=nargs+1;}
  60.    if (exist(dct)) {nargs=nargs+1;}
  61.  
  62.    if ( (nargs != 10) || (nargs != 12) ) {
  63.        error("DLQE: Wrong number of input arguments.");
  64.    }
  65.  
  66. // Create gradient of input matrices for mxglqr
  67.    dalocal=da';
  68.    dblocal=dm';
  69.    dqlocal=dd*q*d' + d*dq*d' + d*q*dd';
  70.    drlocal=dr;
  71.  
  72. // Don't do any error checking, let lqr take care of that.
  73.    if ( exist(ct)) {
  74.         dctlocal=dd*ct + d*dct';
  75.         Dum=glqr(a',dalocal,m',dblocal,d*q*d',dqlocal,r,drlocal,d*ct,dctlocal);
  76.    else
  77.         Dum=glqr(a',dalocal,m',dblocal,d*q*d',dqlocal,r,drlocal);
  78.    }
  79.     
  80.    return << l=d.dk'; p=d.dp' >>
  81. };
  82.