home *** CD-ROM | disk | FTP | other *** search
/ Education Sampler 1992 [NeXTSTEP] / Education_1992_Sampler.iso / Mathematics / Notebooks / COSY_PAK / COSYPAK / chap7.m < prev   
Text File  |  1992-06-07  |  9KB  |  401 lines
  1.  
  2. (* 
  3. **********************************************************
  4.  
  5.          COSY_PAK package chap7.m        
  6.  
  7. **********************************************************
  8. *)
  9.  
  10. If [ TrueQ[ $VersionNumber >= 2.0 ],
  11.      Off[ General::spell ];
  12.      Off[ General::spell1 ] ]
  13.      
  14. (*  B E G I N     P A C K A G E  *)
  15.  
  16. BeginPackage["COSYPAK`chap7`",
  17.         "LinearAlgebra`MatrixManipulation`", 
  18.         "SignalProcessing`Analog`LaPlace`",
  19.         "SignalProcessing`Analog`InvLaPlace`"]
  20.  
  21. matrixpower::usage = 
  22.     "matrixpower[A,n] returns the result of A^n."
  23. (*    Mathematica's multiplication of A^2 != A.A  *)
  24. (*    made function to fix this  *)
  25.  
  26. Controllable::usage = 
  27.         "Controllable[A, B] returns a logic value 
  28.         representing the complete state 
  29.         controllability of system A, B."
  30.  
  31. PolePlaceGain::usage =
  32.         "PolePlaceGain[A, B, poles] returns gain matirix K
  33.          to place poles of system A - BK at 
  34.          locations specified by poles."
  35.  
  36. ObsPolePlace::usage =
  37.         "ObsPolePlace[A, C, newpoles] determines 
  38.         state observer gain matrix using 
  39.         Ackermann's formula."
  40.         
  41. rank::usage =
  42.         "rank[A] returns rank of matrix A."
  43.  
  44. Observable::usage =
  45.         "Observable[A, C] returns gain matrix to 
  46.         place observer poles."
  47.  
  48. sspace::usage =
  49.         "sspace[a, b] returns the state-space form of 
  50.         the ordinary differential equation such as
  51.      y''' + a2 y'' + a3 y' + a4 y = b1 u' + b2 u 
  52.      with the coefficients of the y variables 
  53.      given by the list a and the coefficients 
  54.      of the u variables given by the list b.  
  55.      The list a must start with a 1 and the list 
  56.      b should be padded with leading zeroes 
  57.      if it is not the same length as a. The A,
  58.      B, C, and D matrices of the equations
  59.      x'= Ax + Bu
  60.      y = Cy + Du
  61.      as the global values AOUT, BOUT, COUT,
  62.      and DOUT."
  63.  
  64. ExpAt::usage=
  65.         "ExpAt[a] returns exponential matrix of A."
  66.  
  67. SysResponse::usage=
  68.         "SysResponse[A, B, C, x0, input, s, {t, tmin, tmax}, gopts]: \
  69.         Graphs output of system y and returns time domain \
  70.         solutions for state x and output y. Returns graphics."
  71.  
  72.  
  73. OutControllable::usage=
  74.         "OutControllable[A, B, C, D] returns a logic value 
  75.         representing the output 
  76.         controllability of system A, B, C, D."
  77.  
  78. tpose::usage=
  79.         "tpose[A] returns the transpose of 
  80.         the matrix A."
  81.  
  82.  
  83. Begin["`private`"]
  84.  
  85. rank[a_]:= Block[{red,size,rnk,nz,i,j},
  86.     rnk= 0;
  87.     size= Dimensions[a];
  88.     red= RowReduce[a];
  89.     Do[ nz= False;
  90.         Do[ If [red[[i,j]]!= 0, nz= True], 
  91.             {j,size[ [2] ]} ];
  92.         If [ nz, rnk= rnk + 1], {i, size[ [1] ]} ];
  93.     rnk
  94.     ];
  95.     
  96.  
  97. matrixpower[matrix_,power_Integer?Positive]:=
  98.     Block[ {ii,temp,size},
  99.     size= Dimensions[matrix];
  100.     temp= IdentityMatrix[ size[  [1]  ]  ];
  101.     Do[ temp= temp . matrix, {ii,power} ]; 
  102.     temp
  103.     ];
  104.     
  105.  
  106. Controllable[a_,b_]:=Block[{i, augm, dimena, rowa, cola,
  107.     dimenb, rowb, colb},
  108.     
  109.     (* Get sizes of matrices *)
  110.     dimena= Dimensions[a];
  111.     dimenb= Dimensions[b];
  112.     rowa= dimena[ [1] ];        
  113.     cola= dimena[ [2] ];
  114.     rowb= dimenb[ [1] ];        
  115.     colb= dimenb[ [2] ];
  116.     
  117.     If[rowa != cola, Print["A must be a square matrix."]];
  118.  
  119.     augm= b;
  120.     
  121.     (* Form augmented matrix *)
  122.     Do[augm= AppendRows[augm,matrixpower[a,i].b],
  123.         {i,(rowa-1)}];
  124.  
  125.     (* Check if controllability *)
  126.     (* matrix is of full rank   *)
  127.     rank[augm] == rowa
  128.     ];
  129.     
  130.  
  131. PolePlaceGain[a_,b_,newpoles_]:= Block[{m1, s, dimnpoles,
  132.     phi, i, tot, temp, j, dimena, rowa, m2},
  133.  
  134.     If[ Controllable[a,b],
  135.     dimnpoles= Dimensions[newpoles];
  136.     
  137.     (*  Ackermann's formula  *)
  138.     phi= Expand[ Product[ s - newpoles[ [i] ],
  139.         { i,dimnpoles[ [1] ] } ] ];
  140.     tot= Coefficient[phi,s,0] IdentityMatrix
  141.         [dimnpoles[ [1] ] ];
  142.     Do[tot= tot + 
  143.         Coefficient[phi,s,i] matrixpower[a,i],
  144.         {i,1,dimnpoles[ [1] ]} ];
  145.         
  146.     If[ dimnpoles[ [1] ] == 1, m1= {1}, m1= {0};
  147.         Do [AppendTo[m1,0], {i, 1, (dimnpoles[[1]]-2)} ];
  148.         AppendTo[ m1,1 ]; ];
  149.         m1= {m1};
  150.         
  151.          (*  controllability matrix *)
  152.          dimena= Dimensions[ a ];
  153.         rowa= dimena[ [1] ];
  154.         m2= b;
  155.         Do[m2= AppendRows[m2, matrixpower[a,i].b],
  156.          {i,(rowa-1)} ];
  157.  
  158.     (*  find gain matrix K  *)
  159.     m1 . Inverse[ m2 ] . tot,
  160.     
  161.     (* Not Controllable *)
  162.     Print["The system is not controllable."]]
  163.     ];
  164.     
  165.  
  166. ObsPolePlace[a_,c_,newpoles_]:= Block[{m1, s, dimnpoles,
  167.     phi, i, tot, temp, j, dimena, rowa, m2},
  168.     
  169.         (*  full state observer  *)
  170.     
  171.     If[Observable[a,c],
  172.     dimnpoles= Dimensions[newpoles];
  173.     
  174.     (*  Ackermann's formula  *)
  175.     phi= Expand[ Product[ s - newpoles[ [i] ],
  176.         { i,dimnpoles[ [1] ] } ] ];
  177.     tot=Coefficient[phi,s,0] IdentityMatrix
  178.         [dimnpoles[ [1] ] ];
  179.     Do[tot= tot + 
  180.         Coefficient[phi,s,i] matrixpower[a,i],
  181.         {i,1,dimnpoles[ [1] ]} ];
  182.         
  183.     If[ dimnpoles[ [1] ] == 1, m1= {{1}}, m1= {{0}};
  184.         Do [ m1= AppendColumns[m1, {{0}} ],
  185.             {i, 1, (dimnpoles[[1]]-2)} ];
  186.         m1= AppendColumns[ m1, {{1}} ]; ];
  187.         
  188.          (*  observability matrix *)
  189.          dimena= Dimensions[ a ];
  190.         rowa= dimena[ [1] ];
  191.         m2= c;
  192.         Do[m2= AppendColumns[m2, c . matrixpower[a,i] ],
  193.          {i,(rowa-1)} ];
  194.  
  195.     (*  find gain matrix K  *)
  196.     tot . Inverse[ m2 ] . m1,
  197.     
  198.     (* Not Observable *)
  199.     Print["The system is not observable."]]
  200.     ];
  201.  
  202.  
  203. tpose[a_]:= Block[{i,j,size,row,col,temp},
  204.     (* get # of rows and cols of a *)
  205.     size= Dimensions[a];
  206.     row= size[[1]];
  207.     col= size[[2]];
  208.     
  209.     (* initialize temp matrix *)
  210.     temp= Array[1,{col,row}];
  211.     
  212.     (* transpose elements *)
  213.     Do[
  214.         Do[temp[[j,i]]= a[[i,j]], { j, size[[2]] }],
  215.         { i, size[[1]] }];
  216.     temp ];
  217.  
  218.     
  219. Observable[a_,c_]:=Block[{i,augm,dimena,rowa,cola,dimenc
  220.     ,rowc,colc},
  221.     
  222.     (* Get sizes of matrices *)
  223.     dimena=Dimensions[a];
  224.     dimenc=Dimensions[c];
  225.     rowa=dimena[[1]];
  226.     cola=dimena[[2]];
  227.     rowc=dimenc[[1]];
  228.     colc=dimenc[[2]];
  229.     
  230.     If[rowa != cola, Print["A must be a square matrix."]];
  231.  
  232.     (* Begin augmented matrix *)
  233.     augm= c;
  234.     
  235.     (* Form augmented matrix *)
  236.     Do[augm= AppendColumns[augm,
  237.         c . matrixpower[a,i] ],
  238.         {i,(rowa-1)} ];
  239.         
  240.     (* Check if observability *)
  241.     (* matrix is of full rank   *)
  242.     rank[augm] == rowa
  243.     ];
  244.     
  245.     
  246.     sspace[a_,b_]:= Block[{na,nb,beta,temp},
  247.     (* Convert ordinary differential *)
  248.     (* equation to state space *)
  249.  
  250.     (* find size of input lists *)
  251.     na= Dimensions[a];
  252.     na= na[[1]];
  253.     
  254.     nb= Dimensions[b];
  255.     nb= nb[[1]];
  256.     
  257.     (* lists must be same size *)
  258.     If[nb != na,Print["Coefficient lists a and b",
  259.         " must be the same size.  Remember to pad",
  260.         " the lists with leading zeros, if needed."] ];
  261.         
  262.     beta= {b[[1]]};
  263.     
  264.     Do[ temp= b[[i]];
  265.         Do[ temp= temp - a[[j]] beta[[i-j+1]], {j,2,i}];
  266.         AppendTo[ beta, temp], {i,2,nb}];
  267.         
  268.     AOUT= IdentityMatrix[na-1];
  269.     Do[ Do[ If[ (j-1) == i, AOUT[[i,j]]= 1, AOUT[[i,j]]= 0],
  270.             {i, na-2}];
  271.         AOUT[[na-1,j]]= -a[[na+1-j]], {j,na-1} ];
  272.         
  273.     BOUT= {{beta[[2]]}};
  274.     Do[ BOUT= AppendColumns[ BOUT, {{beta[[i]]}} ],
  275.         {i, 3, na}];
  276.     
  277.     COUT= {{1}};
  278.     Do[ COUT=AppendRows[COUT,{{0}}], {na-2}];
  279.     
  280.     DOUT= beta[[1]];
  281.         
  282.     Print["The A Matrix is:"];
  283.     Print[" "]; Print[" "];
  284.     Print[" ",TableForm[AOUT]];
  285.     Print[" "]; Print[" "];
  286.     Print["The B Matrix is:"];
  287.     Print[" "]; Print[" "];
  288.     Print[" ",TableForm[BOUT]];
  289.     Print["The C Matrix is:"];
  290.     Print[" "]; Print["  "];
  291.     Print[" ",TableForm[COUT]];
  292.     Print[" "]; Print["  "];
  293.     Print["The D Matrix is:"];
  294.     Print[" "]; Print["  "];
  295.     Print[" ",TableForm[DOUT]];
  296. ];
  297.  
  298.  
  299. OutControllable[a_,b_,c_,d_]:=Block[{i, augm, dimena, dimenc, 
  300.     rowa, cola, dimenb, rowb, colb, rowc},
  301.     
  302.     (* Get sizes of matrices *)
  303.     dimena= Dimensions[a];
  304.     dimenb= Dimensions[b];
  305.     dimenc= Dimensions[c];
  306.     rowa= dimena[ [1] ];        
  307.     cola= dimena[ [2] ];
  308.     rowb= dimenb[ [1] ];        
  309.     colb= dimenb[ [2] ];
  310.     rowc= dimenc[ [1] ];        
  311.  
  312.     
  313.     If[rowa != cola, Print["A must be a square matrix."]];
  314.  
  315.     augm= c.b;
  316.     
  317.     (* Form augmented matrix *)
  318.     Do[augm= AppendRows[augm,c.matrixpower[a,i].b],
  319.         {i,(rowa-1)}];
  320.     
  321.     AppendRows[augm, d];
  322.  
  323.     (* Check if controllability *)
  324.     (* matrix is of full rank   *)
  325.     rank[augm] == rowc
  326.     ];
  327.  
  328.  
  329. SysResponse[a_, b_, c_, x0_, input_,s_,{t_,tmin_,tmax_}, opts___]:=
  330.     Block[{tau, intexp, x, y, dimena,dimenb,rowa,cola,rowb,colb,
  331.         dimenc,colc,intu,II,exps},
  332.         (* Get sizes of matrices *)
  333.         dimena= Dimensions[a];
  334.         dimenb= Dimensions[b];
  335.         dimenc= Dimensions[c];
  336.         rowa= dimena[[1]];        
  337.         cola= dimena[[2]];    
  338.         colb= dimenb[[1]];
  339.         colc= dimenc[[1]];
  340.     
  341.         If[rowa != cola, Print["expAt must be a square matrix."];
  342.             Return[{}]];
  343.         If[rowa != colb, Print["expAt and B must have an equal \
  344.         number of columns"];Return[{}]];
  345.         If[cola != colc, Print["expAt and C must have an equal \
  346.         number of rows"];Return[{}]];
  347.  
  348.         II=IdentityMatrix[rowa];     
  349.             exps =Together[Inverse[s II -a]];
  350.            inputs = LaPlace[input,t,s][[1]];
  351.         
  352.         initpart = exps.x0;
  353.         Do[initpart[[i]]= Simplify[ComplexExpand[
  354.             InvLaPlace[initpart[[i]],s,t, Apart->All]]],{ i, cola }];
  355.             
  356.         forcepart = exps.b inputs;
  357.         Do[forcepart[[i]]= Simplify[ComplexExpand[
  358.             InvLaPlace[forcepart[[i]],s,t, Apart->All]]],{ i, cola }];
  359.         
  360.         
  361.         (* integrate and add initial *)
  362.         (* conditions to find x and y *)
  363.         x= initpart + forcepart;
  364.         y= c.x ;
  365.         
  366.         (* output graph and solutions *)        
  367.         Plot[y,{t,tmin,tmax}, 
  368.         FrameLabel->{"Time","Y(t)","Time Response of System "," "},
  369.         Frame->True,PlotRange -> All,GridLines->Automatic, 
  370.             FrameTicks -> {Automatic, Automatic,Automatic, Automatic},
  371.         opts];
  372.         Return[{x,y}]
  373.     ];
  374.                 
  375.  
  376.         
  377. ExpAt[a_]:= 
  378. Block[{i,j,out,xform, II, nn},
  379.     nn = Dimensions[a][[1]];
  380.     II= IdentityMatrix[nn];
  381.     xform= Inverse[s II - a];
  382.     Do[
  383.       Do[ 
  384.       out[[i,j]]= Simplify[ComplexExpand[InvLaPlace[ xform[[i,j]],s]]],
  385.      { j, nn}],
  386.      { i, nn}];
  387.     
  388.     Return[out]
  389.     ];
  390.  
  391.     
  392.     End[]
  393.     EndPackage[]
  394.  
  395.  
  396. (*  E N D     P A C K A G E  *)
  397.  
  398. If [ TrueQ[ $VersionNumber >= 2.0 ],
  399.      On[ General::spell ];
  400.      On[ General::spell1 ] ]
  401.