home *** CD-ROM | disk | FTP | other *** search
/ Education Sampler 1992 [NeXTSTEP] / Education_1992_Sampler.iso / Mathematics / Notebooks / COSY_PAK / COSYPAK / chap2.m < prev    next >
Text File  |  1992-06-01  |  4KB  |  159 lines
  1.  
  2. (* 
  3. **********************************************************
  4.  
  5.          COSY_PAK package chap2.m        
  6.  
  7. **********************************************************
  8. *)
  9.  
  10.  
  11. If [ TrueQ[ $VersionNumber >= 2.0 ],
  12.      Off[ General::spell ];
  13.      Off[ General::spell1 ] ]
  14.      
  15. (*  B E G I N     P A C K A G E  *)
  16.  
  17. BeginPackage["COSYPAK`chap2`"];
  18.  
  19. SS2Transf::usage = 
  20. "SS2Transf[A,B,C,s]";
  21.  
  22. Ode2SS::usage = 
  23. "Ode2SS[lhscoeff, rhscoeff]: Convers linear  ODE to State Space Eqn. 
  24.  The ODE is in the format: y(n) + a1 y(n-1) + ... + an-1 y(1) + an y = 
  25.  b0 u(n) + b1 u(n-1) + ... bn-1 u(1) + bn u   
  26.  where lhscoeff = [1, a1,...,an],  rhscoeff = [b0,b1,...,bn]. 
  27.  The output are the Matrix A and Vector B in the state equation: 
  28.      dX/dt = A x + B u.";
  29.  
  30. Linearize::usage = 
  31. "Linearize[f, {x1,...,xn}, {x10,...,xn0}]: gives the linearization 
  32.  of vector function f[x1,...,xn] = {f1[x1,...,xn],...,fn[x1,...,xn]}
  33.  at the point {x10,...,xn0}. The length of the function variables 
  34.  {x1,...,xn} must be equal to the length of the operating point 
  35.  {x10,...,xn0}.";
  36.  
  37. Begin["`Private`"];
  38.  
  39. (* Function for transferring State Space Eqn to
  40. a transfer function *)
  41. SS2Transf[A_,B_,C_,s_] :=
  42. Block[{transf,II,na1,na2,nb,nc},
  43.       na1 = Dimensions[A][[1]]; 
  44.       na2 = Dimensions[A][[2]];
  45.       nb = Dimensions[B][[1]];
  46.       nc = Dimensions[C][[1]];
  47.       If[na1 != na2 ||nb != na1 || nc != na2,
  48.        Print["Dim[A]= ", na1,"x",na2];
  49.        Print["Dim[B]= ", nb,"x","1"];
  50.        Print["Dim[C]= ", "1","x",nc];
  51.        Return["Warring!! Dimensions Dismatch !!"]
  52.        ];
  53.       
  54.       II=IdentityMatrix[na1];     
  55.       transf=Together[C.Inverse[s II -A].B];
  56.       Print["The transfer function is:",transf];    
  57.       Return[transf]
  58.     ];
  59.  
  60.  
  61. (* Function for transferring ODE to State Space Eqn *)
  62. Ode2SS[num_,den_] :=
  63. Block[{i,j,A,B,nn=(Length[num]-1),n=(Length[den]-1),
  64.             bata,num1,den1,b0,temp},
  65.     A = Table[0,{i,n},{j,n}];
  66.     B = Table[0,{i,n}];
  67.     If[nn<n,num1=Join[Table[0,{i,1,n-nn}],num],num1=num];
  68.     (* Devide all parameter by a0 to make the first coefficient
  69.         to be zero *)
  70.     num1 = num1/den[[1]];
  71.     den1 = den/den[[1]];
  72.     
  73.     b0 = num1[[1]];
  74.     
  75.     (* Take off the first element of numerator and denominator *)
  76.     num1 = Drop[num1,1];
  77.     den1 = Drop[den1,1]; 
  78.     (* The A Matrix *)
  79.     Do[A[[i,i+1]]=1,{i,1,n-1}];    
  80.     Do[A[[n,i]]=-den1[[n+1-i]],{i,1,n}];
  81.     (* The B Vector *)
  82.     B[[1]] = num1[[1]] - den1[[1]]*b0;
  83.     Do[temp = Sum[den1[[j]]*B[[i-j]],{j,1,i-1}];
  84.        B[[i]] = Simplify[num1[[i]] - temp - den1[[i]]*b0],
  85.        {i,2,n}
  86.        ];
  87.     (* Print out the result *)   
  88.     Print["The A Matrix is:  "];Print[" "];Print["  "];
  89.     Print["   ",TableForm[A]];
  90.     Print["     "];Print["     "];
  91.     Print["The B Vector is:  "];Print["  "];
  92.     Print["   ",TableForm[B]];
  93.     Return[{A,B}]
  94.     ];    
  95.  
  96. (* Linearize the nonlinear system *)    
  97. Linearize[f_,xvars_,xpoint_,uvars_,upoint_] :=
  98.   Block[{d1,i,j,nfunc,nx,nxvars,nu,nuvars,xrule,urule,
  99.               A,B,a,b},
  100.      nfunc = Length[f]; 
  101.      nxvars = Length[xvars];
  102.      nx = Length[xpoint];
  103.      nuvars = Length[uvars];
  104.      nu = Length[upoint];
  105.      
  106.      (* Check the lengths of vars and points *)
  107.      If [nx != nxvars,
  108.       Print[StringForm[
  109.       "Linearize::Incompatible no. of xvars (``) and base xpoints (``).",
  110.        nxvars,nx]];
  111.        Return[{}]];
  112.      
  113.      If[nu != nuvars,
  114.       Print[StringForm[
  115.       "Linearize::Incompatible no. of uvars (``) and base upoints (``).",
  116.        nuvars,nu]];
  117.          Return[{}]];
  118.  
  119.              
  120.      (* Substitution Rule *)
  121.      xrule = Table[xvars[[j]] -> xpoint[[j]], {j,1,nx}];
  122.      If[nu != 0,
  123.         urule = Table[uvars[[j]] -> upoint[[j]], {j,1,nu}];
  124.         rule = Join[xrule,urule],
  125.         rule = xrule
  126.         ];
  127.              
  128.      
  129.      (* Produce the A Matrix *)
  130.      A = Array[a,{nfunc,nx}];
  131.      Do[a[i,j] = D[f[[i]],xvars[[j]]]/.rule , {i,1,nfunc},{j,1,nx}];
  132.      
  133.      (* Produce the B Matrix *)
  134.      If[nu != 0,
  135.         B = Array[b,{nfunc,nu}];
  136.         Do[b[i,j] = D[f[[i]],uvars[[j]]]/.rule , {i,1,nfunc},{j,1,nu}]
  137.         ];
  138.     
  139.     (* Print out the result *)   
  140.     Print["The A Matrix is:  "];Print[" "];Print["  "];
  141.     Print["   ",TableForm[A]];
  142.     If[nu != 0,
  143.         Print["     "];Print["     "];
  144.         Print["The B Vector is:  "];Print["  "];
  145.         Print["   ",TableForm[B]];
  146.        ];
  147.     Return[{A,B}]
  148.     ];    
  149.  
  150.        
  151. End[];
  152. EndPackage[];
  153.  
  154. (*  E N D     P A C K A G E  *)
  155.  
  156. If [ TrueQ[ $VersionNumber >= 2.0 ],
  157.      On[ General::spell ];
  158.      On[ General::spell1 ] ]
  159.