(****************************************************************)
(*
*****************************************************************
*
*
* TSIControls is a Mathematica package for the analysis and design of
* feedback control systems. The system is provided in two packages: 
*     TSIConL.m for analysis and design of linear systems
*     TSIConN.m for analysis and design of nonlinear systems
* 
*             THIS IS TSIConN.M for Nonlinear Systems

           Techno-Sciences, Inc.
           10001 Derekwood Lane, Suite 204
           Lanham, MD 20706
           (301) 577-6000
*
*   Copyright © 1993-1997, 1998 Techno-Sciences Incorporated
*   All Rights Reserved
* 
******************************************************************
*)
(****************************************************************)
(*                      Mathematica functions                   *)
(*                  for adaptive nonlinear control              *)
(*                                                              *)

(****************************************************************)
spell1 = (Head[General::spell1] === $Off);
spell = (Head[General::spell] === $Off);
Off[General::spell1];
Off[General::spell];

If[$VersionNumber===2.,BeginPackage["ProPac`ControlN`",{"Global`","ProPac`GeoTools`",
                         "ProPac`ControlL`","LinearAlgebra`MatrixManipulation`","Calculus`DiracDelta`"
                          }]];
If[$VersionNumber===3.,BeginPackage["ProPac`ControlN`",{"Global`","ProPac`GeoTools`",
                         "ProPac`ControlL`","LinearAlgebra`MatrixManipulation`","Calculus`DiracDelta`"
                          }]];
If[$VersionNumber===4.,BeginPackage["ProPac`ControlN`",{"Global`","ProPac`GeoTools`",
                         "ProPac`ControlL`","LinearAlgebra`MatrixManipulation`"
                          }]];
(****************************************************************)

(* Package Help *)

(****************************************************************)

(* Temp Function Usage Statements *)


NonlinearControl::usage=
"Functions in the package for adaptive nonlinear control include:\n
\n
Jacob,Grad,LieBracket,Ad\n
LieDerivative,Involutive,Rank,Span\n
ControlDistribution,Controllability\n
FeedbackLinearizable\n
SIExactFBL, SISONormalFormTrans\n
InverseTransformation,LocalInverseTransformation\n
LocalDecomposition\n
TransformSystem, PartialTransformSystem\n
VectorRelativeOrder,DecouplingMatrix\n
IOLinearize,NormalCoordinates\n
LocalZeroDynamics,DynamicExtension\n
StructureAlgorithm,HToNAndM,HToH1AndH2\n
AdaptiveRegulator\n
PSFFCond, PSFFSolve\n
AdaptiveBackstepRegulator,AdaptiveTracking\n
ExponentialObserver\n
CreateControllerMEX\n
"

(****************************************************************)

(* Function Usage Statements *)

(****************************************************************)

AdaptiveBackstepRegulator::usage=
"AdaptiveBackstepRegulator[f,g,x,Theta,AdGain,Cs] gives adaptive 
control law and parameter update laws based on adaptive 
backstepping regulator design method. 
Calling Syntax:
{r1,r2,r3}=AdaptiveBackstepRegulator[f,g,x,Theta,AdGain,Cs];\n
r1=control law u(x,Thetahat)\n
r2=p-vector of rhs of parameter update law\n
r3=n-vector of Z coordinates defined as a function of (x,Thetahat).\n
"
AdaptiveRegulator::usage =
"AdaptiveRegulator[f,g,h,x,t,theta,AdGain,poles]\n\n
computes an adaptive I-O linearizing controller.\n
This function returns the list\n
  {Parameters,ParameterEstimates,UpdateLaw,\n
                        Control}\n
where\n
\n
  f,g,h - the system expressions\n
  x,t - state vector and time parameter\n
  theta - uncertain parameter vector\n
  AdGain - adaptive gain, may be a number, scalar parameter,\n
           list of length equal to number of parameters to be\n
           estimated, or anything else including the empty list.\n
           The last simply assigns a gain parameter to each estimator\n
           equation.\n
  poles - a list of lists with each sublist of length equal to the\n
           corresponding relative degree+1 and containing the desired\n
           pole values.\n
\n
  Parameters - list of parameter groups to be estimated\n
  ParameterEstimates - list of names given to these parameters\n
  UpdateLaw - right hand side of Estimator ODE's\n
  Control - the control function
"

AdaptiveTracking::usage=
"AdaptiveTracking:\n\n 
AdaptiveTracking[f,g,h,x,t,theta,RefSig,AdGain,poles]\n\n 
computes the adaptive control defined in\n 
R. Ghanadan and G.L. Blankenship, Adaptive Output Tracking of\n 
Invertible MIMO Nonlinear Systems, SRC Technical Report \n
TR 92-68, 1992. This function returns the list\n
  {Parameters,ParameterEstimates,UpdateLaw,\n
                        Control,DecoupMatrix,DerivativeOrders}\n
where\n
\n
  f,g,h - the system expressions\n
  x,t - state vector and time parameter\n
  theta - uncertain parameter vector\n
  RefSig - tracking referance signal\n
  AdGain - adaptive gain, may be a number, scalar parameter,\n
           list of lengthequal to number of parameters to be\n
           estimated, or anything else including the empty list.\n
           The last simply assigns a gain parameter to each estimator\n
           equation.\n
  poles - a list of lists with each sublist of length equal to the corresponding\
          relative degree+1 and containing the desired pole values.\n
\n
  Parameters - list of parameter groups to be estimated\n
  ParameterEstimates - list of names given to these parameters\n
  UpdateLaw - right hand side of Estimator ODE's\n
  Control - the control function\n
  DecoupMatrix - the decoupling matrix\n
  DerivativeOrders - list of the derivative orders in the vector y(n)
"

CoefParameter::usage="CoefParameter[f_,para_] separates the parameter\n
dependent part of vector field f.\n"  

ControlDistribution::usage = 
"ControlDistribution[f,g,x] computes the controllability distribution\n
of a nonlinear system. That is, it produces a basis set of vector fields for\n
the smallest f,g[1],..g[m]-invariant distribution that contains f,g[1],..g[m].\n
ControlDistribution has two options:\n
\n
ControlDrift->False produces the smallest f,g[1],..g[m]-invariant \n
distribution that contains g[1],..g[m].\n
\n
IntermediateResults->True prints intermediate distributions as they are\n
computed.
"

Controllability::usage=
"Controllability[f,g,x] tests the pair (f,g) to determine if the system is\n 
weakly locally controllable. If the controllability distribution has rank=dim[x]\n 
it returns True. If it does not, it returns False. The option\n
LocalControllability->True tests for local controllability"
   
ObservablilityCodistribution::usage="ObservabilityCodistribution[f,G,h,var]\n
returns the controllability codistribution of a affine nonlinear system.
"

Observability::usage="Observability[f,G,h,var] tests local controllbility\n
and returns either True or False.
"

DecouplingMatrix::usage=
"DecouplingMatrix[f,g,h,x,ro] computes the decoupling matrix. f,g,h can\n 
be functions of x or lists of expressions in x. x is a list and ro is\n 
the vector relative degree."

Dist::usage="Dist[f_,g_,var_,k_] constructs distributions:  \n\n
                               1          k\n
               span{g, ad g, ad  g,..., ad g}\n
                         f     f          f\n\n"

DynamicExtension::usage=
"DynamicExtension[f,g,h,x] f,g,h are defined as functions of x \n
returns - {fnew,gnew,hnew,xnew}\n
where\n
\n
fnew,gnew,hnew are functions of xnew which define the new system,\n
\n
See DecouplingMatrix, VectorRelativeOrder.
"

ExponentialObserver::usage=
"ExponentialObserver[f,h,x,u,y,x0,u0,decay] returns\n
{rhs,xhat,eigs} that defines an exponential observer\n
of the form:\n
\n
   xhat_dot=rhs(xhat,u,y)\n
\n
for a system defined by\n
\n
    x_dot=f(x,u), y=h(x)\n
\n
x0,u0 is an equilibrium point, f(x0,u0)=0, and decay is\n
a specified exponential decay rate. The observer has an equilibrium\n
point at xhat=x0, i.e., rhs(x0,u0,h(x0))=0, and its linearization at\n
this point has eigenvalues eigs. Furthermore, Re(eigs)<decay.\n
\n
By default the state estimates assume the names of the states with 'hat'\n
adjoined. The calling syntax:\n
\n
   {rhs,xhat,eigs}=ExponentialObserver[f,h,x,u,y,x0,u0,decay,z]\n
\n
returns state estimates with names z1,z2,...,zn\n
"

FeedbackLinearizable::usage=
"FeedbackLinearizable[f,g,x] tests to see if the pair (f,g) is exactly\n
linearizable by means of feedback and a change of coordinates. The pair\n 
(f,g) must be controllable, and, for each 0<=j<=Length[x]-2, the distribution\n
\n
   Table[Ad[f,g[i],x,k],{k,0,j},{i,1,Length[g]}]\n
\n
must be involutive. The function returns True or False."

FBLCond::usage="FBLCond[f,g,var] checks the necessary and sufficient\n
conditions for feedback linearization of a nonlinear system {f + g.u}.\n
The returned result is True or False.\n"

HToH1AndH2::usage =
"HToH1AndH2[H,LengthY,t] pulls out highest order derivatives from HY \n
as obtained from StructureAlgorithm. Used to construct dynamic decoupling\n
compensator. Returns:\n

        {H1,H2,yH1,yH2,derlist}
"

HToNAndM::usage =
"HToNAndM[H,LengthY,t] pulls out Lowest order derivatives from HY as\n 
obtained from StructureAlgorithm. Used to construct tracking controller in\n
AdaptiveTracking. Retuens:\n

        {N,M,yN,yM,derlist}
"

InverseTransformation::usage="InverseTransformation[var,newvar,Trans] finds the inverse\n
of the given transformation, newvar=Trans[var], using the Solve function.
\n"

IOLinearize::usage=
"IOLinearize[f,g,h,x] computes a feedback linearizing & decoupling\n
control. It returns\n
  {DecoupMat, Nonlin, RelOrder, control}\n
where,\n
  DecoupMat - decoupling matrix,\n
  Nonlin - vector of LieDerivative[f,h[[i]],x, RelOrder[[i]]],\n
  RelOrder - relative order,\n
  control - decoupling control law.\n
Calling syntax:\n
{DecoupMat, Nonlin, RelOrder, control}=IOLinearize[f,g,h,x]\n 
"

LocalInverseTransformation::usage="LocalInverseTransformation[var,newvar,Trans,order] finds the\n 
approximate inverse of the given transformation, newvar=Trans[var], near var=0,\n 
as a power series up to degree 'order'.
\n"

LocalDecomposition::usage=
"LocalDecomposition[f,G,h,var,u,m] returns {Psi,PsiInverse,fnew,Gnew,hnew}\n
where Psi is a transformation var=Psi(z) that puts the system into a \n
partitioned form with four parts:\n
\n
controllable & not observable\n
controllable & observable\n
not controllable & not observable\n
not controllable & observable\n
\n
computations are carried out locally up to order m, if m is an integer.\n
Computations are exact, if this is possible, when m=Infinity.\n 
"

LocalZeroDynamics::usage=
"LocalZeroDynamics[f,g,h,x,u0,z] returns a local representation of\n
the zero dynamics about the origin (x=0)."

NormalCoordinates::usage=
"NormalCoordinates[f,g,h,x,Vro] returns the functions z(x) which\n
define the linearizable states."

PartialTransformSystem::usage="PartialTransformSystem[f,g,var,Trans,newvar] transforms\n
system {f,g} to system {Newfff,Newggg} using newvar=Trans. The new\n
description may still have some of the original coordinates.\n
Callinf Syntax: {Newfff,Newggg}=PartialTransformSystem[f,g,var,Trans,newvar].\n"

PSFFCond::usage="PSFFCond[f,g,var,para] checks the necessary and\n
sufficient geometric conditions to determine if a single-input nonlinear\n
system {f,g} with uncertain parameter {para} is transformable to\n
Parametric Strict Feedback Form. The returned result is True or False."

PSFFSolve::usage="PSFFSolve[f,g,var,para] computes the diffeomorphism\n
required to transform a single-input nonlinear system {f,g} with\n
parameter {para} to a Parametric Strict Feedback Form. It first \n
checks whether such coordinate change exists by checking the necessary\n
and sufficient geometric conditions. It returns the transformation, and\n
the new vector fields {fnew,gnew} describing the original system in the\n
PSFF form.\n"

RelativeOrder::usage="see VectorRelativeOrder"

SIExactFBL::usage="SIExactFBL[f,g,var] solves for a set of\n
coordinates that transforms a single input nonlinear system {f + g.u}\n
to an equivalent linear system if possible. \n
SIExactFBL[f_,g_,var_,True] should be used for selected output
printing.\n\n"

SISONormalFormTrans::usage="SISONormalFormTrans[f,g,h,var] gives\n 
the coordinate transformation that puts a SISO nonlinear system in normal\n
form with the internal dynamics independent of the control. It returns\n
two lists {T1,T2}: T1 isthe normal form partial transformation for I/O\n
linearization, and T2 contains additional transformation equations required\n
to complete the transformation. Thus, r=Length[T1] is the relative degree\n
and q=Length[T2] is the dimension of the internal dynamics.\n
"

StructureAlgorithm::usage=
"StructureAlgorithm[f,g,h,x,t] computes an inverse system as proposed\n
by S. N. Singh, A Modified Algorithm for Invertibility in\n 
Nonlinear Systems, IEEE Trans. Automatic Control, Vol. AC-26,\n 
No 2., April 1981. Returns the list\n
\n
                  {Dbeta ,Cbeta ,Hbeta, zbeta}\n
where:\n
                zbeta=Hbeta*Ybeta=Cbeta+Dbeta*u\n
                \n
and an inverse system is given by\n
\n
           x_dot=f+G*RightInverse[Dbeta]*(-Cbeta+zbeta)\n
"

