(*                VSCTools



           Techno-Sciences, Inc.
           10001 Derekwood Lane, Suite 204
           Lanham, MD 20706
           (301) 577-6000
           
           
Copyright © 1997, 1998 Techno-Sciences Incorporated
All Rights Reserved          
           
*)
spell1 = (Head[General::spell1] === $Off);
spell = (Head[General::spell] === $Off);
Off[General::spell1];
Off[General::spell]; 

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


(* Function Usage Statements *)

SlidingSurface::usage="The calling syntax\n
\n
{rho,s}=SlidingSurface[f,g,h,x,lam]\n
\n
returns the decoupling matrix rho and the sliding surface s.\n
s is returned as a list of expressions in the state variables x.\n
f,g,h deine the square system, i.e. Cols[g]=Rows[h]=m, the number \n
of inputs and outputs. lam is a list of positive numbers of length m\n
that specify the desired decay rates for each of the m channels.
\n
The calling syntax\n
\n
s=SlidingSurface[rho,vro,z,lam]\n
\n
is convenient if the decoupling matrix rho and the normal coordinates z\n
have already been computed. In this case, since the vector relative degree\n
vro has also been computed it is possible to specify all of the sliding\n
poles. lam is a list of m lists defining m sets of poles - one set for each\n
sliding surface. The number of poles in each list must conform to vro. That\n
is, the number of specified poles should be one less than the corresponding\n
index.
"

SwitchingControl::usage="The variable structure controller is obtained\n
with the calling syntax:\n
\n
  control=SwitchingControl[rho,s,bounds,Q,opts] \n
\n
where rho is the decoupling matrix, s is the vector of switching surfaces,\n
bounds is a list of controller bounds each in the form {lower bound, upper bound},\n
Q is an mxm positive definite matrix (a design parameter), and opts are options \n
which allow the inclusion of smoothing and/or moderating functions in the control.\n
(see SmoothingFunctions and ModerationFunctions)\n
\n
The alternative syntax
\n
  control=SwitchingControl[alpha,rho,s,bounds,Q,opts] \n
\n
produces a control that removes the alpha terms: control=-(rho^-1)alpha+vsc.\n
"

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

SmoothingFunctions::usage="SmoothingFunctions is an option of SwitchingControl[].\n
UnitStep functions are multiplied by 'smoothing' functions defined in the statement\n
SmoothingFunctions[x_]->{function1[x],...,functionm[x]}, where m is the number\n
of control variables. Typical smoothing functions are:\n
\n
Sat[x/eps], (1-Exp[-x/eps]), x/(eps+Abs[x]), Tanh[x/eps] \n
\n
In each case eps is a 'small' positive constant.
"
ModeratingFunctions::usage="ModeratingFunctions is an option of SwitchingControl[].\n
UnitStep functions are multiplied by 'moderating' functions defined in the statement\n
ModeratingFunctions->{function1[z],...,functionm[z]}, where m is the number\n
of control variables. Typical moderating functions are:\n
\n
(1-Exp[-norm[z]/cons]), norm[z]/(cons+norm[z]), Tanh[norm[z]/cons] \n
\n
where 'norm' is some appropriate norm in the normal coordinates z.\n
In each case 'cons' is a positive constant.
"
Options[SwitchingControl]={SmoothingFunctions[x_]->{},ModeratingFunctions[x_]->{}};

(*  Functions *)

Begin["`private`"]

Clear[SlidingSurface];
SlidingSurface[rho_?MatrixQ,vro_?VectorQ,z_?VectorQ,lam_List]:=Module[
    {Func,Klist,s},
    (* argument tests *)
    (* compatible dimensions of vro and lam *)
    Func[Xvro_,Xlam_]:=Module[{A,b,c},
       {A,b,c}=NormalAbc[Xvro-1];
       If[A=={{}},{1},Join[PolePlace[A,b,Xlam],{1}]]
             ]; (* end module Func *)
    Klist=MapThread[Func,{vro,lam}];
    s=ProdVp[Klist,z]
    ]    
             
SlidingSurface[f_,g_,h_,x_,lam_List]:=Module[{rho,alpha,vro,control,
        z,Func,Klist,s,temp},
                (* argument tests *)
        If[Length[f]!=Length[x],Print["Incompatible state and dynamics"];Return[]];
        If[Length[f]!=Length[g],Print["Incompatible f and g"];Return[]];
        If[Length[h]!=If[VectorQ[g],1,Dimensions[g][[2]]],Print["System must be square"];Return[]];
        temp=IOLinearize[f,g,h,x];
        If[Length[temp]!=4,Return[],{rho,alpha,vro,control}=temp];
        z=NormalCoordinates[f,h,x,vro];
        Func[Xvro_,Xlam_]:=Module[{A,b,c},
             {A,b,c}=NormalAbc[Xvro-1];
             If[A=={{}},{{1}},AppendRows[-ExponentialRegulator[N[A],b,c,Xlam],{{1}}]]
             ]; (* end module Func *)
        Klist=MapThread[Func,{vro,lam}];
        s=ProdVp[Klist,z];
        {rho,s}
        ]
SetAttributes[SlidingSurface,ReadProtected];
SetAttributes[SlidingSurface,Protected];
SetAttributes[SlidingSurface,Locked];