TransformSystem::usage="TransformSystem[f,g,var,newvar,Trans,InvTrans] transforms\n 
system {f,g} from var coordinates to the newvar=Trans(var) coordinates\n
and gives the corresponding description {Newf,Newg}.\n  
Calling Syntax: {Newf,Newg}=TransformSystem[f,g,var,newvar,Trans,InvTrans];\n
                {Newf,Newg,Newh}=TransformSystem[f,g,h,var,newvar,Trans,InvTrans];
\n"

VectorRelativeOrder::usage=
"VectorRelativeOrder[f,g,h,x] computes the vector relative order if a\n
MIMO system. f,g,h are functions of x defined explicitly as lists of\n
expressions in x"

(********* Options *)

IntermediateResults::usage="IntermediateResults is an option of\n
ControlDistribution. IntermediateResults->True will produce printing\n 
of the distribution:\n
         Span{f,Ad[f,g[i],k],{k,0,n-1},{i,1,m}}\n
"
IncludeDrift::usage="IncludeDrift is an option of\n
ControlDistribution. IncludeDrift->False produces a basis set of vector fields for\n
the smallest f,g[1],..g[m]-invariant distribution that contains g[1],..g[m].
"
LocalControllability::usage="LocalControllability is an option of\n
Controllability. LocalControllability->True tests for local controllability\n
rather than weak local controllability.
"
Options[ControlDistribution]={IntermediateResults->False,IncludeDrift->True};
Options[Controllability]={LocalControllability->False};

(* ****************************************************** *)

(*  Functions *)

Begin["`private`"]

(* *********************************************** *)
(* ******        Controllability            ****** *)
(* *********************************************** *)

Clear[ControllabilityMat];
ControllabilityMat[f_?VectorQ,g_?MatrixQ,var_List] := Module[{i,k,CM},
         CM=Table[Ad[f,Transpose[g][[i]],var,k],{k,0,Length[var]-1},
                                      {i,1,Length[Transpose[g]]}];
         Flatten[CM,1]];
ControllabilityMat[f_?VectorQ,g_?VectorQ,var_List] := Module[{i,k,CM},
         CM=Table[Ad[f,g,var,k],{k,0,Length[var]-1}];
         CM
         ];
SetAttributes[ControllabilityMat,ReadProtected];
SetAttributes[ControllabilityMat,Locked];

Clear[JoinFG]
JoinFG[f_?VectorQ,G_?MatrixQ]:=AppendColumns[{f},Transpose[G]];
JoinFG[f_?VectorQ,G_?VectorQ]:=AppendColumns[{f},{G}];
SetAttributes[JoinFG,ReadProtected];
SetAttributes[JoinFG,Locked];

Clear[ControlDistribution];
ControlDistribution[f_,g_,var_List,opts___]:=Module[{CM,CD},
         (*
         CM=ControllabilityMat[f,g,var];
         CD=Span[AppendColumns[{f},CM]];
         *)
         CM=JoinFG[f,g];
         If[IncludeDrift/.{opts}/.Options[ControlDistribution],
           CD=Span[JoinFG[f,g]],
           CD=Span[JoinFG[f*0,g]]];
         If[IntermediateResults/.{opts}/.Options[ControlDistribution],
            Print["Intermediate distribution is:"];
            Print[CD]];
         MaxExpandDistribution[CM,CD,var,IntermediateResults/.{opts}/.Options[ControlDistribution]]
];
SetAttributes[ControlDistribution,ReadProtected];
SetAttributes[ControlDistribution,Locked]; 

MaxExpandDistribution[tau_List,del_List,var_?VectorQ,IntermediateResults_]:=
  Module[{kdel,ktau,h,vec},
    kdel=Length[del];
    ktau=Length[tau];
    h=Table[LieBracket[tau[[i]],del[[j]],var],{i,1,ktau},{j,1,kdel}];
    vec=Span[Union[Flatten[h,1],del]];
    If[IntermediateResults,
            Print["Intermediate distribution is:"];
            Print[vec]];
    If[(Rank[vec]>Rank[del])&&(Rank[vec]!=Length[var]),
       Span[MaxExpandDistribution[tau,vec,var,IntermediateResults]],Return[vec]]
];

(*
Clear[Controllability];
Controllability[f_,g_,var_List]:=
         If[Rank[ControllabilityMat[f,g,var]]==Length[var],
                True,False]
SetAttributes[Controllability,ReadProtected];
SetAttributes[Controllability,Locked];
*)
Clear[Controllability];
Controllability[f_,g_,var_List,opts___]:=Module[{CM},
   		If[LocalControllability/.{opts}/.Options[Controllability],
				CM=ControlDistribution[f,g,var,IncludeDrift->False],
   			CM=ControlDistribution[f,g,var]];
         If[Rank[CM]==Length[var],True,False]
];
SetAttributes[Controllability,ReadProtected];
SetAttributes[Controllability,Locked];

(* *********************************************** *)
(* ******          Observability            ****** *)
(* *********************************************** *)

Clear[Observability];
Observability[f_?VectorQ,G_?MatrixQ,h_?VectorQ,var_?VectorQ]:=Module[{Del0},
			DelO=LargestInvariantDistribution[Join[{f},Transpose[G]],h,var];
		If[DelO=={},True,False]
		];
Observability[f_?VectorQ,G_?VectorQ,h_?VectorQ,var_?VectorQ]:=
  Observability[f,Transpose[{G}],h,var];
Observability[f_?VectorQ,G_?VectorQ,h_,var_?VectorQ]:=
  Observability[f,Transpose[{G}],{h},var];
SetAttributes[Observability,ReadProtected];
SetAttributes[Obervability,Locked];

Clear[ObservabilityCodistribution];
ObservabilityCodistribution[f_?VectorQ,G_?MatrixQ,h_?VectorQ,var_?VectorQ]:=
  Module[{Del0},
		Del0=LargestInvariantDistribution[Join[{f},Transpose[G]],h,var];
		If[Del0=={},IdentityMatrix[Length[f]],SpecialNullSpace[Del0]]
		];
ObservabilityCodistribution[f_?VectorQ,G_?VectorQ,h_?VectorQ,var_?VectorQ]:=
  ObservabilityCodistribution[f,Transpose[{G}],h,var];
ObservabilityCodistribution[f_?VectorQ,G_?VectorQ,h_,var_?VectorQ]:=
ObservabilityCodistribution[f,Transpose[{G}],{h},var];
SetAttributes[ObservabilityCodistribution,ReadProtected];
SetAttributes[ObervabilityCodistribution,Locked];

(* *********************************************** *)
(* ******        Linearizability            ****** *)
(* *********************************************** *)

Clear[FeedbackLinearizable];
FeedbackLinearizable[f_Function,g_Function,var_List]:=
                        FeedbackLinearizable[f[var], g[var],var];
FeedbackLinearizable[f_?VectorQ,g_?MatrixQ,var_List]:=Module[{cm,cm1,n,AA=True},
         n=Length[var];
         cm=ControllabilityMat[f,g,var];
         Do[cm1=Drop[cm,-(n-i1)*Dimensions[g][[2]]];   (* drop last m elements *)
            If[Involutive[cm1,var]==False,AA=False;Break[]]
         ,{i1,n-1}];
         If[AA && (Rank[cm]==m),True,False]];
FeedbackLinearizable[f_?VectorQ,g_?VectorQ,var_List]:=Module[{cm,cm1},
         cm=ControllabilityMat[f,g,var];
         cm1=Drop[cm,-1];   (* drop last element *)
         If[(Rank[cm]==Length[var])&&
               Involutive[cm1,var],True,False]];
SetAttributes[FeedbackLinearizable,ReadProtected];
SetAttributes[FeedbackLinearizable,Locked];



(* *********************************************** *)
(* ******         Relative Order            ****** *)
(* *********************************************** *)

Clear[RelativeOrder,VectorRelativeOrder];
RelativeOrder[f_?VectorQ,g_?MatrixQ,h_List,var_List]:= 
   Module[{k=0},
   While[k<=Length[var],
                If[LieDerivative[g,LieDerivative[f,h,var,k],var,1]!={{0}},
                        (Return[k+1];Break[]),
                        k++,
                        (Return[k+1];Break[])]];
   Return[Infinity]
]
(* g must be of the form {{g1}, {g2}, ... {gn}} where gi is the ith row *)
RelativeOrder[f_?VectorQ,g_?VectorQ,h_,var_List]:=
   RelativeOrder[f, Map[{#}&, g], h, var]
(* h must be a vector *)
RelativeOrder[f_?VectorQ,g_?MatrixQ, h_, var_List]:=
        RelativeOrder[f,g, {h},var]
SetAttributes[RelativeOrder,ReadProtected];
SetAttributes[RelativeOrder,Locked];


RelativeDegree[f_,g_,h_,var_List]:= RelativeOrder[f,g,h,var]


VectorRelativeOrder[f_Function,g_Function,h_Function,x_List]:= VectorRelativeOrder[f[x],g[x],h[x],x]
VectorRelativeOrder[f_?VectorQ,g_?VectorQ,h_,var_List]:=
   VectorRelativeOrder[f, Map[{#}&, g], h, var]
VectorRelativeOrder[f_,g_,h_,x_List]:= Module[{b,c,d,e,rules,ro},
    rules={b->f,c->x,d->Transpose[g],e->h};
        ro=Distribute[RelativeOrder[b,Transpose[g],h,c],List]/.rules; 
        ro=Transpose[Partition[ro,Length[h]]]; 
        Map[Min,ro]
]
SetAttributes[VectorRelativeOrder,ReadProtected];
SetAttributes[VectorRelativeOrder,Locked];


(* ************************************************************** *)
(* ******        Elementary FBL & Transformations          ****** *)
(* ************************************************************** *)


Clear[SIExactFBL];
SIExactFBL[f_,g_,var_,FBLprint_:False]:=Module[{nnn,i1,i2,k1,fff,ggg,trans1,
lambda,Lambda,Lambdasss,GLambda,Trans,sol,PDEs,pde,ZZ,InverseRule,FBLvar,
trans},
(* "FBLprint" are used here to suppress printing certain outputs when
   function SIExactFBL is called from within another function. Set: 
   Set the last (optional) argument to False if you don't want to print
   corresponding outputs. The default is True.    *)
If[FBLCond[f,g,var],    (* This is a long If statement to the end!*)
nnn=Length[f]; fff:=f; ggg:=g; 
(* There are n-1 pde(=0) equations to solve *)
PDEs:=Array[pde,nnn-1];  
Trans=Array[trans,nnn-1];
lambda=u[Apply[##&,var ] ];          (*{x1,x2}->x1,x2 : List-> Sequence *)
(* Beginning of the computations *)
Lambda={};Lambdasss={};
Do[                                  (* Loop for PDE computations: *)
  pde[i1]=Jacob[lambda,var].Ad[fff,ggg,var,i1-1];
  sol=DSolve[ pde[i1] ==0, lambda , var ];
  GLambda=Apply[List,lambda/.sol][[1]];
  Lambdasss=Flatten[Append[Lambdasss,Table[GLambda[[i2]],
                                        {i2,1,Length[GLambda]}]]];   
,{i1,1,nnn-1}];       (*End of Do Loop *)
Do[         (* Loop to check all the solutions and get the final solution *)
  If[TrueQ[Apply[And,Flatten[Table[ 
    Simplify[Jacob[Lambdasss[[i1]],var].Ad[fff,ggg,var,k1-1]]==0 ,{k1,1,nnn-1}
             ]]]],         Lambda=Union[Append[Lambda,Lambdasss[[i1]]]];
     ];   
,{i1,1,Length[Lambdasss]}];                  (* End of Do Loop *)
Print["Linearizing Output Solutions: ", Lambda];
trans1=Lambda[[1]];
(*
Print["\n"];
Print["FBL Transformation = ",trans1 ];
*)
FBLTransformation=Join[{trans1},
               Flatten[Table[LieDerivative[fff,trans1,var,i3],
               {i3,1,nnn-1}]]];
If[FBLprint,
    Print["Exact Feedback Linearizing Transformation z=T(",var,"):"]
   ];
ZZ=Table[ToExpression["z"<>ToString[i1]],{i1,1,nnn}];
If[FBLprint , Do[ Print["         ",ZZ[[i1]]," =  ", FBLTransformation[[i1]]]; 
              ,{i1,1,nnn}] 
];
(*
Print["\n"];
Print["          **Simplifing transformation for the output**\n"];
*)
Return[Simplify[FBLTransformation]]
(* If FB linearization conditions are not satisfied then return:  *)
,Print["\n"];
Print["FBL: System is not feedback linearizable.\n"];
 ]
];
SetAttributes[SIExactFBL,ReadProtected];
SetAttributes[SIExactFBL,Locked];

Clear[InverseTransformation];
InverseTransformation[var_,newvar_,Trans_]:=Module[
{InverseRule,InvTrans},
InverseRule=Solve[Inner[Equal,newvar,Trans,List],var];
If[InverseRule!={},
 InvTrans=var//.InverseRule[[1]]; 
 Print["Inverse Transformation: ",var," =  ", InvTrans];
 Return[Simplify[InvTrans]],
 Print["Inverse Not Found"]
];
];
SetAttributes[InverseTransformation,ReadProtected];
SetAttributes[InverseTransformation,Locked];

Clear[LocalInverseTransformation];
LocalInverseTransformation[var_,newvar_,Trans_]:=LocalInverseTransformation[var,newvar,Trans,4];
LocalInverseTransformation[var_,newvar_,Trans_,order_]:=Module[
{A,NN,v},
A=Jacob[Trans,var]/.Inner[Rule,var,Table[0,{Length[var]}],List];
NN=Simplify[Trans-A.var];
v=0*var;
Do[v=LinearSolve[A,-(NN/.Inner[Rule,var,v,List])+newvar],
  {i,1,order+1}];
Clear[MySeries];
MySeries[FF_,{XX__}]:=Series[FF,XX];
v=MySeries[v,Map[{#,0,order+1}&,newvar]];
Simplify[Normal[v]]
];
SetAttributes[LocalInverseTransformation,ReadProtected];
SetAttributes[LocalInverseTransformation,Locked];

Clear[TransformSystem];
TransformSystem[f_,g_,h_,var_,newvar_,Trans_,InvTrans_]:=Module[
{Newfff,Newggg,Newhhh,InverseRule},
{Newfff,Newggg}=TransformSystem[f,g,var,newvar,Trans,InvTrans];
InverseRule=Inner[Rule,var,InvTrans,List];
Newhhh=Simplify[h/.InverseRule];
{Newfff,Newggg,Newhhh}
];

TransformSystem[f_,g_,var_,newvar_,Trans_,InvTrans_]:=Module[
{nnn,InverseRule,Newfff,Newggg},
nnn=Length[f];
InverseRule=Inner[Rule,var,InvTrans,List];
Newfff:=Table[
   LieDerivative[f,Trans[[i1]],var]//.InverseRule  ,{i1,1,nnn}];
Newggg:=Table[
   LieDerivative[g,Trans[[i1]],var]//.InverseRule  ,{i1,1,nnn}];
Simplify[{Newfff,Newggg}]
];
SetAttributes[TransformSystem,ReadProtected];
SetAttributes[TransformSystem,Locked];

Clear[PartialTransformSystem];
PartialTransformSystem[f_,g_,var_,Trans_,newvar_]:=Module[{nnn,InverseRule,Newfff,Newggg},
nnn=Length[f];
InverseRule=Inner[Rule,Trans,newvar,List];
Newfff:=Table[
   LieDerivative[f,Trans[[i1]],var]//.InverseRule  ,{i1,1,nnn}];
Newggg:=Table[
   LieDerivative[g,Trans[[i1]],var]//.InverseRule  ,{i1,1,nnn}];
{Simplify[Newfff],Simplify[Newggg]}
];
SetAttributes[PartialTransformSystem,ReadProtected];
SetAttributes[PartialTransformSystem,Locked];

Clear[SISONormalFormTrans];
SISONormalFormTrans[f_,g_,h_,var_]:=Module[
  {fff,ggg,hhh,nnn,rrr,xxx,var2xxx,xxx2var,ddd,sol,lambda,
   i1,LambdaSSS,NormalRule,NormalSet,ZDSet,ZDSetNormal,u,pde},
nnn=Length[f];
xxx=Table[ToExpression["x"<>ToString[i1]],{i1,1,nnn}];
var2xxx=Inner[Rule,var,xxx,List];
xxx2var=Inner[Rule,xxx,var,List];
fff=f/.var2xxx;
ggg=g/.var2xxx;
hhh=h/.var2xxx;
rrr=RelativeDegree[fff,ggg,hhh,xxx];
ddd=nnn-rrr;    (* # of ind. fnc.'s needed *)
lambda=u[Apply[##&,xxx ] ];     (*{x1,x2}->x1,x2 : List-> Sequence *)
pde=Table[D[lambda,ToExpression["x"<>ToString[i1] ]],{i1,1,nnn}].ggg;
sol=DSolve[pde==0, lambda,xxx ];
       (* u[x1,x2] ->{x1,x2}:Sequence->List *)
LambdaSSS=Apply[List,Evaluate[lambda/.sol][[1]]];
(* Check and choose the solutions that are independent with the rest of
   normal set of coordinates*)
NormalSet=Table[LieDerivative[fff,hhh,xxx,i1],{i1,0,rrr -1}];
ZDSet={};
Do[
If[Rank[Jacob[Flatten[Union[NormalSet,ZDSet,{LambdaSSS[[i1]]} ]],xxx]
                                    ]==Length[NormalSet]+Length[ZDSet]+1,
ZDSet=Append[ZDSet,LambdaSSS[[i1]]]],{i1,1,Length[LambdaSSS]}];
(* Normalize the new functions to zero at x==0 *)
NormalRule=Inner[Rule,xxx,Table[0,{i1,1,nnn}],List];
ZDSetNormal=Table[ZDSet[[i1]]- (ZDSet[[i1]]/.NormalRule ),
                                        {i1,1,Length[ZDSet]}]; 
(*Return the normal form transformation*)
{Flatten[NormalSet/.xxx2var],ZDSetNormal/.xxx2var}
];
SetAttributes[SISONormalFormTrans,ReadProtected];
SetAttributes[SISONormalFormTrans,Locked];


(* *********************************************** *)
(* ******        Decoupling Matrix          ****** *)
(* *********************************************** *)

Clear[DecouplingMatrix];

DecouplingMatrix[f_Function, g_Function,h_Function, x_List,ro_List]:= 
        DecouplingMatrix[f[x],g[x],h[x],x,ro]
DecouplingMatrix[f_?VectorQ,g_?VectorQ,h_,x_List,ro_List]:=
   DecouplingMatrix[f, Map[{#}&, g], h,x,ro]
DecouplingMatrix[f_,g_,h_,x_List,ro_List]:=Module[{A,i,j,ans},
    A[i_,j_]:=LieDerivative[Map[#[[j]]&,g],
               LieDerivative[f,h[[i]],x,ro[[i]]-1],x];
    ans=Table[A[i,j], {i,1,Length[h]}, {j,1,Length[h]}];
        Clear[A];
        Return[ans]
]
DecouplingMatrix[f_,g_,h_,x_List]:=Module[{ro,ans},
   ro=VectorRelativeOrder[f,g,h,x];
   ans=DecouplingMatrix[f,g,h,x,ro];
   Return[ans]
]
SetAttributes[DecouplingMatrix,ReadProtected];
SetAttributes[DecouplingMatrix,Locked];


(* *********************************************** *)
(* ******      Input-Output Linearize       ****** *)
(* *********************************************** *)

Clear[IOLinearize];

IOLinearize[f_,g_,h_,x_]:=Module[
{
NumOutputs,
RelOrder,
EE1,
EE2,            (* decoupling matrix *)
test1,          (* test whether Det[EE2]==0 and finds the regions where it is *)
v,                      (* New inputs *)
control         (* Linearizing/decoupling control law *)
},
(* Init *)
        NumOutputs=Length[h];
        If[VectorQ[g],NumInputs=1,NumInputs=Dimensions[g][[2]]];
        v=Table[{ToExpression["v"<>ToString[i]]}, {i,NumOutputs}];
        (* check that the number of inputs is the same as the number of outputs *)
        If[NumInputs!=NumOutputs,
            Print["Number of inputs is not equal to the number of outputs"];
            Return[],
        (* Compute vector relative order *)
        RelOrder=VectorRelativeOrder[f,g,h,x];
        If[Max[RelOrder]==Infinity,Print["Relative Order is Infinite!"];Return[]];
        (* Compute decoupling matrix *)
        Print["Computing Decoupling Matrix"];
            EE2=DecouplingMatrix[f,g,h,x,RelOrder];
            (* If decoupling matrix is singular report & return *)
           If[Rank[EE2]!=NumInputs,
                Print["Decoupling matrix is singular!"];
                Return[]
           ]; 
           (* If decoupling matrix is generically nonsingular 
           test to see where it may be singular *)
       (*
       Print["Locating Singular Points of decoupling matrix"];
       test1=Reduce[Det[EE2]==0];
           *)
          
       (* Compute the vector of Lie derivatives *)
       Print["Computing linearizing/decoupling control"];
           EE1=Table[LieDerivative[f, h[[i]], x,RelOrder[[i]]],
                        {i,1,NumOutputs}, {j,1,1}];
       (*Compute control *)
           control=MyInverse[EE2].(-EE1+v);
   ];
{EE2,Flatten[EE1],RelOrder,Flatten[control]}
]
SetAttributes[IOLinearize,ReadProtected];
SetAttributes[IOLinearize,Locked];

(* ******************************************************* *)
(* ******           Local Decomposition             ****** *)
(* ******************************************************* *)
Clear[LocalDecomposition];
LocalDecomposition[f_?VectorQ,G_?MatrixQ,h_?VectorQ,var_?VectorQ,u_?VectorQ,
    m_]:=Module[{DelC,DelCbar,DelO,DelCObar,DelCO,DelCbarObar,DelCbarO,Del,
      Psi,Phi,ff,hh},
		DelC=SmallestInvariantDistribution[Join[{f},Transpose[G]],Transpose[G],
        var];
		DelCbar=ComplementDistribution[IdentityMatrix[Length[f]],DelC];
		If[DelCbar=={},Print["System is locally controllable"]];
		DelO=LargestInvariantDistribution[Join[{f},Transpose[G]],h,var];
		If[DelO=={},Print["System is locally observable"]];
		DelCObar=IntersectionDistribution[DelC,DelO];
		DelCO=ComplementDistribution[DelC,DelCObar];
		DelCbarObar=IntersectionDistribution[DelCbar,DelO];
		DelCbarO=ComplementDistribution[DelCbar,DelCbarObar];
		Del=Join[DelCObar,DelCO,DelCbarObar,DelCbarO];
		{Psi,Phi,ff,hh}=TriangularDecomposition[f+G.u,h,Del,var,0*var,m];
		{Psi,Phi,ff/.Inner[Rule,u,0*u,List],Transpose[Map[Coefficient[ff,#]&,u]],
      hh}
];
SetAttributes[LocalDecomposition,ReadProtected];
SetAttributes[LocalDecomposition,Protected];
SetAttributes[LocalDecomposition,Locked];

(* *********************************************** *)
(* ******           Normal Form             ****** *)
(* *********************************************** *)

Clear[NormalCoordinates];
NormalCoordinates[f_,g_,h_,x_List,Vro_List]:=NormalCoordinates[f,h,x,Vro];
NormalCoordinates[f_,h_,x_List,Vro_List]:=
  Module[{ro,z,NumOutputs,ZZ},
  NumOutputs=Length[h];
  ZZ={};
  Do[
    ro=Vro[[j]];
    z=Table[LieDerivative[f,h[[j]],x,i],{i,0,ro-1}];
    ZZ=Join[ZZ,z],
  {j,1,NumOutputs}];
  Return[ZZ]
]
SetAttributes[NormalCoordinates,ReadProtected];
SetAttributes[NormalCoordinates,Locked];
  

(* *********************************************** *)
(* ******       Local Zero Dynamics         ****** *)
(* *********************************************** *)

Clear[LocalZeroDynamics];
LocalZeroDynamics[f_?VectorQ,g_?VectorQ,h_,x_List,uo_,z_]:=
   LocalZeroDynamics[f, Map[{#}&, g], h,x,uo,z]
LocalZeroDynamics[f_?VectorQ,g_?VectorQ,h_,x_List,uo_,z_,order_]:=
   LocalZeroDynamics[f, Map[{#}&, g], h,x,uo,z,order]
LocalZeroDynamics[f_,g_,h_,x_,u0_,z_]:=LocalZeroDynamics[f,g,h,x,u0,z,4]
LocalZeroDynamics[f_,g_,h_,x_,u0_,z_,order_]:=Module[{A,NN,
  Astar,NNstar,K,Kstar,v,w,vstar,xstar,wdot,XX,FF},
  If[Length[z]==Length[x],Print["The system is completely linearizable."]; 
                          Print["There are no zero dynamics."];
                          ans={}
  ,
  A=Jacob[z,x]/.Inner[Rule,x,Table[0,{Length[x]}],List];
  NN=Simplify[z-A.x];
  Astar=RightInverse[A];
  K=Transpose[NullSpace[A]];
  v=Table[ToExpression[StringJoin["v",ToString[i]]],{i,Length[z]}];
  w=Table[ToExpression[StringJoin["w",ToString[i]]],{i,Length[x]-Length[z]}];
  xnew=Astar.v+K.w;
  NNstar=NN/.Inner[Rule,x,xnew,List];
  vstar=0*v;
  Do[vnew=-NNstar/.Inner[Rule,v,vstar,List];
     vstar=vnew,
  {i,1,order+1}];
  (*
  zeval=vstar+NNstar/.Inner[Rule,v,vstar,List];
  Series[zeval,Flatten[Map[{#,0,4}&,w],1]];
  *)
  Clear[MySeries];
  MySeries[FF_,{XX__}]:=Series[FF,XX];
  vstar=MySeries[vstar,Map[{#,0,order+1}&,w]];
  xstar=Astar.vstar+K.w;
  Kstar=LeftInverse[K];
  f0=f+g.u0;
  wdot=Kstar.(f0/.Inner[Rule,x,xstar,List]);
  wdot=MySeries[wdot,Map[{#,0,order}&,w]];
  Clear[MySeries];
  ans=Normal[wdot]
  ];
  Return[ans]
  ]
SetAttributes[LocalZeroDynamics,ReadProtected];
SetAttributes[LocalZeroDynamics,Locked];



(****************************************************************)
(*                      DynamicExtension                        *)
(****************************************************************)
Clear[DynamicExtension];
Options[DynamicExtension]:={ScreenOutput->True}
DynamicExtension[f_List, g_List,h_List,x_List]:=
Module[
{Opts,
EE2,
RelOrder,
f1,
g1,
h1,
x1
},
           Opts={ScreenOutput}/.Options[DynamicExtension];
           RelOrder=VectorRelativeOrder[f,g,h,x];
           EE2=DecouplingMatrix[f,g,h,x,RelOrder];

           If[Det[EE2]==0,
              {EE2,f1,g1,x1,NewVarNum,RelOrder,Tfinal,test2}=
                        DynamicExtension[EE2,f,g,h,x,RelOrder];
                  If[test2,Print["Dynamic extension completed"];
                        If[Opts,
                                Print["Decoupling matrix  = ",EE2];
                                Print[" New f  = ", f1];
                                Print[" New g  = ", g1];
                                Print[" New x  = ", x1]
                        ];
                        ans={f1,g1,h,x1}
                        ,
            If[Opts,
                        Print["Dynamic Extension unsuccessful"]
                        ];
                        ans={}
                  ];
             ,
             Print["Dynamic Extension not required"];
             ans={}
             ,
             Print["Dynamic Extension not required"];
             ans={}
        ];

Return[ans]
        
];


DynamicExtension[EE_, f_, g_,h_,x_List,ro_List]:=Module[
{f1=f,      (* f1,g1,h1,x1 are temp versions of f,g,h,x *)
x1=x,
g1=g,
h1=h,
E1=EE,      (* decoupling matrix *)
RelOrder=ro,
NumStates=Length[x],
NumOutputs=Dimensions[EE][[1]],
k=Rank[EE],
Transformation,   (* puts the EE into a form of k lin indep columns followed zero columns *)
FinalTransf,      (* Transformation after dynamic extension has been applied the right number of times *)
limit,            (* limits the number of times dynamic extension can be applied *)
NewRankE,         (* Rank of EE after dynaimc extension *)
NewVarNum=0,      (* Number of states added by dynamic extension *) 
count=0           (* number of times dynamic extension has been applied *)
},
                (*Print["k=  ", k];
        Print["n=  ", NumStates];*)

        FinalTransf=IdentityMatrix[NumOutputs];
        limit=NumStates-(Apply[Plus,Take[RelOrder,k]])-Min[Take[RelOrder,-(NumOutputs-k)]];
        While[Det[E1]==0,
                If[count>limit,
                
                        Return[{EE,f,g,x,NewVarNum,RelOrder,1,False}],
                        
                        count++ ;
                        Transformation=GaussElimColumns[E1]; 
                        {x1,f1,g1,NewVarNum}=
                                        DExtend[E1,Transformation,f1,g1,x1,NewVarNum];
                        FinalTransf=FinalTransf.Transformation;                         
                        RelOrder=VectorRelativeOrder[f1,g1,h1,x1];
                                (*Print["RO ", RelOrder];
                                Print["f1   ", f1//TableForm];
                                Print["g1   ", g1//MatrixForm];
                                Print["h   ", h];
                                Print["x1   ", x1]; *)
                        E1=DecouplingMatrix[f1,g1,h,x1];
                                (*Print[" E1  " ,E1//MatrixForm];*)
                        NewRankE=Rank[E1];
                        If[NewRankE1!=k, 
                                limit=NumStates-Apply[Plus,Take[RelOrder,k]]-Min[Take[RelOrder,-(NumOutputs-k)]];
                                k=NewRankE;
                                count=0
                        ]

                ]
        ];
        Return[{E1,f1,g1,x1,NewVarNum,RelOrder,FinalTransf,True}]
]
SetAttributes[DynamicExtension,ReadProtected];
SetAttributes[DynamicExtension,Locked];



(****************************************************************)
(*                      DExtend                                 *)
(****************************************************************)

Clear[DExtend]
DExtend[E_?MatrixQ, T_?MatrixQ, f_, g_,x_List,NewVarNum_]:=Module[
{NumStates=Length[x],
NumOutputs=Dimensions[g][[2]],
k=Rank[E],      
beta1,          (* [beta1 beta2] = [g1 g2].T *)
beta2,
newStates,      (* states to be added to x *)
NewX,
NewF,
NewBeta1,
NewBeta2,
NewG
}, 
    beta1=Transpose[Take[Transpose[g.T],k]];
                (* Print["beta1 =  ",beta1]; *)
        beta2=Transpose[Take[Transpose[g.T],-(NumOutputs-k)]];
        newStates=Map[ToExpression["z"<>ToString[#]]&,Range[NewVarNum+1,NewVarNum+k]];
        NewX=Join[x,newStates];
        NewF=Flatten[{f+beta1.newStates,Table[0,{k}]}];
        NewBeta1=Partition[Flatten[Fold[Append,Table[0,{NumStates},{k}],
                                                        IdentityMatrix[k]]],k];
        NewBeta2=Partition[Flatten[Fold[Append,beta2,
                                                   Table[0,{k},{NumOutputs-k}]]],{NumOutputs-k}];
        NewG=Partition[Flatten[Thread[Join[{NewBeta1,NewBeta2}]]],NumOutputs];
        {NewX,NewF,NewG,NewVarNum+k}
]
SetAttributes[DExtend,ReadProtected];
SetAttributes[DExtend,Locked];



(****************************************************************)
(*                              StructureAlgorithm                              *)
(****************************************************************)

Clear[StructureAlgorithm]
Options[StructureAlgorithm]={ScreenOutput->False};
StructureAlgorithm[f_List,g_List,h_List,x_List,t_]:=Module[
{A,     
 B,
 c,
 y,
 yprime,
 u,
 
 RelOrder,              
 NumStates,
 NumOutputs,
 NumInputs,
F,              (* F[k] used in making dep rows zero*)
G,
E1,             (* E1[k] reorders the rows of DD[k] with indep rows on top *)
E2,             (* E2[k] makes the dep rows of E1.DD zero *)
R,              (* R[k] = E2[k] . E1[k] *)
r,              (* r[k] = Rank of DD[k]  *)
z,               
zbar,           (* zbar[k] -> Elem's of z[k] corresp. to the indep. rows of DD[k]*)
zhat,           (* zhat[k] -> Elem's of z[k] corresp. to the dep. rows of DD[k]*)
cc,              
cbar,           (* cbar[k] -> Elem's of cc[k] corresp. to the indep. rows DD[k] *)
chat,           (* chat[k] -> Elem's of cc[k] corresp. to the dep. rows DD[k] *)
DD,              
Dbar,           (* Dbar[k] -> Unordered and unreduced DD[k+1] *)
K,              (* Describes the relationship between z[k] and y and its deriv's *)
H,              (* H[k] -> rows of K[k] corresp. to indep. rows of DD[k] *)
J,              (* J[k] -> rows of K[k] corresp. to dep. rows of DD[k]   *)
Y,
test1,          (* checks ??? *)
Ahat,           (* parts of the inverse system *)
Bhat, 
Dhat,
ans,
Opts
},
        
(* Set options values *)
    Opts={ScreenOutput}/.Options[StructureAlgorithm];

(*Initialize System*)
A=f;    
B=g;
c=h;
NumStates=Length[x];
NumOutputs=Length[h];
NumInputs=Length[g];
RelOrder=1;

(* Create {y1[t],...,yl[t]} *)
        y=ToExpression[Map["y"<>ToString[#]<>"[t]"&, Range[1,NumOutputs]]];
        yprime=D[y,t];
        
    (* Define internal functions *)

        F[k_]:=F[k]=MakeDepRowsZero[E1[k], Dbar[k],x][[2]];
        E1[k_]:=E1[k]=ReorderRows[Dbar[k],x][[1]];
        E2[k_]:=E2[k]=MakeDepRowsZero[E1[k], Dbar[k],x][[1]];

        R[k_]:=R[k]=E2[k].E1[k]; 

        r[0]:=0;
        r[k_]:=r[k]=Rank[DD[k]];
        
        z[0]:=y;
        z[k_]:=z[k]=R[k-1].Join[zbar[k-1],D[zhat[k-1],t]];

        zbar[k_]:=zbar[k]=Take[z[k],r[k]];
        zhat[k_]:=zhat[k]=Take[z[k],-(NumOutputs-r[k])];

        cc[0]:=c;
        cc[k_]:=cc[k]=R[k-1].Join[cbar[k-1],
                LieDerivative[A,chat[k-1],x]];
        
        cbar[k_]:=cbar[k]=Take[cc[k], r[k]];
        chat[k_]:=chat[k]=Take[cc[k], -(NumOutputs-r[k])];

        DD[0]:=Chop[LieDerivative[B,c,x]];
        DD[k_]:= DD[k]=Chop[R[k-1].Dbar[k-1]];

        Dbar[0]:=DD[0];
        Dbar[k_]:=Dbar[k]=Module[{
                Dk1,
                Dhat
                },
        Dk1=Take[DD[k],r[k]];
        Dhat=LieDerivative[B,chat[k],x];
        Join[Dk1, Dhat-G[k]]
        ];
        
 
        H[k_]:= H[k]=Take[K[k], r[k]];
        J[k_]:= J[k]=Take[K[k], -(NumOutputs-r[k])];

        K[1]:= R[0];
        K[k_]:=K[k]=Module[
                {zeros,
                 zeros2,
                 KKpart1,
                 KKpart2},
                zeros=Table[0,{i,r[k-1]},{j,NumOutputs}];
                zeros2=Table[0,{i,NumOutputs-r[k-1]},{j,NumOutputs}]; 

                KKpart1=Thread[aaa[H[k-1],zeros]]/.aaa->Join;
                (*If[zeros=={},
                        KKpart2=(Thread[LieDerivative[aaa,J[k-1],
                                bbb]]/.{aaa->A,bbb->x})+ J[k-1],*)

                KKpart2=(Thread[ccc[Thread[LieDerivative[aaa,J[k-1],
                        bbb]]/.{aaa->A,bbb->x},zeros2]]/.{ccc->Join})+
                        (Thread[ccc[zeros2,J[k-1]]]/.{ccc->Join});
         
                R[k-1].Join[KKpart1, KKpart2]
        ];
        
        Y[1]:=y;
        Y[k_]:=Y[k]=Join[Y[k-1], D[y,{t,k}]];
        
        G[k_]:=G[k]=LieDerivative[B,zhat[k],x];
        
(* Compute relative order *)
        If[Opts[[1]],Print["Running StructureAlgorithm"]];
        
        While[r[RelOrder]<NumOutputs && RelOrder<=NumStates,
                ((* Print["relOrder = ",RelOrder]; *)RelOrder++)];
 
(* Output *)
    (* Simplify answers *)
        DD[RelOrder]=Simplify[Expand[DD[RelOrder],Trig->True]];
        cc[RelOrder]=Simplify[Expand[cc[RelOrder],Trig->True]];
        K[RelOrder]=Simplify[Expand[K[RelOrder],Trig->True]];
    
    (* Print information *)  
        If[r[RelOrder]==NumOutputs, 
                If[Opts[[1]],
                        Print["relative order= ",RelOrder,"\n"];
                        Print["D=  ", DD[RelOrder]//MatrixForm];
                        Print["\n\n"];
                        Print["c=  ", cc[RelOrder]//MatrixForm];
                        Print["\n\n"];
                        Print["H=  ", K[RelOrder]//MatrixForm];
                        Print["\n\n"];
                        If[Dimensions[DD[RelOrder]][[1]]==Dimensions[DD[RelOrder]][[2]],
                                Print["Inverse system\n
                                        xdot=Ahat + Bhat.uhat\n
                                        yhat=chat + Dhat.uhat\n"];
                                DDinv=Inverse[DD[RelOrder]];
                                Ahat=A-B.DDinv.cc[RelOrder];
                                Bhat=B.DDinv.K[RelOrder];
                                chat=-DDinv.cc[RelOrder];
                                Dhat=DDinv.K[RelOrder];
                                Print["where,\n\n"];
                                Print["Ahat=   ",Ahat//MatrixForm];
                                Print["\n\n"];
                                Print["Bhat=   ",Bhat//MatrixForm];
                                Print["\n\n"];
                                Print["chat=   ",chat//MatrixForm];
                                Print["\n\n"];
                                Print["Dhat=   ",Dhat//MatrixForm];
                                Print["\n\n"]
                        ];
        
                ];
                ans={DD[RelOrder], 
                         cc[RelOrder], 
                         K[RelOrder], 
                         z[RelOrder]}
        ,
        Print["StructureAlgorithm algorithm failed"];
        ans={},
        Print["StructureAlgorithm algorithm failed"];
        ans={}
        ];

          (* Remove internal functions *)
                 Clear[F,E1,E2,R, r, z,zbar,zhat,cc,cbar,chat,DD,Dbar,H,J,K,G,Y];   

          (* Return answer *)
                Return[ans];     
]
SetAttributes[StructureAlgorithm,ReadProtected];
SetAttributes[StructureAlgorithm,Locked];

(****************************************************************)
(*                      ExponentialObserver                     *)
(****************************************************************)

Clear[ExponentialObserver];
ExponentialObserver[f_,h_,x_,u_,y_,x0_,u0_,decay_,opt___]:=Module[
  {rhs,xhat,staterule,staterule2,controlrule,AA,CC,LL,PP,eigs,
   nn=Length[f],
   G,Q,R,Estimate},
   G=0*IdentityMatrix[nn];
   Q=0*IdentityMatrix[nn];
   R=IdentityMatrix[Length[h]];
   If[nn!=Length[x],
      Print["Incompatible number of state variables and state equations."];
      Return[]];
   If[nn!=Length[x],
      Print["Incompatible number of output variables and output equations."];
      Return[]];
   If[Length[x0]!=Length[x]||Length[u0]!=Length[u],
      Print["Incompatible nominal values."];Return[]];
   If[opt===Null,
      xhat=ToExpression[Map[StringJoin[ToString[#],"hat"]&,x]],
      xhat=ToExpression[Map[StringJoin[ToString[opt],ToString[#]]&,Range[Length[x]]]]
   ];
   staterule=Inner[Rule,x,x0,List];
   staterule2=Inner[Rule,x,xhat,List];
   controlrule=Inner[Rule,u,u0,List];
   AA=Jacob[f,x]/.controlrule/.staterule;
   CC=Jacob[h,x]/.controlrule/.staterule;
   If[!ObservablePair[AA,CC],Print["Exponential observer may not exist"];Return[]];
   {LL,PP,eigs}=LQE[AA+decay*IdentityMatrix[nn],G,CC,Q,R];
   rhs=(f/.staterule2)-LL.(y-(h/.staterule2));
   {rhs,xhat,Map[(#-decay)&,eigs]}
 ]
SetAttributes[ExponentialObserver,ReadProtected];
SetAttributes[ExponentialObserver,Locked];
               

(****************************************************************)
(*                      AdaptiveRegulator                        *)
(****************************************************************)


Clear[AdaptiveRegulator];
AdaptiveRegulator[f_,g_,h_,x_,t_,theta_,AdGain_,poles_List]:=  Module[
{Rho,RhoBar,Rho1, Alpha,AlphaBar,Alpha1, RelOrder, Control,NumOutputs,NumInputs,v,z,UU,Poles,derlist,
PPBlock,PP,RR,ControlRule,LinearControlRule,K,ThetaBar1,ThetaBar2,W2,unc,EE,Q},
(* Apply IOLinearize *)
   {Rho, Alpha, RelOrder, Control}=IOLinearize[f,g,h,x];
   Print["Finished Linearizing Control"];
   z=Table[ToExpression["z"<>ToString[i]], {i,Apply[Plus,RelOrder]}];
   (*
   z=NormalCoordinates[f,g,h,x,RelOrder]; 
   *)
   NumOutputs=Length[h];
   If[VectorQ[g],NumInputs=1,NumInputs=Dimensions[g][[2]]];
   v=Table[ToExpression["v"<>ToString[i]], {i,NumInputs}];
(* Obtain Stabilizing Feedback - pole placement *)
   Poles=Map[Drop[Poly[#],-1]&, poles];
   
   (* Form linear part of control, v=Kz(x) *)
   K=Table[-Poles[[i1]],{i1,1,NumInputs}];
   LinearControlRule=Inner[Rule,v,K.z,List];
      
   (* Assemble Closed Loop Matrix, PP *)
           derlist=RelOrder;
           (*
           PPBlock=
                BlockMatrix[ 
                   Table[               (*define diagonal blocks in companion*)
                      If[               (*form and all other blocks are zero *)                 
                         i1==j1,
                        If[derlist[[i1]]==1,
                             PP[i1,j1]={-Poles[[i1]]},
                
                             PP[i1,j1]=AppendColumns[
                                          AppendRows[
                                             ZeroMatrix[derlist[[i1]]-1,1],
                                             IdentityMatrix[derlist[[i1]]-1]
                                          ],{-Poles[[i1]]}
                                        ]
                           ],
                             PP[i1,j1]=ZeroMatrix[derlist[[i1]],derlist[[j1]]]
                         ],{i1,1,NumOutputs},{j1,1,NumOutputs}
                      ]
               ];
               *)

               PP =Table[               (*define diagonal blocks in companion*)
                                        (*form and all other blocks are zero *)                 
                        If[derlist[[i1]]==1,
                             PP[i1]={-Poles[[i1]]},
                
                             PP[i1]=AppendColumns[
                                          AppendRows[
                                             ZeroMatrix[derlist[[i1]]-1,1],
                                             IdentityMatrix[derlist[[i1]]-1]
                                          ],{-Poles[[i1]]}
                                        ]
                           ],{i1,1,NumOutputs}
                      ];
(* Apply Separate to alha and rho *)
   {ThetaBar2,{RhoBar,AlphaBar}}=
                 Separate[{Rho, Alpha},theta];
         Print["New Parameters =  ", ThetaBar2];
         unc=Length[ThetaBar2];                  (* Number of uncertainties *)
         (*Construct a vector ThetaBar1={thetahat1,<, thetahatk} *)
         ThetaBar1=Table[  
                   ToExpression[
                        "thetahat"<>ToString[i1]
                   ],{i1,1,Length[ThetaBar2]}
                 ];
(* Recompute Linearizing control with new parameterization *) 
   Control=(MyInverse[RhoBar].(-AlphaBar+v))/.LinearControlRule;
   Print["Finished Stabilizing Control"];
(* Obtain regressor *)
   UU=Table[ToExpression["u"<>ToString[i1]],{i1,1,NumInputs}];
  (*separate Alpha into Alpha1 and Alpha0: Alpha=Alpha1.ThetaBar1+Alpha0*)
          Alpha1=Transpose[
                     Map[
                        Coefficient[Expand[AlphaBar],#]&,ThetaBar1
                     ]
                  ];
          
  (*sep. Rho into Rho1 and Rho0: Rho=Rho1.ThetaBar1+Rho0*)
          Rho1=Transpose[
                       Map[
                             Coefficient[Expand[RhoBar.UU],#]&,ThetaBar1
                           ]
                   ];
          
  (* compute regressor PSi *)
   Psi=Alpha1+Rho1;
   Print["Finished regressor computation"];
(* Assemble E Matrix *)
   W2=IdentityMatrix[Length[derlist]];
   EE=BlockMatrix[
          Table[                       
             If[derlist[[i1]]==1,
                 {{W2[[i1]]}},
                 {AppendColumns[
                       ZeroMatrix[derlist[[i1]]-1,Length[derlist]],
                       {W2[[i1]]}
                ]}
               ],{i1,1,Length[derlist]}
          ]
        ];
      
(* Solve Liapunov Equation *)

   gamma=1000;
   Q=Map[gamma*IdentityMatrix[#]&,derlist];
   RR=LiapunovEquation[PP,Q,derlist];
   Print["Finished Liapunov Equation"];
(* Assemble update law *)
(* Omega  is  the  identifier gain  matrix*)
     (* Case 1 AdGain is a number *)
        If[NumberQ[AdGain],
                Omega=AdGain*IdentityMatrix[unc],
     (* Case 2 Adgain is a list of the right length *)
                If[ListQ[AdGain]&&Length[AdGain]==unc,
                        Omega=DiagonalMatrix[AdGain],
     (* Case 3 AdGain is a scalar variable *)
                        If[ListQ[AdGain]==False,
                              Omega=AdGain*IdentityMatrix[unc],
     (* Case 4 AdGain is something else *)
                              Omega=DiagonalMatrix[Table["AdaptGain"<>ToString[i], {i, unc}]]
                        ]
                ]
        ];
   ControlRule=Inner[Rule,UU,Control,List];
   dthetahat=(Omega.Transpose[Psi].Transpose[EE].RR.z)/.ControlRule;
   Print["Finished parameter update law"];
(* Output  *)
        
            ans1={ThetaBar2,
                  ThetaBar1,
                  Flatten[dthetahat],
                  Control};
Return[ans1]
]
SetAttributes[AdaptiveRegulator,ReadProtected];
SetAttributes[AdaptiveRegulator,Locked];

Clear[LiapunovEquation];
LiapunovEquation[P_,Q_,n_Integer]:=Module[{RR},
(* find RR: solution to the Liapunov eq. RR.P+Transpose[P].RR=-Q *)
(* Brute force solve *)                   
                        (* Setup RR *)      
                RR=Table[ rr[i1,j1], {i1,1,n},{j1,1,n}];
                        (* Solve for RR *)
                RR=RR/.Flatten[
                            Solve[
                                RR.P+Transpose[P].RR==-Q
                             ]
                         ];
                Return[RR]
]
LiapunovEquation[PP_,Q_,derlist_List]:=Module[{RR,rr,RRSolution},
(* solve Liapunov equation block by block *)
                Map[    (RRtemp[#,#]=Table[rr[i1,j1],{i1, derlist[[#]]}, {j1,derlist[[#]]}];
                         RRSolution=Solve[RRtemp[#,#].PP[[#]]+Transpose[PP[[#]]].RRtemp[#,#]==-Q[[#]]];
                         RRtemp[#,#]=N[RRtemp[#,#]/.Flatten[RRSolution]])&
                        ,
                     Range[Length[derlist]]];

                RR=BlockMatrix[Table[If[i1==j1, RRtemp[i1,j1], ZeroMatrix[derlist[[i1]],derlist[[j1]]]], 
                                {i1,Length[derlist]}, 
                                {j1,Length[derlist]}]];
                Return[RR]
]
SetAttributes[LiapunovEquation,ReadProtected];
SetAttributes[LiapunovEquation,Locked];


(****************************************************************)
(*                      AdaptiveTracking                        *)
(****************************************************************)


Clear[AdaptiveTracking];
Options[AdaptiveTracking]={ScreenOutput->False};
(* If ScreenOutput->False results will not be printed to the screen 
*)
(* AdaptiveTracking needs BlockMatrix and 
                   ZeroMatrix from LinearAlgebra/MatrixManipulation.m
                   StructureAlgorithm
                   HToNAndM
                   Separate *)

(*AdGain is the adaptive gain for the parameter update law, (pp1,pp2)
are two desired poles: (s+pp1)*(s+pp2)^(#-1), RefSig is the desired
reference signal    *)
AdaptiveTracking[f_,g_,h_,x_,t_,theta_,RefSig_,AdGain_,poles_List]:=  Module[
{Opts,                  (* options *)
m,                      (* number of inputs *)
l,                      (* number of outputs *)
YD,                     (* desired trajectory *) 
YY,
UU,
Cbeta,
DDbeta,
Hbeta,
NMat,
MMat,
yN,
yM,
derlist,
Poles={},
Q,
KK,
kk1,
kk2,
DDbetaInv,
ThetaBar2,
ThetaBar1,
DDbetaBar,
CbataBar,
NMatBar,
MMatBar,
Cbeta1,
Cbeta0,
DDbeta1,
DDbeta0,
NMat1,
NMat0,
MMat1,
MMat0,
W2,
WW,
PPBlock,
PP, 
DerSum,
RR,
rr
},
    
    
(*Initialize*)
    (*Status - options, Internal->True if AdaptiveTracking is used inside another function*)
        Opts={ScreenOutput}/.Options[AdaptiveTracking];
        m=Length[Transpose[g]];                 (*Number of inputs*)
        l=Length[h];                            (*Number of outputs*)
        
    
    (*Make output vector YY={y1[t],<,yl[t]} and desired output vector 
                YD={yd1[t],<,ydl[t]} *)
        YD=Table[ToExpression["yd"<>ToString[i1]<>"[t]"],{i1,1,l}];
        YY=Table[ToExpression["y"<>ToString[i1]<>"[t]"],{i1,1,l}];
        UU=Table[ToExpression["u"<>ToString[i1]<>"[t]"],{i1,1,m}]; 
         
    (*Apply structure algorithm*)
        
    {DDbeta,Cbeta,Hbeta,zbeta}=StructureAlgorithm[f,g,h,x,t];
        Cbeta=Expand[Cbeta];
        DDbeta=Expand[DDbeta];
        Hbeta=Expand[Hbeta];
        Print["Finished StructureAlgorithm"];
        
    (*Separate H into M and N*)
    
        {NMat,MMat,yN,yM,derlist}= HToNAndM[Hbeta,l,t];
        
        (* Place poles *)
        
        Poles=Map[Drop[Poly[#],-1]&, poles];
        If[Opts[[1]], Print["Placed Poles"]];
            
    (*Q default is the identity matrix*)

        gamma=1000;
        Q=Map[gamma*IdentityMatrix[#]&,derlist];
    
    (*The notation follows Ghanadan and Blanknship *)

    (*Define KK*)                                       (*******************)
        kk1=D[YD,{t,derlist}];                          (*   list of y[t]  *)
                                                        (***********************************)
                                                        (*                   (j)  (j)      *)
        kk2=Table[(Poles[[i1]]*Table[                   (*   Plus sum of  p(yd   -y   )    *)           
                    D[                                  (*              ij                 *)
                                                        (***********************************)
                      YD[[i1]]-YY[[i1]],{t,j1}
                    ],{j1,0,derlist[[i1]]-1}])/.List->Plus ,{i1,1,l}
                 ];
        KK=kk1+kk2;

        YDRule=Inner[   Rule,
                        Flatten[Table[D[YD[[i1]],{t,j1}],{i1,1,l},{j1,0,derlist[[i1]]}]],
                        Flatten[Table[D[RefSig[[i1]],{t,j1}],{i1,1,l},{j1,0,derlist[[i1]]}]],
                        List
                    ];
   
    (*Find pseudoinverse of DDbeta*) 
           (*  DDbetaInv=Transpose[DDbeta].MyInverse[DDbeta.Transpose[DDbeta]];*)
           (*  Print["Computed DDbetaInv"];       *)   

    (*Compute parameter update law*)
                
        (*Find ThetaBar*)  
        (* Separate[A,theta,var] finds functions of elements of theta which             
                  appear as factors of elements of A *)
    (*      
            ThetaBar2=Union[ Separate[DDbeta,theta,x],
                             Separate[Cbeta,theta,x], 
                             Separate[NMat.yN,theta,x],
                             Separate[MMat.yM,theta,x]
                                   ];
        (*set unc after calling Separate.m to let unc==Length[ThetaBar2]*)
        ThetaBar2=ThetaBar2/. 1.->1;            
        ThetaBar2=Union[ThetaBar2];             (* Eliminate redundant entries *)
        unc=Length[ThetaBar2];                  (* Number of uncertainties *)
        Print["Thetabar2 =  ", ThetaBar2];
         
         (*Write a substitution rule which will replace appropriate fn. of theta     
         by thetabarj.  Then apply the new substitution rule to everything to 
         make it linear in thetabar*)
         ruleBar=Inner[Rule,ThetaBar2,ThetaBar1,List];
         DDbetaBar=DDbeta/.ruleBar;
         CbetaBar=Cbeta/.ruleBar;
         NMatBar=NMat/.ruleBar;
         MMatBar=MMat/.ruleBar;
   *)
         {ThetaBar2,{DDbetaBar,CbetaBar,NMatBar,MMatBar}}=
                 Separate[{DDbeta,Cbeta,NMat,MMat},theta];
         Print["Thetabar2 =  ", ThetaBar2];
         unc=Length[ThetaBar2];                  (* Number of uncertainties *)
         (*Construct a vector ThetaBar1={thetabar1,<, thetabark} *)
         ThetaBar1=Table[  
                   ToExpression[
                        "thetahat"<>ToString[i1]
                   ],{i1,1,Length[ThetaBar2]}
                 ];
   (* Omega  is  the  identifier gain  matrix*)
     (* Case 1 AdGain is a number *)
        If[NumberQ[AdGain],
                Omega=AdGain*IdentityMatrix[unc],
     (* Case 2 Adgain is a list of the right length *)
                If[ListQ[AdGain]&&Length[AdGain]==unc,
                        Omega=DiagonalMatrix[AdGain],
     (* Case 3 AdGain is a scalar variable *)
                        If[ListQ[AdGain]==False,
                              Omega=AdGain*IdentityMatrix[unc],
     (* Case 4 AdGain is something else *)
                              Omega=DiagonalMatrix[Table["AdaptGain"<>ToString[i], {i, unc}]]
                        ]
                ]
        ];
                   
   (* Print[Omega]; *)
  
            
    (*separate Cbeta into Cbeta1 and Cbeta0: Cbeta=Cbeta1.ThetaBar1+Cbeta0*)
          Cbeta1=Transpose[
                     Map[
                        Coefficient[Expand[CbetaBar],#]&,ThetaBar1
                     ]
                  ];
          Cbeta0=Expand[CbetaBar-Cbeta1.ThetaBar1,Trig->True];
           
   
     (*sep. DDbeta into DDbeta1 and DDbeta0: DDbeta=DDbeta1.ThetaBar1+DDbeta0*)
          DDbeta1=Transpose[
                       Map[
                             Coefficient[Expand[DDbetaBar.UU],#]&,ThetaBar1
                           ]
                   ];
          DDbeta0=Simplify[DDbetaBar.UU-DDbeta1.ThetaBar1];
                                                                                                        
     (*sep. NMat into NMat1 and NMat0: NMat=NMat1.ThetaBar1+NMat0*)
          NMat1=Transpose[
                       Map[
                             Coefficient[Expand[NMat.yN],#]&,ThetaBar1
                           ]
                   ];
          NMat0=NMat.yN-NMat1.ThetaBar1;
                         
     (*sep. MMat into MMat1 and MMat0: MMat=MMat1.ThetaBar1+MMat0*)
          If[MMat=={},
            MMat0=0; MMat1=0,
            MMat1=Transpose[
                       Map[
                             Coefficient[Expand[MMat.yM],#]&,ThetaBar1
                           ]
                   ];
            MMat0=MMat.yM-MMat1.ThetaBar1;

           ];           

     (*compute W2 and WW={{0,<,0},<,W2[[1]]}*)
        W2=Expand[Inverse[NMat].(Cbeta1+DDbeta1-MMat1-NMat1)];
           
            WW=BlockMatrix[
                  Table[                        (*define blocks of WW*)
                     If[derlist[[i1]]==1,
                          {{W2[[i1]]}},
                          {AppendColumns[
                             ZeroMatrix[derlist[[i1]]-1,Length[ThetaBar1]],
                             {W2[[i1]]}
                          ]}
                       ],{i1,1,l}
                  ]
                ];       
                
        
     (*construct PP*)
           PPBlock=
                BlockMatrix[ 
                   Table[               (*define diagonal blocks in companion*)
                      If[               (*form and all other blocks are zero *)                 
                         i1==j1,
                        If[derlist[[i1]]==1,
                             PP[i1,j1]={-Poles[[i1]]},
                
                             PP[i1,j1]=AppendColumns[
                                          AppendRows[
                                             ZeroMatrix[derlist[[i1]]-1,1],
                                             IdentityMatrix[derlist[[i1]]-1]
                                          ],{-Poles[[i1]]}
                                        ]
                           ],
                             PP[i1,j1]=ZeroMatrix[derlist[[i1]],derlist[[j1]]]
                         ],{i1,1,l},{j1,1,l}
                      ]
               ];
     (* find RR: solution to the Lyapunov eq. RR.PP+Transpose[PP].RR=-Q *)
            (* Brute force solve *)
                (*      DerSum=Apply[Plus,derlist];     
                        (*Write RR*)      
                RR=Table[ rr[i1,j1], {i1,1,DerSum},{j1,1,DerSum}];
                        (*Solve for RR*)
                RR=RR/.Flatten[
                            Solve[
                                RR.PPBlock+Transpose[PPBlock].RR==-Q
                             ]
                         ];             *)

            (* solve Lyapunov equation block by block *)
                Map[    (RRtemp[#,#]=Table[rr[i1,j1],{i1, derlist[[#]]}, {j1,derlist[[#]]}];
                         RRSolution=Solve[RRtemp[#,#].PP[#,#]+Transpose[PP[#,#]].RRtemp[#,#]==-Q[[#]]];
                         RRtemp[#,#]=N[RRtemp[#,#]/.Flatten[RRSolution]])&
                        ,
                     Range[Length[derlist]]];

                RR=BlockMatrix[Table[If[i1==j1, RRtemp[i1,j1], PP[i1,j1]], 
                                {i1,Length[derlist]}, 
                                {j1,Length[derlist]}]];


    (* form the control 
 law *)
        Print["Computing control"];
        controlrule=Inner[Rule,Join[YY,ThetaBar2],Join[h,ThetaBar1],List];
        epsilon=Partition[Flatten[Table[
                                D[YD[[i1]]-YY[[i1]],{t,j1}],
                                        {i1,1,l},{j1,0,derlist[[i1]]-1}
                                ]/.controlrule],1] ;
        control=(Pseudoinverse[DDbetaBar].(
                                -CbetaBar+MMatBar.yM+NMatBar.KK))/.
                                controlrule;
                Print["Computing parameter update law"];
                dthetarule= Inner[Rule,Join[UU,ThetaBar2],Join[control,ThetaBar1],List];
                dThetaBar=-Omega.Transpose[WW].RR.epsilon;
                dThetaBar=Chop[dThetaBar/.dthetarule,10^-9];
                                        
                Print["Substituting reference output into control"];
                control2=control/.YDRule;   
                Print["Substituting reference output into parameter update law"];
                dThetaBar2=dThetaBar/.YDRule;
        
        
(* Output  *)
        
            ans1={ThetaBar2,
                  ThetaBar1,
                  Flatten[dThetaBar2],
                  control2,
                  DDbetaBar,
                  derlist};
                
                If[Opts[[1]],
                        Print["\n\n"];
                        Print["Dbeta  = ", DDbeta//MatrixForm];
                        Print["\n\n"];
                        
                        Print["Cbeta  = ", Cbeta//MatrixForm];
                        Print["\n\n"];
                        
                        Print["N(x,theta)  = ",NMat//MatrixForm];
                        Print["\n\n"];
                        
                        Print["M(x,theta)  = ", MMat];
                        Print["\n\n"];
                        Print["yM  = ", yM];
                        Print["\n\n"];
                        Print["derlist  = ", derlist];
                        
                        Print["W  = ", WW];
                        Print["\n\n"];

                        Print["P  = ", PPBlock];
                        Print["\n\n"];
                        
                        Print["K  = ", KK];
                        Print["\n\n"];
                        
                        Print["R  = ", RR//MatrixForm];
                        Print["\n\n"];
                        
                        
                        Print["ThetaBar = ", ThetaBar2];
                        Print["\n\n"];
                
                        Print["control  = ", control2];
                        Print["\n\n"];
                        
                        Print["dThetaBar2 = ", dThetaBar2];
                        Print["\n\n"]
                ];
         
          Return[ans1]

]
SetAttributes[AdaptiveTracking,ReadProtected];
SetAttributes[AdaptiveTracking,Locked];


(********************************************************************************)
(*                      Adaptive Backstepping Controllers                       *)
(********************************************************************************)


Clear[CoefParameter];
CoefParameter[f_,para_]:=
             Table[Coefficient[f, para[[j1]] ],{j1,1,Length[para]}];
SetAttributes[CoefParameter,ReadProtected];
SetAttributes[CoefParameter,Locked];

Clear[PSFFCond];
PSFFCond[f_,g_,var_,para_]:=Module[{k2,k3,ns,pp,f0,g0,fi,gi,
   PSFFCondition,FBLCondition},
   ns=Length[f];
   pp=Length[para];
   fi=CoefParameter[f,para];
   gi=CoefParameter[g,para]; 
   f0:=f - Transpose[fi].para;
   g0:=g - Transpose[gi].para;

   FBLCondition=FBLCond[f0,g0,var];

   PSFFCondition1=TrueQ[Apply[And,
     Flatten[Table[ Flatten[gi][[k1]]==0 , {k1,1,ns}] ]]];

   PSFFCondition2=TrueQ[Apply[And,
     Flatten[Table[ Rank[ 
       Union[ Dist[f0,g0,var,k2], {Ad[ Ad[f0,g0,var,k2], fi[[k3]] ,var,1  ]} ]
                ]==k2+1 , {k3,1,pp}, {k2,0,ns-2} ] ]
         ]];

   Print["Feedback Linearization Condition = ", FBLCondition ];
   Print["1st Parametric Strict Feedback Condition = ", PSFFCondition1];
   Print["2nd Parametric Strict Feedback Condition = ", PSFFCondition2];
   TrueQ[ FBLCondition && PSFFCondition1 && PSFFCondition2 ]
];
SetAttributes[PSFFCond,ReadProtected];
SetAttributes[PSFFCond,Locked];

Clear[PSFFSolve];
PSFFSolve[f_,g_,var_,para_]:=Module[{PsffTest,ns,pp,i1,f0,g0,fii,gii,
PSFFtr,ZZ},
PsffTest=PSFFCond[f,g,var,para];
If[PsffTest,
Print["Computing Parametric-Strict-Feedback Form\n"];
ns=Length[f];
pp=Length[para];
fii=CoefParameter[f,para];
gii=CoefParameter[g,para]; 
f0:=f - Transpose[fii].para; 
g0:=g - Transpose[gii].para;
(* Beginning of the computations *)
PSFFtr=SIExactFBL[f0,g0,var,False];
Print["PSFF Transformation z=T(",var,"):\n"];
ZZ=Table[ToExpression["z"<>ToString[i1]],{i1,1,ns}];
Do[
Print["         ",ZZ[[i1]]," =  ", PSFFtr[[i1]] ];  ,{i1,1,ns}];
{PSFFnewfff,PSFFnewggg}=PartialTransformSystem[f,g,var,PSFFtr,ZZ];
{PSFFtr,Simplify[PSFFnewfff],Simplify[PSFFnewggg]}
(* If psff conditions are not satisfied then return:  *)
,Print["\n"];
Print["System can not be transformed into Parametric Strict Feedback
Form\n"]]
];
SetAttributes[PSFFSolve,ReadProtected];
SetAttributes[PSFFSolve,Locked];


Clear[AdaptiveBackstepRegulator];
AdaptiveBackstepRegulator[f_,g_,xvar_,Theta_,Adgain_,Cs_]:= Module[{
n,p,x,
ZZZ,zzz,Z,
StFun,aa,Thetahat,thetahat,beta0,psi0,psi0rule,th,
StFunXXX,aaXXX,Thetabar,ruletheta,
Tune,tune,TuneXXX,tuneXXX,
UncertainPart,UncertainVec,PsiVectors},
x=xvar;
n=Length[x];
p=Length[Theta];
(* Take out (nonlinear) coefficients of Theta in f[x] *)
UncertainPart:=Array[UncertainVec,p];
UncertainVec[j1_]:=Coefficient[f, Theta[[j1]]];
(* Form columns \Psi_i = PsiVectors[[i]] in order to write: 
        Thetahat.columns at each row,  use this in a(i) *)
PsiVectors:=Transpose[Table[UncertainVec[i2],{i2,1,p}]];
psi0rule=Inner[Rule,Table[Theta[[i2]],{i2,1,p}],Table[0,{i2,1,p}],List];
psi0=f[[n]]/.psi0rule;   (* known nonlinear part*)
beta0=g[[n]];  (* known coefficient of control u *)
Thetahat=Array[thetahat,p];
ZZZ:=Array[zzz,n];
Tune:=Array[tune,n];
StFun:=Array[aa,n];
TuneXXX:=Array[tuneXXX,n];
StFunXXX:=Array[aaXXX,n];
Z:=x-Drop[Prepend[StFunXXX,0],-1];
(* Recursive computations of tuning functions tune[i], and stabilizing 
   functions aa[i]. First in z coordinates, then in x coordinates via 
   "dispsub" rule *)
tune[1]= Flatten[{ZZZ[[1]] Adgain.PsiVectors[[1]] }];   
aa[1]=-Cs[[1]] ZZZ[[1]] -Thetahat.PsiVectors[[1]];
tuneXXX[1]=tune[1]//.zzz[1]->x[[1]];
aaXXX[1]=aa[1]//.zzz[1]->x[[1]];
(* dispsub is the rule to substitute zzz(i)-> x(i) - aa(i-1) :=Z for final
   computation of parameter update law and adaptive control law expressed
   in the original coordinates x, i.e. we go back to the original coordinates
   at each step of the recursive computations so that the partials are
   evaluated accordingly *)
dispsub=Flatten[Join[{zzz[1]->x[[1]]},
                     Table[zzz[k]->(x[[k]]- aaXXX[k- 1]),{k,2,n}]]];
(***** Main recursive loop *****)
Do[
tune[i]=Flatten[tuneXXX[i-1] +  Flatten[{ ZZZ[[i]] Adgain.(
            PsiVectors[[i]] - Sum[
                D[ aaXXX[i-1],x[[j]] ] PsiVectors[[j]],
                          {j,1,i-1}    ]     )}]];
tuneXXX[i]=Flatten[tune[i]/.dispsub];
aa[i] =  - ZZZ[[i-1]] - Cs[[i]] ZZZ[[i]] 
         + Sum[D[ aaXXX[i-1],x[[j1]] ] x[[j1+1]], {j1,1,i-1}]   
         + Grad[ aaXXX[i-1], Thetahat].tuneXXX[i] 
         + (Sum[ZZZ[[j1+1]] Flatten[{ Grad[ aaXXX[j1], Thetahat].Adgain}],{j1,1,i-2}] 
                 - Thetahat)
            .(PsiVectors[[i]] - Sum[D[ aaXXX[i-1],x[[j1]] ] PsiVectors[[j1]],{j1,1,i-1}]);
aaXXX[i]=aa[i]//.dispsub; ,
{i,2,n,1}];         
(***** End of Main loop *****)
(* Output Formating: expressing Thetahat[i]->thi in the output format *)
Thetabar=Table[ToExpression["thetahat"<>ToString[i1]],{i1,1,p}];
ruletheta=Inner[Rule,Thetahat,Thetabar,List];
ParameterUpdate=tuneXXX[n]/.ruletheta;
AdaptiveControl= (aaXXX[n] - psi0)/beta0/.ruletheta;
CoordinateZZZ=Z//.ruletheta;
StabilizingFuncXXX=StFunXXX/.ruletheta;
TuningFuncXXX=TuneXXX/.ruletheta;
(*Return 
{AdaptiveControl,ParameterUpdate,StabilizingFuncXXX,
CoordinateZZZ,TuningFuncXXX,PsiVectors,UncertainPart}
*)
{Simplify[AdaptiveControl],Simplify[ParameterUpdate],CoordinateZZZ}
];
SetAttributes[AdaptiveBackstepRegulator,ReadProtected];
SetAttributes[AdaptiveBackstepRegulator,Locked];



(********************************************************************************)
(*                      Supporting Functions                                    *)
(********************************************************************************)



(************************************************************************)
(*                              HToNAndM                                 *)
(************************************************************************)

(* HToNAndM - pulls out the lowest order derivatives of HY *)

Unprotect[Transpose];

Transpose[{}]={}

Protect[Transpose];


Clear[HToNAndM];
Options[HToNAndM]={Internal->True};

HToNAndM[H_,LengthY_,t_]:=Module[{NMatT,MMatT,yN,yM,Htrans,Nlist={},Mlist={},
    derlist={},columns,NumDer,yy,n,test,der,k,NMat,MMat,Status},
      (*Set options*)
        Status=Internal/.Options[HToNAndM]; (*Internal->True suppresses 
                                                        printed output*)
      (*Initialize*)
        Htrans=Transpose[H];  
        columns=Length[Htrans];
        NumDer=columns/LengthY;
        yy=Flatten[Table[D[ToExpression["y"<>ToString[i1]<>"[t]"],{t,i2}],
             {i2,NumDer},{i1,LengthY}]];   (*creates output vector*)

        Do[test[i]=False,{i,columns}]; (*test[i]->true means column i is done*)
        n=1;
        
      (*Separate into N and M*)
        While[n<=columns,  
            der=Floor[(n-1)/LengthY+1];
            If[test[n],   (* If column n has not been done *)
                    
            n++,          (* else index *)
                    
            If[Apply[Plus,Htrans[[n]]^2]==0,    (* and if column n is not 0 *)
                        
                n++,                                (* else index *)
                
                AppendTo[Nlist,n];      (* then add column n to N *)
                AppendTo[derlist,der];  (* and add derivative *)
                i1=n+LengthY;
                While[i1<=columns,    (*and add all columns!=0 corresponding to the derivatives of yn[t] to M*) 
                    test[i1]=True;              
                    If[Apply[Plus,Htrans[[i1]]^2]==0,
                        i1=i1+LengthY,
                                
                        AppendTo[Mlist,i1];
                        i1=i1+LengthY,
                                
                        AppendTo[Mlist,i1];
                        i1=i1+LengthY
                    ]
                ];
                n++,
            
            AppendTo[Nlist,n];   (*undecided is same as column is not 0*)
                AppendTo[derlist,der];
                i1=n+LengthY;
                                While[i1<=columns,
                                    test[i1]=True;
                                    If[Apply[Plus,Htrans[[i1]]^2]==0,

                                        i1=i1+LengthY,
                                    
                                        AppendTo[Mlist,i1]; 
                                        i1=i1+LengthY,
                                        
                                        AppendTo[Mlist,i1];
                                        i1=i1+LengthY
                                    ]
                                ];
                        n++                             
                        ]
                ]
        ];
      (*Create N,M and the corresponding yN,yM*)
        NMat=Transpose[Htrans[[Nlist]]];
        MMat=Transpose[Htrans[[Mlist]]];
        yN=yy[[Nlist]];
        yM=yy[[Mlist]];
      (*Output*)
        If[Status,    
                Return[{NMat,MMat,yN,yM,derlist}],
                
                Print["H y= N y1+M y2"];
                Print["where\n\n"];
                Print["H=   ", HH//MatrixForm];
                Print["\n\n"];
                Print["y=   ", yy//MatrixForm];
                Print["\n\n"];
                Print["N=   ", NMat//MatrixForm];
                Print["\n\n"];
                Print["y1=  ", yN//MatrixForm];
                Print["\n\n"];
                Print["M=   ", MMat//MatrixForm];
                Print["\n\n"];
                Print["y2=  ", yM//MatrixForm];
                Print["derlist = ", derlist];
                
        ]
]
SetAttributes[HToNAndM,ReadProtected];
SetAttributes[HToNAndM,Locked];

(************************************************************************)
(*                              HToH1AndH2                                      *)
(************************************************************************)

(* HToH1AndH2 - pulls out the highest order derivatives of HY *)



Clear[HToH1AndH2];
Options[HToH1AndH2]={Internal->True};

HToH1AndH2[H_,LengthY_,t_]:=Module[{NMatT,MMatT,yN,yM,Htrans,Nlist={},Mlist={},         derlist={},columns,NumDer,yy,n,test,der,k,NMat,MMat,Status},
      (*Set options*)
        Status=Internal/.Options[HToNAndM]; (*Internal->True suppresses 
                                                        printed output*)
      (*Initialize*)
        Htrans=Transpose[H];  
        columns=Length[Htrans];
        NumDer=columns/LengthY;
        yy=Flatten[Table[D[ToExpression["y"<>ToString[i1]<>"[t]"],{t,i2}],
             {i2,NumDer},{i1,LengthY}]];   (*creates output vector*)

        Do[test[i]=False,{i,columns}]; (*test[i]->true means column i is done*)
        n=columns;
        
      (*Separate into N and M*)
        While[n>0,  
            der=Floor[(n-1)/LengthY+1];
            If[test[n],   (* If column n has not been done *)
                    
            n--,          (* else index *)
                    
            If[Apply[Plus,Htrans[[n]]^2]==0,    (* and if column n is not 0 *)
                        
                n--,                                (* else index *)
                        
                PrependTo[Nlist,n];      (* then add column n to N *)
                PrependTo[derlist,der];  (* and add derivative *)
                i1=n-LengthY;
                While[i1>0,    (*and add all columns!=0 corresponding to the derivatives of yn[t] to M*)
                    test[i1]=True;
                    If[Apply[Plus,Htrans[[i1]]^2]==0,
                        i1=i1-LengthY,
                                
                        PrependTo[Mlist,i1];
                        i1=i1-LengthY,
                                
                        PrependTo[Mlist,i1];
                        i1=i1-LengthY
                    ]
                ];
                n--,
        
                PrependTo[Nlist,n];   (*undecided is same as column is not 0*)
                PrependTo[derlist,der];
                i1=n-LengthY;
                                While[i1>0,
                                    test[i1]=True;
                                    If[Apply[Plus,Htrans[[i1]]^2]==0,

                                        i1=i1-LengthY,
                                    
                                        PrependTo[Mlist,i1]; 
                                        i1=i1-LengthY,
                                        
                                            PrependTo[Mlist,i1];
                                        i1=i1-LengthY
                                    ]
                                ];
                        n--                             
                        ]
                ]
        ];
      (*Create H1,H2 and the corresponding yH1,yH2*)
        H2Mat=Transpose[Htrans[[Nlist]]];
        H1Mat=Transpose[Htrans[[Mlist]]];
        yH2=yy[[Nlist]];
        yH1=yy[[Mlist]];
      (*Output*)
        If[Status,    
                Return[{H1Mat,H2Mat,yH1,yH2,derlist}],
                
                Print["H y= H1 y1+H2 y2"];
                Print["where\n\n"];
                Print["H=   ", HH//MatrixForm];
                Print["\n\n"];
                Print["y=   ", yy//MatrixForm];
                Print["\n\n"];
                Print["H1=   ", H1Mat//MatrixForm];
                Print["\n\n"];
                Print["y1=  ", yH1//MatrixForm];
                Print["\n\n"];
                Print["H2=   ", H2Mat//MatrixForm];
                Print["\n\n"];
                Print["y2=  ", yH2//MatrixForm];
                Print["derlist = ", derlist];
                
        ]
]
SetAttributes[HToH1AndH2,ReadProtected];
SetAttributes[HToH1AndH2,Locked];


(************************************************************************)
(*                      Separate                                        *)
(************************************************************************)

(*file name Separate.m*)

(*
A function used by adaptive to find elements of ThetaBar in terms of theta
*)

Clear[Separate];
Separate[A1_,T1_]:=Module[{A2,A3,A4,A5,Div1,TBar1,ThetaBar1,RuleBar},
     A2=Expand[A1];                 (*multiply out the expression*)
     A3=Level[Flatten[A2],{2}];     (*separate out the terms*)
     A4=A3; 
     (*set all T1=1 to find parts dep. on theta *)
     Div1=A4/.Inner[Rule,T1,Table[1,{Length[T1]}],List]/.{0->1};
     A5=A4/Div1;
     (*Drop purely numerical factors *)
     (*This is done by dropping level-1 real numbers*)
     TBar1=Union[DeleteCases[A5, n_?NumberQ, 1]/.{1.->1}];
     ThetaBar1=Table[  
                   ToExpression[
                        "thetahat"<>ToString[i1]
                   ],{i1,1,Length[TBar1]}
                 ];
     RuleBar=Inner[Rule,TBar1,ThetaBar1,List];
     Return[{TBar1,(A2/.RuleBar)/.(RuleBar/.RuleBar)}]
]

(*
Separate[A1_,T1_,var_]:=Module[{A2,A3,A4,A5,Div1,Tbar1},

        A2=Expand[A1];          (*multiply out the expression*)
        A3=Level[A2,{2}];       (*separate out the terms*)

(* Convert all primary summations to a list *)
    A4=Union[Flatten[ReplacePart[A3,List,Position[A3,Plus,{2}]]]];

(*set all var=1 to find parts dep. on theta *)
        Div1=A4/.Inner[Rule,T1,Table[1.0,{Length[T1]}],List]/.{0->1};
        A5=A4/Div1;

(*Drop the numerical factor caused mainly by setting Theta=1.0.*)
(*This is done by dropping a level-2 real number*)
        Tbar1=Union[DeleteCases[A5, n_?NumberQ, 2]/.{1.->1}];

        Return[Tbar1]
]
*)

Separate[0,T1_,var_]:={}
SetAttributes[Separate,ReadProtected];
SetAttributes[Separate,Locked];



(************************************************************************)
(*                      Pseudoinverse                                   *)
(************************************************************************)



(* Pseudoinverse *)

Clear[Pseudoinverse];
Pseudoinverse[A_?MatrixQ]:=Transpose[A].MyInverse[A.Transpose[A]];

(************************************************************************)
(*                      MyInverse                                       *)
(************************************************************************)
(*  Finds the inverse symbolically, then substitutes the elements of the *)
(*    original matrix                                                   *)


Clear[MyInverse];

MyInverse[A_?MatrixQ]:=Module[{
mat,            (* matrix of symbolic terms of same size as A*)
a,              (* a[i,j] are elements of mat *)
Inv,            (* inverse[mat] *)
ElemsA,         (* List of elements of A *)
ZeroElemsA,     (* positions of the zero elements of A *)
NonZeroElemsA,  (* Complement of ZeroElemsA*)
ZeroRule,       (* Rule to substitute the zero elements first *)
NonZeroRule     (* Rule to substitute the rest of the elements *)
},
mat=Table[a[i,j], {i,Dimensions[A][[1]]},{j,Dimensions[A][[2]]}];
Inv=Inverse[mat];
ElemsA=Flatten[A];
ZeroElemsA=Flatten[Position[ElemsA,0]];
NonZeroElemsA=Complement[Range[Length[ElemsA]],ZeroElemsA];
ZeroRule=Thread[Flatten[mat][[ZeroElemsA]]->ElemsA[[ZeroElemsA]]];
NonZeroRule=Thread[Flatten[mat][[NonZeroElemsA]]->ElemsA[[NonZeroElemsA]]];
Inv/.ZeroRule/.NonZeroRule
]
SetAttributes[MyInverse,ReadProtected];
SetAttributes[MyInverse,Locked];


(* Right Inverse *)
RightInverse[A_?MatrixQ]:=Module[{m,n,test,
  Id,B,DD,DD1,DD2,ZeroRow,G,IndexSet,XX,row,RowNo},
  {m,n}=Dimensions[A];
  Id=IdentityMatrix[m];
  B=Transpose[Join[Transpose[A],Id]];
  DD=RowReduce[B];
  DD1=DD[[Range[1,m],Range[1,n]]];
  DD2=DD[[Range[1,m],Range[n+1,n+m]]];
  {ZeroRow}=Table[0,{1},{m}];
  G=Position[DD1,1];
  IndexSet={};RowNo=1;
   Do[(If[G[[j,1]]==RowNo,
          IndexSet=Append[IndexSet,G[[j,2]]];
          RowNo=RowNo+1
         ]
   ),{j,Length[G]}];
   XX={};row=1;
   Do[(If[Intersection[{j},IndexSet]=={},
          XX=Append[XX,ZeroRow],
          XX=Append[XX,DD2[[row]]];row=row+1;
         ]
   ),{j,n}];
  XX
]

(*Left Inverse *)
LeftInverse[A_?MatrixQ]:=Module[{B,XX},
  B=Transpose[A];
  XX=Transpose[RightInverse[B]]
]

(************************************************************************)
(*                      Modification of D                               *)
(************************************************************************)
(*  D[{y1, y2, ..., yn}, {{a1, a2,..., an},t] does {D[y1,{a1,t}], D[y2, {a2, t}], ..., D[yn,{an,t}] *)

Unprotect[D]

D[y_List, {t_Symbol,a_List}]:=Map[D[y[[#]], {t,a[[#]]}]&, Range[Length[y]]]/;Length[y]==Length[a]

Protect[D]


(************************************************************************)
(*                      Poly                                            *)
(************************************************************************)
(* Makes a polynomial from roots                                        *)

Clear[Poly];
Poly[roots_List, s_Symbol]:=Expand[Apply[Times, Map[s-#&, roots]]]

Poly[roots_List]:=CoefficientList[Poly[roots, s],s]
SetAttributes[Poly,ReadProtected];
SetAttributes[Poly,Locked];


(****************************************************************)
(*                      GaussElimRows                           *)
(****************************************************************)


Clear[GaussElimRows, GaussElimColumns];

GaussElimRows[E_?MatrixQ]:=Module[
{NumRows=Dimensions[E][[1]], 
NumColumns=Dimensions[E][[2]],
r=Rank[Etemp],                  (* Rank of the matrix *)
Etemp=E, 
Transformation,         (* Linear transformation which performas Guassian elimination *)
coef,           
count,
EZeroRow,
TransformationZeroRow,
pivotX,
pivotY},
        
    (* init *)
        Transformation=IdentityMatrix[NumRows];
        pivotX=1;
        pivotY=1;
        
        While[pivotX<=NumColumns && pivotY<=NumRows,  
                count=0;
            
         (* look for a non-zero pivot *)
                While[Etemp[[pivotY,pivotX]]==0 && count<=NumRows-pivotY,

                    (* save the zero pivot row of E and the corresponding row in the transformation *)
                         EZeroRow=Etemp[[pivotY]]; 
                         TransformationZeroRow=Transformation[[pivotY]];

                     (* shift rows up *)
                         Do[Etemp[[i]]=Etemp[[i+1]],{i,pivotY,NumRows-1}];
                         Do[Transformation[[i]]=Transformation[[i+1]],{i,pivotY,NumRows-1}];

                     (* Place the zero row at the bottom *)
                         Etemp[[NumRows]]=EZeroRow;
                         Transformation[[NumRows]]=TransformationZeroRow;
                         count++;
                ];

                If[Etemp[[pivotY,pivotX]]==0, 
                        pivotX++,
                     
                     (* elimination *)
                        Do[     
                            (* compute pivot *)
                                coef=Etemp[[j,pivotX]]/Etemp[[pivotY,pivotX]]; 
                            (* eliminate *)
                                Etemp[[j]]=Etemp[[j]]-coef*Etemp[[pivotY]];
                                Transformation[[j]]=Transformation[[j]]-coef*Transformation[[pivotY]]; 
                            ,
                                {j,pivotY+1,NumRows}
                        ]; 
                     (* increment coordinates of the pivot *)
                        pivotX++;
                        pivotY++
                ,
                     (* elimination *)
                        Do[     
                            (* compute pivot *)
                                coef=Etemp[[j,pivotX]]/Etemp[[pivotY,pivotX]]; 

                            (* eliminate *)
                                Etemp[[j]]=Etemp[[j]]-coef*Etemp[[pivotY]];
                                Transformation[[j]]=Transformation[[j]]-coef*Transformation[[pivotY]]; 
                            ,
                                {j,pivotY+1,NumRows}
                        ]; 
                     (* increment coordinates of the pivot *)
                        pivotX++;
                        pivotY++

                ]
        ];
        Transformation
]
SetAttributes[GaussElimRows,ReadProtected];
SetAttributes[GaussElimRows,Locked];
        
GaussElimColumns[E_?MatrixQ]:= Transpose[GaussElimRows[Transpose[E]]]



(* Functions used by StructureAlgorithm[f,g,h,x] *)

(****************************************************************)
(*                      LinearlyIndep                           *)
(****************************************************************)

LinearlyIndep[x1_?VectorQ, x2_?VectorQ]:=
                Rank[{x1,x2}]==2

(****************************************************************)
(*                      NRank                                   *)
(****************************************************************)


Clear[NRank]
NRank[A_?MatrixQ, x_?VectorQ]:=Module[{},
Max[Table[Rank[A/.Inner[Rule, x, Table[Random[],{i,Length[x]}],List]],{i,10}]]

]


(****************************************************************)
(*                      ReorderRows                             *)
(****************************************************************)

Clear[ReorderRows]
ReorderRows[A_?MatrixQ,x_]:=Module[
{k=1,
TempA=A,                (* Temp. version of A *)
RankA=Rank[A],                  (* Rank of A *)
DimA=Length[A],         (* Dim of A  *)
Trans,                  (* Transformation matrix which reorders the rows *)
ZeroRowList={},         (* Numbers of rows which contain only zeros *)
NonZeroRowList,
DepRowList,             (* Num of rows which are dep on earlier rows *)
IndepRowList={1,2}
},
 
        (* init *)
                DepRowList=Complement[Range[1,DimA],
                                IndepRowList
                                ];
 
    Trans=IdentityMatrix[DimA];
    If[RankA==0||RankA==DimA, 
        Return[{Trans,TempA}]
    ,

     (* Find zero rows *)
        Do[
          If[ Simplify[TempA[[i]].TempA[[i]]]==0
          ,
              AppendTo[ZeroRowList, i]
          ],
          {i,1,DimA}
        ];

      (* Find nonzero rows *)
        NonZeroRowList=Complement[Range[1,DimA], ZeroRowList];

      (* Reorder with zero rows at the bottom *)
        TempA=Join[TempA[[NonZeroRowList]],TempA[[ZeroRowList]]];
        Trans=Join[Trans[[NonZeroRowList]],Trans[[ZeroRowList]]];
        If[RankA==1,
                Null,

        
        While[Length[IndepRowList]<RankA,
                DepRowList=Complement[
                                Range[1,DimA-Length[ZeroRowList]],
                                IndepRowList
                           ];
                
                Map[
                    If[Rank[TempA[[Append[IndepRowList,#]]]]>Length[IndepRowList],
                        AppendTo[IndepRowList, #]]&, DepRowList
                  ]
            ];

        DepRowList=Complement[
                                Range[1,DimA],
                                IndepRowList
                           ];
     
 (* Compute the reordered matrix and the transformation matrix *)
        TempA=Join[TempA[[IndepRowList]],TempA[[DepRowList]]];
        Trans=Join[Trans[[IndepRowList]],Trans[[DepRowList]]]
        ]
    ]; 
Return[{Trans, TempA}]]
SetAttributes[ReorderRows,ReadProtected];
SetAttributes[ReorderRows,Locked];



(****************************************************************)
(*                      MakeDepRowsZero                         *)
(****************************************************************)

Clear[MakeDepRowsZero]

MakeDepRowsZero[E1_?MatrixQ,D_?MatrixQ,x_?VectorQ]:=Module[
{RankD=Rank[D],
DimD=Length[D],
D1=E1.D,
Dbar,
Dhat,
FF,
I1,
I2,
Z1,
Part1,
Part2,
Trans2},
 
        If[RankD==DimD||Rank[Take[D1, -(DimD-RankD)]]==0||RankD==0,
                Return[{IdentityMatrix[DimD],0}]
        ,
            (* reorder rows *)
                Dbar=D1[[Range[1,RankD]]];
                Dhat=D1[[Range[RankD+1,DimD]]];
            
            (* solve for FF in the transformation [ I1  Z1 ]  *)
            (*                                    [ FF  I2 ]  *)
                FF=ToExpression[
                        Outer["ff"<>ToString[#1]<>ToString[#2]&,
                        Range[1,DimD-RankD],Range[1,RankD]]];

        
                FF=FF/.Flatten[Solve[FF.Dbar==-Dhat,Flatten[FF]]];

            (* construct the transformation *)
                I1=IdentityMatrix[RankD];
                I2=IdentityMatrix[DimD-RankD];
                Z1=Table[0,{i,RankD},{j,DimD-RankD}];
                Part1=MapThread[Join,{I1,Z1}];
                Part2=MapThread[Join,{FF,I2}];
                Trans2=Join[Part1,Part2];
                Return[{Trans2,FF}]
        ]
]
SetAttributes[MakeDepRowsZero,ReadProtected];
SetAttributes[MakeDepRowsZero,Locked];


Clear[WriteCCode];
WriteCCode[CCodeList_,sfile_]:=
       Module[{NumberOfCCodeLines,n},(
        NumberOfCCodeLines = Length[CCodeList];
        For[n=1, n<NumberOfCCodeLines+1, n++,
           WriteString[sfile,StringJoin[CCodeList[[n]]," \n"]];
        ];
)];
SetAttributes[WriteCCode,ReadProtected];
SetAttributes[WriteCCode,Locked];


Clear[FunctionReplace];
FunctionReplace[EquationList_]:= Module[{FCombined,FExp,FExpU,FExpLength,
                                     FunctionHeads,FunctionTerms ,
                                     NumberOfFunctionTerms,Replacements,
                                     ArcTanTerms,NumberOfArcTanTerms,ArcTanTermsNumberOfArgs,
                                     ArcTanRules,i,NewRule,
                                     FNew},(

       FCombined = Apply[Plus,Flatten[EquationList]];
       FExp = Level[FCombined,Infinity];
       FExpU = Union[FExp];
       FExpLength = Length[FExp];
       FunctionHeads = {_Sin,_ArcSin,_Cos,_ArcCos,_Tan,_ArcTan,_Global`ArcTan2,
                           _Csc,_Sec,_Cot,_Power,_Exp,_Log,_Sinh,_Cosh,
                           _Tanh,_Csch,_Sech,_Coth,_ArcSinh,_ArcCosh,_ArcTanh};
       (* First examine ArcTan functions and deteremine whether any are called with two arguments *)
       ArcTanTerms = Cases[FExpU,_ArcTan];
       NumberOfArcTanTerms = Length[ArcTanTerms];
       ArcTanTermsNumberOfArgs=Flatten[Map[Dimensions,Map[Level[#,{1}]&,ArcTanTerms]]];
       ArcTanRules = {};
       For[i=1,i<NumberOfArcTanTerms+1,i++,
          If[ArcTanTermsNumberOfArgs[[i]]==2, 
             NewRule = Rule[ArcTanTerms[[i]],ArcTanTerms[[i]]/.ArcTan->Global`ArcTan2];
             ArcTanRules = Join[ArcTanRules,{NewRule}];
          ];
       ];

       FNew = EquationList/.ArcTanRules;
       FExpU = FExpU/.ArcTanRules;
       FunctionTerms = Flatten[Map[Cases[FExpU,FunctionHeads[[#]]]&,
                                   Range[1,Length[FunctionHeads]]]];
       FunctionTerms=Sort[FunctionTerms,(Depth[#2]>Depth[#1])&];
       NumberOfFunctionTerms = Length[FunctionTerms];

       Replacements = Map[Rule[FunctionTerms[[#]],
                        ToExpression[StringJoin["t",ToString[#]]]]&,
                   Range[1,NumberOfFunctionTerms]];

       FNew  = FNew/. Replacements;
(* Apply replacement rules to the rhs of the replacement rules themselves *)
       For[i=2,i<NumberOfFunctionTerms+1,i++,
          FunctionTerms[[i]] = FunctionTerms[[i]]/.Drop[Replacements,{i,NumberOfFunctionTerms}];

];

       Replacements = Map[Rule[FunctionTerms[[#]],
                        ToExpression[StringJoin["t",ToString[#]]]]&,
                   Range[1,NumberOfFunctionTerms]];
 

{FNew,Replacements} )];
SetAttributes[FunctionReplace,ReadProtected];
SetAttributes[FunctionReplace,Locked];


Clear[ReduceFLOPSToo];
ReduceFLOPSToo[Expr_]:=Module[{FExp,FExpU,FExpLength,AcceptableHeads,
       ListOfExpressions,LengthListOfExpresions,ContinueCollection,
       CollectionList,InstancesOfOccurences,NumberOfOccurences,MaxElem,
       IndexOfMaxElem,MostFrequentExpression,ExprTmp},(
(* Compute a list of all expression with specified heads *)
       ExprTmp = Expr;
       FExp = Level[ExprTmp,Infinity];
       FExpU = Union[FExp];
       FExpLength = Length[FExp];
       AcceptableHeads = {_Cos,_Sin,_Power,_Symbol,_Tan,_Cot,_Sec,_Csc};
       ListOfExpressions = Flatten[Map[Cases[FExpU,#]&,AcceptableHeads]];
       LengthListOfExpresions = Length[ListOfExpressions];
       ContinueCollection = True;
       CollectionList = {};
       Exprnew= Collect[ExprTmp,CollectionList];
     While[ContinueCollection,(
(* Compute how many times each expression in ListOfExpressions appears in Expr *)
         InstancesOfOccurences = Flatten[Map[Cases[FExp,#]&,AcceptableHeads]];
         NumberOfOccurences = Map[Count[InstancesOfOccurences,#]&,
                                     ListOfExpressions];
(* Find expression(s) with the highest frequency of occurence *)
         MaxElem = Max[NumberOfOccurences];
         IndexOfMaxElem = Flatten[Position[NumberOfOccurences,MaxElem]];
       
  MostFrequentExpression =ListOfExpressions[[IndexOfMaxElem]];
(* Add it to collection list *)
         CollectionList =Flatten[{CollectionList,MostFrequentExpression}];
(* Remove it from list of expressions *)
         ListOfExpressions = Complement[ListOfExpressions,CollectionList];
        (* Print[ListOfExpressions];
         Print[CollectionList];*)
         If [Length[ListOfExpressions] != 0,(
           (*then*)
           (* Perform Collections *)
            Exprnew= Collect[ExprTmp,CollectionList];
            FExp = Level[Exprnew,Infinity];
            ContinueCollection = True),
        (*else*)   
            ContinueCollection = False;];
    )];
  Exprnew)];
SetAttributes[ReduceFLOPSToo,ReadProtected];
SetAttributes[ReduceFLOPSToo,Locked];


(****************************************************************)
(*                      Utitlity Functions                      *)
(****************************************************************)


Clear[Dist];
Dist[f_,g_,var_,k_]:=Module[{k1},
Union[Table[Ad[f,g,var,k1],{k1,0,k}]] ];
SetAttributes[Dist,ReadProtected];
SetAttributes[Dist,Locked];

Clear[FBLCond]
FBLCond[f_,g_,var_]:=Module[{ns},
ns=Length[f];
TrueQ[Rank[ Dist[f,g,var,ns-1]]==ns  &&
    Involutive[Dist[f,g,var,ns-2],var] ]
];
SetAttributes[FBLCond,ReadProtected];
SetAttributes[FBLCond,Locked];

Print["  *** NonlinearControl successfully loaded ***"];
End[]

EndPackage[ ]
If[!spell1, On[General::spell1]];
If[!spell, On[General::spell]];