Clear[SwitchingControl];
SwitchingControl[rho_?MatrixQ,s_,bounds_,Q_,opts___]:=Module[{sstar,
          control,control1,control2,SmoothingList,SmoothingRules,
          ModeratingList,ModeratingRules},
          sstar=Transpose[rho].Q.s;
          control=Map[(bounds[[#,1]]*UnitStep[sstar[[#]]]
                       +bounds[[#,2]]*UnitStep[-sstar[[#]]])&,Range[1,Length[bounds]]];
          (* Apply smoothing *)
          If[(SmoothingFunctions[y]/.{opts}/.Options[SwitchingControl])!={},
              SmoothingList=Inner[Times,Table[UnitStep[y],{i,Length[s]}],
                                   SmoothingFunctions[y]/.{opts}/.Options[SwitchingControl],List];
              SmoothingRules=
                 Inner[Rule,Table[UnitStep[y_],{i,Length[s]}],SmoothingList,List];
              control1=Inner[ReplaceAll,control,SmoothingRules,List],
              control1=control];     (* end if *)
          control2=control1;
          (* Apply Moderating *)
          If[(ModeratingFunctions/.{opts}/.Options[SwitchingControl])!={},
              ModeratingList=Inner[Times,Table[UnitStep[y],{i,Length[s]}],
                                   ModeratingFunctions/.{opts}/.Options[SwitchingControl],List];
              ModeratingRules=
                 Inner[Rule,Table[UnitStep[y_],{i,Length[s]}],ModeratingList,List];
              control2=Inner[ReplaceAll,control1,ModeratingRules,List],
              control2=control1];     (* end if *)
          UnitStep2Sign[control2]
          ]
          
SwitchingControl[alpha_?VectorQ,rho_?MatrixQ,s_,bounds_,Q_,opts___]:=Module[{sstar,
          control,control1,control2,SmoothingList,SmoothingRules,
          ModeratingList,ModeratingRules},
          sstar=Transpose[rho].Q.s;
          control=Map[(bounds[[#,1]]*UnitStep[sstar[[#]]]
             +bounds[[#,2]]*UnitStep[-sstar[[#]]])&,Range[1,Length[bounds]]];
          control=-MyInverse[rho].alpha+control;
          (* Apply smoothing *)
          If[(SmoothingFunctions[y]/.{opts}/.Options[SwitchingControl])!={},
              SmoothingList=Inner[Times,Table[UnitStep[y],{i,Length[s]}],
                                   SmoothingFunctions[y]/.{opts}/.Options[SwitchingControl],List];
              SmoothingRules=
                 Inner[Rule,Table[UnitStep[y_],{i,Length[s]}],SmoothingList,List];
              control1=Inner[ReplaceAll,control,SmoothingRules,List],
              control1=control];     (* end if *)
          control2=control1;
          (* Apply Moderating *)
          If[(ModeratingFunctions/.{opts}/.Options[SwitchingControl])!={},
              ModeratingList=Inner[Times,Table[UnitStep[y],{i,Length[s]}],
                                   ModeratingFunctions/.{opts}/.Options[SwitchingControl],List];
              ModeratingRules=
                 Inner[Rule,Table[UnitStep[y_],{i,Length[s]}],ModeratingList,List];
              control2=Inner[ReplaceAll,control1,ModeratingRules,List],
              control2=control1];     (* end if *)
          UnitStep2Sign[control2]
          ]          
SetAttributes[SwitchingControl,ReadProtected];
SetAttributes[SwitchingControl,Protected];
SetAttributes[SwitchingControl,Locked];

(***************************** Utility ****************************)


ProdVp[V_,p_]:=V.p /; MatrixQ[V];                          
ProdVp[V_,p_]:=Module[{Finish,Start,Inds,VdimsCols,Vp},(
    VdimsCols = Transpose[Map[Dimensions,V]][[2]];
    LengthV = Length[VdimsCols];
    Finish = Map[Apply[Plus,Take[VdimsCols,#]]&,Range[1,LengthV]];
    Start = Finish-VdimsCols+1;
    Inds = Map[Range[Start[[#]],Finish[[#]]]&,Range[1,LengthV]];
    Vp = Flatten[Map[V[[#]].p[[Inds[[#]]]]&,Range[1,LengthV]]];
    Vp
   )];

NormalAbc[0]:={{{}},{},{}};
NormalAbc[n_Integer?Positive]:=Module[{Temp,A,b,c},
      Temp=Join[IdentityMatrix[n-1],{Table[0,{i,n-1}]}];
      A=Transpose[Join[{Table[0,{i,n}]},Transpose[Temp]]];
      b=Join[Table[0,{i,n-1}],{1}];
      c=Join[{1},Table[0,{i,n-1}]];
      {A,b,c}
      ];
                
ExponentialRegulator[A_,b_?VectorQ,c_?VectorQ,decay_]:= 
ExponentialRegulator[A,Transpose[{b}],{c},decay]
ExponentialRegulator[A_,B_?MatrixQ,C_?MatrixQ,decay_]:=
   Module[{nn=Length[A],
     mm=Dimensions[B][[2]],Q,R,KK,PP,eigs},
     Q=Transpose[C].C;
     R=IdentityMatrix[mm];
     {KK,PP,eigs}=LQR[A+decay*IdentityMatrix[nn],B,Q,R];
     KK
     ];
     
(************************************************************************)
(*                      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,Protected];
SetAttributes[MyInverse,Locked];

End[]

EndPackage[ ]

(* ********************************************************************** *)
(* ********************************************************************** *)
If[!spell1, On[General::spell1]];
If[!spell, On[General::spell]];

