(************************************************************************
* 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 TSICONL.M FOR LINEAR 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
* 
* Functions are provided for the manipulation of linear control 
* systems in state space or frequency domain forms.
* Functions for the conversion of one form to the other are also
* provided. 
* 
***********************************************************************)
spell1 = (Head[General::spell1] === $Off);
spell = (Head[General::spell] === $Off);
Off[General::spell1];
Off[General::spell];

If[$VersionNumber===2.,BeginPackage["ProPac`ControlL`",{"ProPac`GeoTools`","LinearAlgebra`MatrixManipulation`",
    "Algebra`ReIm`","Graphics`Graphics`","Graphics`Legend`","Calculus`Limit`","Calculus`LaplaceTransform`",
    "Calculus`Common`TransformCommon`"}]];
If[$VersionNumber===3.,BeginPackage["ProPac`ControlL`",{"ProPac`GeoTools`","LinearAlgebra`MatrixManipulation`",
    "Algebra`ReIm`","Graphics`Graphics`","Graphics`Legend`","Calculus`Limit`","Calculus`LaplaceTransform`",
    "Calculus`Common`TransformCommon`"}]];
If[$VersionNumber===4.,BeginPackage["ProPac`ControlL`",{"ProPac`GeoTools`","LinearAlgebra`MatrixManipulation`",
    "Algebra`ReIm`","Graphics`Graphics`","Graphics`Legend`",
    "Calculus`Limit`"}]];
(****************************************************************)

(* Package Help *)

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

LinearControl::usage=
"Functions in the package for linear control are:\n
\n
MatrixRank, Jacobian, TaylorLinearize,\n 
RandomMatrix, RangeSpace, MatrixTrace,n
BlockCompanionMatrix,\n
\n
StateSpaceToTransferFunction,\n
LeastCommonDenominator, Poles, Zeros,\n
AssociatedHankelMatrix,LaurentSeries,\n
McMillanDegree,ControllableRealization,\n
ObservableRealization, MinimalRealization,\n
\n
ControllabilityMatrix, ObservabilityMatrix,\n
ControllablePair, ObservablePair,\n
KalmanDecomposition,RelativeDegree,\n
\n
AlgebraicRiccatiEquation,LQR,LQE,\n
DecouplingControl,PolePlace,\n
LyapunovEquation,\n
\n
RootLocus,Nyquist,ColorNyquist,Bode,\n
PoleZeroPlot";

(* usage messages for the exported functions and the context itself *)
   
LyapunovEquation::usage = 
"LyapunovEquation[A,Q] Returns the solution P of the Matrix\n
Lyapunov equation\n
      PA + A^TP = -Q\n
Ordinarily, Q is a symmetric, positive semidefiite matrix.
"

AlgebraicRiccatiEquation::usage =
"AlgebraicRiccatiEquation[A,Q,U] computes the positive definite solution
P of the algebraic Riccati equation
         PA + A'P - PUP + Q = 0
where Q and U are nxn positive semidefinite symmetric matrices and 
A is detectable in Q and stabilizable with respect to U."

LQR::usage = 
"{K,P,Eigs} = LQR[A,B,Q,R] calculates the optimal feedback gain 
matrix K such that the feedback law  u = Kx  minimizes the 
cost function:

     J = int_0^\infty x^T Qx + u^T Ru dt

subject to the state equation
     .
     x = Ax + Bu 

Also returned is P, the steady-state solution to the associated 
algebraic Riccati equation:

       0 = PA + A^T P - PBR^{-1} B^T P + Q
";

LQE::usage = 
"Executes linear quadratic estimator design for the continuous
time system:
     .
     x = Ax + Bu + Gw, (State equation)

     z = Cx + Du + v,  (Measurements)

with process noise and measurement noise covariances:

      Ew = Ev = 0, Eww^T = Q, Evv^T = R

The function LQE[A,G,C,Q,R] determines the gain matrix L
such that the stationary Kalman filter:
      .
      x = Ax + Bu + L(Cx + Du - z)

produces an LQG optimal estimate of x.
{L,P,Eigs} = LQE[A,G,C,Q,R] returns the gain matrix L and the Riccati
equation solution P which is the estimate error covariance. The 
estimator parameters are obtained using LQR and duality.";

DecouplingControl::usage = 
"DecouplingControl[A,B,C] computes the feedback gain K and coordinate
change G which decouple the closed loop feedback system 
         xdot = A x + B u, 
that is, u=Kx+Gv, with K=-E^-1 F, G=E^-1
when the
Falb--Wolovich decoupling conditions are satisfied.";

RelativeDegree::usage = 
"RelativeDegree[A,B,C] computes the vector of relative degrees of
the transfer function G[s]=C (Is-A)^-1 B. 
RelativeDegree[G,s] assumes the first argument is a transfer function.";

Until::usage=
"From R. Maeder, Programming in Mathematica: Until[body,test] checks
the test after each iteration of body.";

Kernel::usage = 
"Kernel[A] finds the null space of the matrix A.";

RangeSpace::usage = 
"RangeSpace[A] finds the range space of the matrix A.";

MatrixTrace::usage =
"MatrixTrace[A] computes the trace of the square matrix A.";

MatrixRank::usage = 
"MatrixRank[A], with A a numerical (real values) nxm matrix, returns
the rank of the matrix. It uses the singular value decomposition
of the matrix A.";

Poles::usage = 
"Poles[G,s] finds the roots of the LeastCommonDenominator of all minors\n
of all orders of a proper rational (matrix) transfer function G[s]. It\n
returns a list consisting of the distinct values of the poles and their\n 
multiplicities. The poles correspond to the eigenvalues of a minimal\n
realization of G[s].\n
Examples:\n 
\n
G[s_]:={{s-1,s},{-6,s-2}}/(1.25*(s+1)*(s+2))\n
Poles[G,s] returns the list {{-2,1},{-1,1}}\n
\n
G[s_]:={{(s-1)*(s+2),0,(s-1)^2},\n
   {-(s+1)*(s+2),(s-1)*(s+1),(s-1)*(s+1)}}/((s+1)*(s+2)*(s-1))\n
Poles[G,s] returns the list {{-2,2},{-1,1},{1,1}}\n
";

Zeros::usage =
"Zeros[G,s] finds the transmission zeros of a proper rational (matrix)\n
transfer funtion G[s]. They correspond to the zeros of a mimal realization\n
G[s]. I resturns a list of pairs of the distinct values of the zeros and\n
their multiplicities.\n
Examples:\n
\n
G[s_]:={{s-1,s},{-6,s-2}}/(1.25*(s+1)*(s+2))\n
Zeros[G,s] returns {}\n
\n
G[s_]:={{(s-1)*(s+2),0,(s-1)^2},\n
   {-(s+1)*(s+2),(s-1)*(s+1),(s-1)*(s+1)}}/((s+1)*(s+2)*(s-1))\n
Zeros[G,s] returns {{1,1}}\n
\n
G[s_]:={{s-1,4},{4.5,2*(s-1)}}/(s+2)\n
Zeros[G,s] returns {{4,1}}
"

ControllableRealization::usage = 
"ControllableRealization[G,s] constructs the controllable realization
of a transfer function G[s]. It handles both scalar and matrix cases.";

AssociatedHankelMatrix::usage =
"AssociatedHankelMatrix[G,s] computes the Hankel matrix associated with the
Laurent expansion of the transfer function G[s].";

LaurentSeries::usage = 
"LaurentSeries[G,s,n] computes the first n terms of the 
Laurent series expansion of G[s] about s=Infinity. It includes a
formal remainder estimate. Use Normal[...] to keep only the terms.";

McMillanDegree::usage = 
"McMillanDegree[G,s] computes the degree of the minimal
realization of a transfer function (matrix or scalar valued).";

ObservableRealization::usage = 
"ObservableRealization[G,s] constructs the observable realization
of a transfer function G[s]. It handles both scalar and matrix cases.";

MinimalRealization::usage=
"MinimalRealization[G,s] returns a minimal realization for a transfer\n
matrix G[s]. Example:\n
\n   
G[s_]:={{1/((s+2)^3 (s+5)),1/(s+5)},{1/(s+2),0}}\n
\n
{AA,BB,CC,DD}=MinimalRealization[G,s]\n
\n
returns\n
\n
AA {{-2,1,0,0},{0,-2,1,0},{0,0,-2,0},{0,0,0,-5}}
BB {{0,0},{0,0},{1,0},{1,-27}}
CC {{1/3, -1/9, 1/27, -730/27}, {0, 0, 1, 0}}
DD {{0,0},{0,0}}
\n
MinimalRealization can also be called with the synatx:\n
\n
MinimalRealization[A,B,C,D]\n
\n
if the system is defined in state space form. MinimalRealization\n
always returns a system in Jordan Canonical form. An analternative\n
irreducible realization for systems in state variable form can be\n
obtained with KalmanDecomposition.
"

KalmanDecomposition::usage=
"KalmanDecomposition[AA,BB,CC,DD,opts] returns a canonical for exhibiting\n
the four subsystems: controllable & observable, controllable & not \n
observable, not controllable & observable, not controllable & not observable.\n
An option (IrreducibleRealization->True) returns only the minimal\n
(or irreducible) system, i.e., the controllable & observable part. Example:
\n
AA={{1,1,0},{0,1,0},{0,1,1}};
BB={{0,1},{1,0},{0,1}};
CC={{1,1,1}};
DD={{0,0}};
\n
{Anew,Bnew,Cnew,Dnew}=KalmanDecomposition[AA,BB,CC,DD];\n
\n
returns
Anew {{1,1,0},{0,1,0},{0,0,1}}
Bnew {{0,1},{1,0},{0,0}}
Cnew {{2,1,0}}
Dnew {{0,0}}
\n
Using the option\n
{Anew,Bnew,Cnew,Dnew}=\n
KalmanDecomposition[AA,BB,CC,DD,IrreducibleRealization->True];\n
\n
Anew {{1,1},{0,1}}
Bnew {{0,1},{1,0}}
Cnew {{2,1}}
Dnew {{0,0}}
"

BlockCompanionMatrix::usage = 
"BlockCompanionMatrix[poly_coeff, block_size] 
constructs a block companion matrix with 
the blocks of size block_size, an integer. 
The poly_coeff is the list of coefficients
of a monic polynomial with the coefficient 1 
of the highest power omitted.

Example: BlockCompanionMatrix[{p0,p1,p2},2]
associated with the polynomial 
p[s] = p0 + p1 s + p2 s^2 + s^3
and block size 2.";

StateSpaceToTransferFunction::usage=
"StateSpaceToTransferFunction[A,B,C,D,s] computes the
transfer function of a linear system defined by the state
equation 
                xdot=A x + B u, y=C x + D u, 
It returns the function 
                C(Is-A)^{-1}B + D
All entries must be matrices. An error is given if the
dimensions are not correct.";

LeastCommonDenominator::usage = 
"LeastCommonDenominator[G,s] finds the least 
common denominator of the elements of the 
matrix transfer function G[s].
G[s] must be a proper rational function of s; 
and the element denominators must be monic 
polynomials in s. Common factors between 
numerator and denominator are cancelled.";

PolePlace::usage = 
"PolePlace[A,b,poles], with b a vector, uses Ackermann's formula 
to place the poles of the closed loop
system  A-bk^T at the locations {pole_1, ... , pole_n}
defined in the list poles. Complex poles in the
list must appear in consecutive complex conjugate pairs.

PolePlace[A,B,poles], with B a matrix, finds the ``most
controllable'' port of the pair (A,B), defined as the port yielding
the lowest condition number for the Controllability matrix, and uses
Ackermann's formula to place the poles of A-BK through this port. 
It returns a matrix gain. 

PolePlace[A,B,port,poles], with B a matrix and port an integer
between 1 and the number of columns of B, uses Ackermann's formula to
place the poles through the designated port.";

ObservablePair::usage =
"ObservablePair[A,C] tests the pair of matrices (A,C) to
determine if they are observable";

ControllablePair::usage =
"ControllablePair[A,B] tests the pair of matrices (A,B) to
determine if they are Controllable";

ObservabilityMatrix::usage = 
"ObservabilityMatrix[A,C] computes the observability matrix
defined by the matrices A,C.";

ControllabilityMatrix::usage = 
"ControllabilityMatrix[A,B] computes the Controllability matrix
defined by the matrices A,B.";

Jacobian::usage= "Jacobian[f_, x_] returns the Jacobian matrix
of f  
with respect to x. Both f and x must be lists.";

TaylorLinearize::usage = 
"TaylorLinearize[f,g,x,x0,u,u0] linearizes the nonlinear system\n 
\n
      .
      x = f(x,u)\n
      y = g(x,u)\n
\n
around the vectors x = x0 and u = u0.\n
The arguments may be scalars as well as vectors\n
\n
The result will have the form {A,B,C,D}.\n
\n
TaylorLinearize[F,f,g,x,x0,u,u0] linearizes the nonlinear system\n 
\n
       .
   F(x)x = f(x,u)\n
       y = g(x,u)\n
\n
with E nonsingular. The result is of the form\n
{E,A,B,C,D}.
";

RandomMatrix::usage=
"RandomMatrix[n,m] returns an n times m 
matrix of random entries between -1.0 and 1.0.
RandomMatrix[n] returns an n times n matrix 
of random entries between -1.0 and 1.0.";

PoleZeroPlot::usage = 
"PoleZeroPlot[G,s] \n
produces a plot of the transfer function \n
of a (scalar) continuous time system. \n
G[s] is declared as a function of s, a complex number.\n
If the poles are complex,\n
the graph is colored using the argument of G[s].\n
\n
Example: \n
   G[s_]:=10/((s+5)(s^2 + 5 s + 2)(s^2 + 1 s + 3))\n
   PoleZeroPlot[G,s] \n"

RootLocus::usage = 
"RootLocus[G,s,k,kmin,kmax,kdel]  \n
produces a root locus plot of the transfer function \n
of a (scalar) continuous time system. \n
G[s] is declared as a function
of s, a complex number. \n
sigma=Re[s],omega=Im[s], and\n
[kmin,kmax] is the range of feedback gains.\n
Default values are provided for the arguments, if omitted.\n\n
Example: \n
    G[s_]:=((s+10)(s+4))/((s+5)(s^2 + 2 s + 2)(s^2 + 1 s + 1)) \n
    RootLocus[G,s,k,-10,+20,0.5]  \n
"

Nyquist::usage = 
"Nyquist[G,s,minomega,maxomega]  \n
produces a Nyquist plot of the transfer function \n
of a (scalar) continuous time system. \n
G[s] is declared as a function
of s, a complex number. \n
sigma=Re[s],omega=Im[s], and\n
[minomega,maxomega] is the range of frequencies. \n
Default values are provided for the arguments, if omitted.\n\n
ColorNyquist[G,s,minomega,maxomega,delomega]  \n
(note the additional argument for the frequency step) \n
produces a plot with 
the colors proportional to the frequency.\n\n
Example: \n
    G[s_]:=((s+10)(s+4))/((s+5)(s^2 + 2 s + 2)(s^2 + 1 s + 1)) \n
    Nyquist[G,s,-20,+20]\n
    ColorNyquist[G,s,-20,+20,0.2]\n
    \n
Nyquist and ColorNyquist have the following Options:\n
\n
	PlotRange\n
   Bandwidth\n
   SensitivityPeak\n
   Margins
"

ColorNyquist::usage =
"See Nyquist"

InverseNyquist::usage = 
"InverseNyquist[G,s,minomega,maxomega]  \n
produces an inverse Nyquist plot of the transfer function \n
of a (scalar) continuous time system. \n
G[s] is declared as a function
of s, a complex number. \n
sigma=Re[s],omega=Im[s], and\n
[minomega,maxomega] is the range of frequencies.\n
Default values are provided for the arguments, if omitted."

Bode::usage = 
"Bode[G,s,minomega,maxomega]  \n
produces a Bode plot of the transfer function
of a (scalar) continuous time system.\n
G[s] is declared as a function
of s, a complex number. \n
sigma=Re[s],omega=Im[s], and\n
[minomega,maxomega] is the range of frequencies.\n
Default values are provided for the arguments, if omitted."
   
(********* Options *********)

PlotRange::usage="The standard plot option PlotRange is an option of\n
Nyquist and ColorNyquist. PlotRange->{{minReal,maxReal},{minImag,maxImag}}.
"

SenitivityPeak::usage="SensitivityPeak->r, r a real, positive number > 1 puts\n
a sensitivity peak circle (green) of radius 1/r and centered at {-1,0} on the Nyquist\n
plot. If the Nyquist graph does not penetrate the circle, the closed loop\n
sensitivity function peak is < r. It is an option of Nyquist and ColorNyquist.
"

Bandwidth::usage="Bandwidth->True puts a circle (red) on the Nyquist plot. The\n
intersection with the Nyquist plot identifies the closed loop system\n
bandwidth (based on Sensitivity function defiinition). It is an\n
option of Nyquist and ColorNyquist.
"

Margins::usage="Margins->True puts a unit circle (blue) centered at {0,0}\n
on the Nyquist plot. It can be used to identify gain and phase margins.\n
 It is an option of Nyquist and ColorNyquist.
"


Options[Nyquist]={PlotRange->All, SensitivityPeak->0, Bandwidth->False,Margins->False};
Options[ColorNyquist]={PlotRange->All, SensitivityPeak->0, Bandwidth->False,Margins->False};



 
Begin["`Private`"];   (* begin the private context *)
 
Clear[LyapunovEquation];
LyapunovEquation[A_?MatrixQ,Q_?MatrixQ]:=Module[{RR,n,m},
		{n,m}=Dimensions[A];
		If[(Dimensions[Q]=!={m,n})||(m=!=n),
      Print["Arguments have incorrect dimensions."];Return[{}]];
		RR=Table[ rr[i1,j1], {i1,1,n},{j1,1,n}];
		RR=RR/.Flatten[ Solve[ RR.A+Transpose[A].RR==-Q,Flatten[RR]]]
		]
SetAttribute[LyapunovEquation,ReadProtected];
SetAttribute[LyapunovEquation,Protected];
SetAttribute[LyapunovEquation,Locked];

Clear[AlgebraicRiccatiEq];
AlgebraicRiccatiEq[A_,Q_,U_]:=Module[{H1,H2,H,t,n,Theta11,Theta21,
   Theta,Pt,P,Rules1},
    H1=Join[A,-Q];
    H2=Join[-U,-Transpose[A]];
    H=Transpose[Join[Transpose[H1],Transpose[H2]]];
    Theta=HamiltonianMatExp[Rationalize[H],t];
    n=Length[A];
    Rules1={(Exp[a_?NumberQ*t]/;a>0)->0,Exp[t]->0};
    Theta11=Theta[[ Range[1,n],Range[1,n] ]]/.Rules1;
    Theta21=Theta[[ Range[n+1,2*n],Range[1,n] ]]/.Rules1;
    Pt=Simplify[BInvA[Theta11,Theta21]];
    P=Pt;
    Limit[P,t->-Infinity]
    ];
SetAttribute[AlgebraicRiccatiEq,ReadProtected];
SetAttribute[AlgebraicRiccatiEq,Locked];

Clear[AlgebraicRiccatiEquation];
AlgebraicRiccatiEquation[A_,Q_,U_]:=Module[{H1,H2,H,t,n,Theta11,Theta21,
   P,Eigs,V,Seigs,IndexList},
    n=Length[A];
    H1=Join[A,-Q];
    H2=Join[-U,-Transpose[A]];
    H=Transpose[Join[Transpose[H1],Transpose[H2]]];
    {Eigs,V}=Eigensystem[H];
    {Seigs,IndexList}=EigSort[Eigs];
    Theta11=Transpose[V[[ IndexList[[Range[1,n]]],Range[1,n] ]]];
    Theta21=Transpose[V[[ IndexList[[Range[1,n]]],Range[n+1,2*n] ]]];
    If[MatrixRank[Theta11]==Length[Theta11],
    P=Re[ComplexExpand[Simplify[BInvA[Theta11,Theta21]]]],
    Print["Hamiltonian has nonsimple divisors."];
    Print["This could take a while."];
    P=AlgebraicRiccatiEq[A,Q,U]];
    {P,Seigs[[ Range[1,n] ]]}
        ];
SetAttribute[AlgebraicRiccatiEquation,ReadProtected];
SetAttribute[AlgebraicRiccatiEquation,Locked];

Clear[LQR];
LQR[A_?SquareMatrixQ,B_?MatrixQ,Q_?MatrixQ,R_?MatrixQ]:=
    Module[{RIBT,U,P,Eigs},
        RIBT=InvAB[R,Transpose[B]];
        U=B.RIBT;
        {P,Eigs}=AlgebraicRiccatiEquation[A,Q,U];
        K=-RIBT.P;
        {K,P,Eigs}
        ]
LQR[A_?SquareMatrixQ,B_?VectorQ,Q_?MatrixQ,R_?MatrixQ]:=
   LQR[A,Transpose[{B}],Q,R];
LQR[A_?SquareMatrixQ,B_?VectorQ,Q_?MatrixQ,R_Real]:=
   LQR[A,Transpose[{B}],Q,{{R}}];
LQR[A_Real,B_Real,Q_Real,R_Real]:=
   LQR[{{A}},{{B}},{{Q}},{{R}}];
SetAttribute[LQR,ReadProtected];
SetAttribute[LQR,Locked];


Clear[LQE];
LQE[A_?SquareMatrixQ,G_?MatrixQ,C_?MatrixQ,Q_?MatrixQ,R_?MatrixQ]:= 
  Module[{K,P}, 
        {K,P,Eigs} = LQR[Transpose[A],Transpose[C],G.Q.Transpose[G],R];
        {Transpose[K],Transpose[P],Eigs}
        ]
LQE[A_?SquareMatrixQ,G_?MatrixQ,C_?VectorQ,Q_?MatrixQ,R_?MatrixQ]:=
  LQE[A,G,{C},Q,R];
LQE[A_?SquareMatrixQ,G_?MatrixQ,C_?VectorQ,Q_?MatrixQ,R_Real]:=
  LQE[A,G,{C},Q,{{R}}];
LQE[A_?SquareMatrixQ,G_?VectorQ,C_?MatrixQ,Q_?MatrixQ,R_?MatrixQ]:=
  LQE[A,Transpose[{G}],C,Q,R];
LQE[A_?SquareMatrixQ,G_?VectorQ,C_?MatrixQ,Q_Real,R_?MatrixQ]:=
  LQE[A,Transpose[{G}],C,{{Q}},R];
LQE[A_?SquareMatrixQ,G_?VectorQ,C_?VectorQ,Q_Real,R_Real]:=
  LQE[A,Transpose[{G}],{C},{{Q}},{{R}}];
LQE[A_Real,G_Real,C_Real,Q_Real,R_Real]:=
  LQE[{{A}},{{G}},{{C}},{{Q}},{{R}}];
SetAttribute[LQE,ReadProtected];
SetAttribute[LQE,Locked];
    

Clear[McMillanDegree];
McMillanDegree[G_,s_]:=  
    Module[{lcd,len,real,A,B},
        lcd=LeastCommonDenominator[G,s]; (* LCD of elements *)
        len=Exponent[lcd,s];             (* Highest power *)
real=ObservableRealization[G,s];    (* Compute Observable Realization *)
A=real[[1]];
B=real[[2]];
If[MatrixQ[G[s]],
     MatrixRank[AppendRows @@ NestList[A . #&, B, len-1]], (* MIMO Case *)
     MatrixRank[NestList[A . #&, B, len-1]]]         (* SIMO Case *)
]
SetAttribute[McMillanDegree,ReadProtected];
SetAttribute[McMillanDegree,Locked];


(* ObservableRealization used in McMillanDegree *)
Clear[ObservableRealization];
ObservableRealization[G_,s_]:=Module[{p,A,B,C,lcd,len,coeff,list,L0,listend},
If[MatrixQ[G[s]],
        p=Dimensions[G[s]][[1]];     (* Number of outputs *)
        lcd=LeastCommonDenominator[G,s]; (* LCD of elements *)
        len=Exponent[lcd,s];               (* Highest power *)
        coeff=Drop[CoefficientList[lcd,s],-1]; (* Delete coeff s^n *)
A=BlockCompanionMatrix[coeff,p];
list=LaurentCoeff[G,s];
L0=list[[1]];
listend=Drop[list,1];
B=Fold[AppendColumns,L0,listend];
C=
Transpose[Flatten[Prepend[Table[ZeroMatrix[p],{len-1}],IdentityMatrix[p]],1]];
{A,B,C}, 
        (* Now the case when g[s] is a scalar *)
        lcd=LeastCommonDenominator[G,s]; (* LCD of elements *)
        len=Exponent[lcd,s];               (* Highest power *)
        coeff=Drop[CoefficientList[lcd,s],-1]; (* Delete coeff s^n *)
A=BlockCompanionMatrix[coeff,1];
B=LaurentCoeff[G,s];
C=Prepend[Table[0,{len-1}],1];
{A,B,C}
]];
SetAttribute[ObservableRealization,ReadProtected];
SetAttribute[ObservableRealization,Locked];


(*
LaurentCoeff::usage = 
"LaurentCoeff[G,s] is a list of coeff. of s^(-k), k=1,2,...,len
Laurent Series expansion of G[s] where len is the degree of the
LeastCommonDenominator polynomial of G[s].
It is used in constructing an observable realization for G[s]."
*)
Clear[LaurentCoeff];
LaurentCoeff[G_,s_]:=  
    Module[{lcd,len,powers,expand},
        lcd=LeastCommonDenominator[G,s]; (* LCD of elements *)
        len=Exponent[lcd,s];               (* Highest power *)
        powers=Table[-k,{k,1,len}];         (* List of powers *)
        expand=Normal[LaurentSeries[G,s,len]];
Map[Coefficient[expand,s^#]&,powers]];
SetAttribute[LaurentCoeff,ReadProtected];
SetAttribute[LaurentCoeff,Locked];

Clear[LaurentSeries];
LaurentSeries[G_,s_,n_?IntegerQ]:=Module[{q},
                Series[G[1/q],{q,0,n}]/.{q->1/s}];
SetAttribute[LaurentSeries,ReadProtected];
SetAttribute[LaurentSeries,Locked];


row[a_,b_,n_,m_]:=Module[{list},
                         list=Table[b,{n-1}];
                         Transpose[Flatten[Insert[list,a,m],1]]];


top[a_,b_,n_]:=Flatten[Map[row[a,b,n,#]&,Table[{i},{i,2,n}]],1];


bottom[a_,list_]:=Transpose[Flatten[Map[(a #)&,(-1 list)],1]];

Clear[BlockCompanionMatrix];
BlockCompanionMatrix[p_List, block_Integer] :=
                Module[{n = Length[p],im,zm,aa,bb},
                im=IdentityMatrix[block];
                zm=ZeroMatrix[block];
                aa=top[im,zm,n];
                bb=bottom[im,p];
                Join[aa,bb]];
SetAttribute[BlockCompanionMatrix,ReadProtected];
SetAttribute[BlockCompanionMatrix,Locked];

Clear[StateSpaceToTransferFunction];
StateSpaceToTransferFunction[Amat_,Bmat_,Cmat_,s_]:=
        Module[{Dmat},
        Dmat=ZeroMatrix[Dimensions[Cmat][[1]],Dimensions[Bmat][[2]]];
        StateSpaceToTransferFunction[Amat,Bmat,Cmat,Dmat,s]
    ]

StateSpaceToTransferFunction[Amat_,Bmat_,Cmat_,Dmat_,s_]:=
Module[
     {nst,nrb,ncb,nrc,ncc,nrd,ncd},
If[Not[MatrixQ[Amat]] || Not[MatrixQ[Bmat]] || 
       Not[MatrixQ[Cmat]] || Not[MatrixQ[Dmat]],
      Print["Error[TransferFunction]::Arguments must be matrices."],
      nst=Dimensions[Amat][[1]];
     {nrb,ncb}=Dimensions[Bmat];
     {nrc,ncc}=Dimensions[Cmat];
     {nrd,ncd}=Dimensions[Dmat];
If[(nrb!=nst || ncc!= nst || ncd != ncb || nrd != nrc),
     Print["Error[TransferFunction]::Matrix dimensions are incorrect"],
     Together[Dmat+Dot[Cmat,Dot[Inverse[s IdentityMatrix[nst]
                                - Amat],Bmat]]]
]]];
SetAttribute[StateSpaceToTransferFunction,ReadProtected];
SetAttribute[StateSpaceToTransferFunction,Locked];


Clear[LeastCommonDenominator];
LeastCommonDenominator[G_,s_]:=Module[{f,d,d1},
If[MatrixQ[G[s]],
Apply[PolynomialLCM,Flatten[Denominator[SimplifyArray[G[s]]]]],
PolynomialLCM[Denominator[Simplify[G[s]]]]
]];
SetAttribute[LeastCommonDenominator,ReadProtected];
SetAttribute[LeastCommonDenominator,Locked];


MatrixRank[A_?MatrixQ]:= Length[SingularValues[N[A]][[2]]];

(*
MakeGain::usage = 
"MakeGain[B,port,gainvector] inserts the gain
vector computed in pole placement via a designated port (column
of B) in a zero matrix with dimensions = dimensions of Transpose[B].";
*)
Clear[MakeGain];
MakeGain[(B_)?MatrixQ,port_?IntegerQ, (k_)?VectorQ] := 
  Module[{dim}, dim = Dimensions[Transpose[B]]; 
    ReplacePart[ZeroMatrix[dim[[1]], dim[[2]]], k,port]];
SetAttribute[MakeGain,ReadProtected];
SetAttribute[MakeGain,Locked];


Clear[PolePlace];
PolePlace[A_?SquareMatrixQ,B_?MatrixQ,poles_?ListQ]:=
                Module[{bestport,list,condnums,portgain},
                   list=Table[
                ControllabilityMatrix[A,Transpose[B][[i]]],
                        {i,1,Dimensions[B][[2]]}];
                condnums=
                Table[ConditionNumber[N[list[[i]]]],{i,1,Length[list]}];
                    bestport=ArgMin[condnums];  (* Place the poles from
                                                   this port *)
               portgain=PortGain[A,B,bestport,poles]; (* Gain at the port *)
               MakeGain[B,bestport,portgain]];

(* Use the single input case to compute the gain vector *)

PortGain[A_?SquareMatrixQ,B_?MatrixQ,port_?IntegerQ,poles_?ListQ]:=
                Module[{portgain},
                portgain= 
                    PolePlace[A,Flatten[TakeColumns[B,{port,port}]],poles]];

PolePlace[A_?SquareMatrixQ,B_?MatrixQ,port_?IntegerQ,poles_?ListQ]:=
                Module[{portgain},
                portgain=
                    PolePlace[A,Flatten[TakeColumns[B,{port,port}]],poles];
               MakeGain[B,port,portgain]];

PolePlace[A_?SquareMatrixQ,b_?VectorQ,poles_?ListQ]:=
    Module[{lengthb,gains},
                           lengthb=Length[b];
    gains = Append[Flatten[ZeroMatrix[lengthb-1,1]],1] . 
    Inverse[ControllabilityMatrix[A,b]] . MatrixPolynomialRoots[poles,A];
    Return[gains]];
SetAttribute[PolePlace,ReadProtected];
SetAttribute[PolePlace,Locked];

Clear[ConditionNumber];
ConditionNumber[(m_)?MatrixQ] := 
                      Module[{sv},
                           sv=SingularValues[N[m]][[2]];
                      If[Length[sv]==Dimensions[m][[1]],
                         Last[sv]/First[sv],
                         Infinity]]
SetAttribute[ConditionNumber,ReadProtected];
SetAttribute[ConditionNumber,Locked];

Clear[ArgMin];
ArgMin[list_?ListQ] := 
      Module[{min, i}, 
                min=Min[list];
                i=0;
        While[True, i += 1; 
             If[list[[i]] === min, Return[i]; Break[]]]]
SetAttribute[ArgMin,ReadProtected];
SetAttribute[ArgMin,Locked];

Clear[ArgMax];
ArgMax[list_?ListQ] := 
      Module[{max, i}, 
                max=Max[list];
                i=0;
        While[True, i += 1; 
             If[list[[i]] === max, Return[i]; Break[]]]]
SetAttribute[ArgMax,ReadProtected];
SetAttribute[ArgMax,Locked];

(* Construct a polynomial from its roots *)
Clear[PolynomialRoots];
PolynomialRoots[roots_?ListQ,s_]:=Expand[Times @@ Map[(s-#)&,roots]];  
SetAttribute[PolynomialRoots,ReadProtected];
SetAttribute[PolynomialRoots,Locked];

(* Construct a matrix polynomial from its "roots" *)
Clear[MatrixPolynomialRoots];
MatrixPolynomialRoots[roots_?ListQ,A_?SquareMatrixQ]:=Module[{dimA},
                dimA=Dimensions[A][[2]];
     Expand[Dot @@ Map[(A- # IdentityMatrix[dimA])&,roots]]]
SetAttribute[MatrixPolynomialRoots,ReadProtected];
SetAttribute[MatrixPolynomialRoots,Locked];

(* Construct a polynomial from its coefficients *)
Clear[PolynomialCoefficients];
PolynomialCoefficients[coeff_?ListQ,s_]:=Module[{lengthcoeff},
                      lengthcoeff=Length[coeff];
       Inner[Times,coeff,Table[s^(lengthcoeff-1-k),{k,0,lengthcoeff-1}],Plus]]
SetAttribute[PolynomialCoefficients,ReadProtected];
SetAttribute[PolynomialCoefficients,Locked];

(* Construct a matrix polynomial from its coefficients *)
Clear[MatrixPolynomialCoefficients];
MatrixPolynomialCoefficients[coeff_,A_?SquareMatrixQ]:=
Module[{lengthcoeff,listpowers},
                               lengthcoeff=Length[coeff];
        listpowers=Table[MatrixPower[A,lengthcoeff-1-k],{k,0,lengthcoeff-1}];
                     Plus @@ ((#1 #2)& @@ {coeff,listpowers})]
SetAttribute[MatrixPolynomialCoefficients,ReadProtected];
SetAttribute[MatrixPolynomialCoefficients,Locked];

Clear[ControllablePair];
ControllablePair[a_?SquareMatrixQ, b_?MatrixQ] :=
Module[{n = Dimensions[a][[1]]},
   Rank[ControllabilityMatrix[a,b]]===n
] /; Dimensions[a][[2]] === Dimensions[b][[1]];

ControllablePair[a_?SquareMatrixQ, b_?VectorQ] :=
Module[{n = Dimensions[a][[1]]},
   Rank[ControllabilityMatrix[a,b]]===n
] /; Dimensions[a][[2]] === Length[b];
SetAttribute[ControllablePair,ReadProtected];
SetAttribute[ControllablePair,Locked];

Clear[ObservablePair];
ObservablePair[a_?SquareMatrixQ, c_?MatrixQ] :=
Module[{n = Dimensions[a][[1]]},
   Rank[ObservabilityMatrix[a,c]]===n
]/; Dimensions[a][[2]] === Dimensions[c][[2]];

ObservablePair[a_?SquareMatrixQ, c_?VectorQ] :=
Module[{n = Dimensions[a][[1]]},
   Rank[ObservabilityMatrix[a,c]]===n
] /; Dimensions[a][[2]] === Length[c];
SetAttribute[ObservablePair,ReadProtected];
SetAttribute[ObservablePair,Locked];

Clear[ObservabilityMatrix];
ObservabilityMatrix[a_?SquareMatrixQ, c_?MatrixQ] := Module[{},
If[Not[TestDataObservabilityMatrix[a,c]],
Print["Error[ObservabilityMatrix]::Data dimensions are incorrect"],
  AppendColumns @@ NestList[# . a&, c, Length[a]-1]]];

ObservabilityMatrix[a_?SquareMatrixQ, c_?VectorQ] := Module[{},
If[Not[TestDataObservabilityMatrix[a,c]],
Print["Error[ObservabilityMatrix]::Data dimensions are incorrect"],
                   NestList[# . a&, c, Length[a]-1]]];
SetAttribute[ObservabilityMatrix,ReadProtected];
SetAttribute[ObservabilityMatrix,Locked];

Clear[TestDataObservabilityMatrix];
TestDataObservabilityMatrix[a_?SquareMatrixQ, b_?VectorQ] := Module[{da,db},
                da=Dimensions[a][[2]];
                db=Length[b];
                If[da==db,True,False]];

TestDataObservabilityMatrix[a_?SquareMatrixQ, b_?MatrixQ] := Module[{da,db},
                da=Dimensions[a][[2]];
                db=Dimensions[b][[2]];
                If[da==db,True,False]];
SetAttribute[TestDataObservabilityMatrix,ReadProtected];
SetAttribute[TestDataObservabilityMatrix,Locked];

Clear[ControllabilityMatrix];
ControllabilityMatrix[a_?SquareMatrixQ, b_?MatrixQ] := Module[{},
If[Not[TestDataControllabilityMatrix[a,b]],
Print["Error[ControllabilityMatrix]::Data dimensions are incorrect"],
      AppendRows @@ NestList[a . #&, b, Length[a]-1]]];

(* ControllabilityMatrix for the case when the B "matrix" is a vector *)

ControllabilityMatrix[a_?SquareMatrixQ, b_?VectorQ] := Module[{},
If[Not[TestDataControllabilityMatrix[a,b]],
Print["Error[ControllabilityMatrix]::Data dimensions are incorrect"],   
                    Transpose[NestList[a . #&, b, Length[a]-1]]]];
SetAttribute[ControllabilityMatrix,ReadProtected];
SetAttribute[ControllabilityMatrix,Locked];

Clear[TestDataControllabilityMatrix];
TestDataControllabilityMatrix[a_?SquareMatrixQ, b_?VectorQ] := Module[{da,db},
                da=Dimensions[a][[2]];
                db=Length[b];
                If[da==db,True,False]];
TestDataControllabilityMatrix[a_?SquareMatrixQ, b_?MatrixQ] := Module[{da,db},
                da=Dimensions[a][[2]];
                db=Dimensions[b][[1]];
                If[da==db,True,False]];
SetAttribute[TestDataControllabilityMatrix,ReadProtected];
SetAttribute[TestDataControllabilityMatrix,Locked];


Unprotect[Jacobian];
Jacobian[f_List, x_List] := Outer[D, f, x]
Protect[Jacobian];

Clear[TaylorLinearize];
TaylorLinearize[F_?MatrixQ,f_List, g_List, x_List, x0_List, u_List, u0_List] :=
        Join[{F/.Thread[x -> x0]},TaylorLinearize[f,g,x,x0,u,u0]];
TaylorLinearize[f_List, g_List, x_List, x0_List, u_List, u0_List] :=
         TaylorLinearize[f, g, x, u]/.
         Flatten[{Thread[x -> x0], Thread[u -> u0]}];
TaylorLinearize[f_List, g_List, x_List, u_List] :=
{Jacobian[f, x], Jacobian[f, u], Jacobian[g, x], Jacobian[g, u]
            } /; (Length[f] === Length[x] );
SetAttribute[TaylorLinearize,ReadProtected];
SetAttribute[TaylorLinearize,Locked];

Format[LinearSystem[x__ ]] := 
   ColumnForm[ MatrixForm /@ {x} ];

Clear[SystemForm];
SystemForm[LinearSystem[a_, b_, c_, d_]] := Block[{xeqn,yeqn},
 xeqn= ColumnForm[ a . Array["x", {Length[a]}] ] + 
 ColumnForm[ b . Array["u", {Length[Transpose[b]]} ]] ;

 yeqn= ColumnForm[ c . Array["x", {Length[Transpose[c]]}] ] + 
 ColumnForm[ d . Array["u", {Length[d]}] ];
 Return[ColumnForm[{xeqn," ",yeqn}]]
];
SetAttribute[SystemForm,ReadProtected];
SetAttribute[SystemForm,Locked];

Clear[RandomMatrix];
RandomMatrix[n_Integer,m_Integer]:=
                     Table[Random[],{n},{m}]-Table[Random[],{n},{m}];
RandomMatrix[n_Integer]:=RandomMatrix[n,n];
SetAttribute[RandomMatrix,ReadProtected];
SetAttribute[RandomMatrix,Locked];


Clear[AssociatedHankelMatrix];
AssociatedHankelMatrix[G_,s_]:=
    Module[{lcd,len,expand,powers,list,shifts},
        lcd=LeastCommonDenominator[G,s];   (* LCD of elements *)
        len=Exponent[lcd,s];               (* Highest power *)
        expand=Normal[LaurentSeries[G,s,2(len-1)]];
        powers=Table[-k,{k,1,2(len-1)}];   (* List of powers *)
        list=Map[Coefficient[expand,s^#]&,powers];
        shifts=Table[k,{k,1,len-1}];
Map[hrow[list,len,#]&,shifts]
]
SetAttribute[AssociatedHankelMatrix,ReadProtected];
SetAttribute[AssociatedHankelMatrix,Locked];

Clear[hrow]; 
hrow[list_,len_,shift_]:=Part[list,pp[len,shift]]
Clear[pp];
pp[n_,shift_]:=Table[k+shift,{k,0,n-1}]

Clear[ControllableRealization];
ControllableRealization[G_,s_]:=Module[{m,A,B,C,lcd,len,coeff,powers,
                                       list},
If[MatrixQ[G[s]],
        m=Dimensions[G[s]][[2]];           (* Number of control inputs *)
        lcd=LeastCommonDenominator[G,s];   (* LCD of elements *)
        len=Exponent[lcd,s];               (* Highest power *)
        coeff=Drop[CoefficientList[lcd,s],-1]; (* Delete coeff s^n *)
A=BlockCompanionMatrix[coeff,m];
(* Print["A = ",A//MatrixForm];*)
B=Flatten[Append[Table[ZeroMatrix[m],{len-1}],IdentityMatrix[m]],1];
(* Print["B = ",B//MatrixForm];*)
h=Expand[Together[SimplifyArray[lcd G[s]]]];
Q0=h/.s->0;                           (* Coeff of s^0 in prod. *)
powers=Table[k,{k,1,len-1}];          (* List of powers *)
list=Map[Coefficient[h,s^#]&,powers]; (* Coeff of s^k, k=1,...,len-1 *)
C=Fold[AppendRows,Q0,list];           (* Lists coeffs in right form *)
(* Print["C = ",C//MatrixForm]; *)
{A,B,C}, 
        (* Now the case when g[s] is a scalar *)
        lcd=LeastCommonDenominator[G,s];       (* LCD of elements *)
        len=Exponent[lcd,s];                   (* Highest power *)
        coeff=Drop[CoefficientList[lcd,s],-1]; (* Delete coeff s^n *)
A=BlockCompanionMatrix[coeff,1];
Print["A = ",A//MatrixForm];
B=Append[Table[0,{len-1}],1];
Print["b = ",B//MatrixForm];
h=Expand[Together[lcd G[s]]];
Q0=h/.s->0;                           (* Coeff of s^0 in prod. *)
powers=Table[k,{k,1,len-1}];          (* List of powers *)
list=Map[Coefficient[h,s^#]&,powers]; (* Coeff of s^k, k=1,...,len-1 *)
C=Prepend[list,Q0];
Print["c = ",C//MatrixForm];
{A,B,C}
]];
SetAttribute[ControllableRealization,ReadProtected];
SetAttribute[ControllableRealization,Locked];

Clear[Poles];
(* 
Poles[G_,s_]:=
    Module[{lcd,polerules,polelist,distinctpoles},
        lcd[s]=LeastCommonDenominator[G,s];
        polerules=Solve[lcd[s]== 0,s];
        If[Length[polerules]==0,polelist={},
            polelist= (s/.polerules)];
        distinctpoles=Intersection[polelist,polelist];
        Map[({#,Count[polelist,#]})&,distinctpoles]];
*)
Poles[G_,s_]:=
    Module[{lcd,polerules,polelist,distinctpoles,GG,MS},
		If[MatrixQ[G[s]],GG=Rationalize[G[s]],GG={{Rationalize[G[s]]}}];
		MS=Together[Flatten[Map[Minors[GG,#]&,Range[Min[Dimensions[GG]]]]]];
		lcd=Apply[PolynomialLCM,Denominator[SimplifyArray[MS]]];
      polerules=Solve[lcd==0,s];
      If[Length[polerules]==0,polelist={},
          polelist= (s/.polerules)];
      distinctpoles=Intersection[polelist,polelist];
      Map[({#,Count[polelist,#]})&,distinctpoles]];
SetAttribute[Poles,ReadProtected];
SetAttribute[Poles,Locked];

Clear[Zeros];
Zeros[G_,s_]:= 
  Module[{polepoly,zeropoly,AdjustList,k,zerorules,zerolist,distinctzeros,GG,
      MS,MZ},
		GG=Rationalize[G[s]];
		MS=Together[Flatten[Map[Minors[GG,#]&,Range[Min[Dimensions[GG]]]]]];
	   polepoly=Apply[PolynomialLCM,Denominator[SimplifyArray[MS]]];
		k=Rank[GG];
		MZ=Simplify[Together[Flatten[Minors[GG,k]]]];
		AdjustList=Simplify[polepoly/Denominator[MZ]];
		AdjustMinors=Inner[Times,Numerator[MZ],AdjustList,List];
		zeropoly=Apply[PolynomialGCD,AdjustMinors];
		zerorules=Solve[zeropoly==0,s];
      If[Length[zerorules]==0,zerolist={},
            zerolist= (s/.zerorules)];
      distinctzeros=Intersection[zerolist,zerolist];
      Map[({#,Count[zerolist,#]})&,distinctzeros]
     ]
SetAttribute[Zeros,ReadProtected];
SetAttribute[Zeros,Locked];

Clear[Kernel];
Kernel[A_?MatrixQ] := NullSpace[A];
SetAttribute[Kernel,ReadProtected];
SetAttribute[Kernel,Locked];

Clear[RangeSpace];
RangeSpace[X_?MatrixQ]:=Module[{XX},
        XX=RowReduce[Transpose[X]];
        XX//.{XX->If[XX[[-1]].XX[[-1]]==0,Drop[XX,-1],XX]}
];
SetAttribute[RangeSpace,ReadProtected];
SetAttribute[RangeSpace,Locked];

Clear[MatrixTrace];
MatrixTrace[A_?SquareMatrixQ]:=
                Plus @@ Transpose[A,{1,1}];
SetAttribute[MatrixTrace,ReadProtected];
SetAttribute[MatrixTrace,Locked];

Clear[Until];
Until[body_,test_]:=Module[{t},For[t=False,!t,t=test,body]];
SetAttribute[Until,ReadProtected];
SetAttribute[Until,Locked];

Clear[RelativeDegree];
RelativeDegree[g_,s_]:=Module[{pow},
pow=Exponent[LeastCommonDenominator[g,s],s];
Return[-Map[Max[#]&,Exponent[Normal[LaurentSeries[g,s,pow]],s]]]];

RelativeDegree[a_,b_,c_]:=Module[{nrc,ncb,dmat,g,s},
nrc=Dimensions[c][[1]];
ncb=Dimensions[b][[2]];
dmat=ZeroMatrix[nrc,ncb];
g[p_]:=StateSpaceToTransferFunction[a,b,c,dmat,p];
RelativeDegree[g,s]];
SetAttribute[RelativeDegree,ReadProtected];
SetAttribute[RelativeDegree,Locked];

Clear[DecouplingControl];
DecouplingControl[A_,B_,C_]:=Module[{rd,Dmat,E,F,K,G},
rd=RelativeDegree[A,B,C];
Dmat=Map[(C[[#]].MatrixPower[A,rd[[#]]-1])&,Range[1,Length[rd]]];
E=Dmat.B;F=Dmat.A;
If[Det[E]==0,(Print["Decoupling conditions fail"];Break[]),
(G=Inverse[E];K=-G.F;
Print["Feedback gain = ",K];
Print["Control coordinates = ",G];
Return[{K,G}])] (* End If[] statement *)
];
SetAttribute[DecouplingControl,ReadProtected];
SetAttribute[DecouplingControl,Locked];

(*************************************

     GRAPHICS Functions

**************************************)
Clear[ColorNyquist];
ColorNyquist[g_,s_,minomega_,maxomega_,delomega_,opts___]:=
	Module[{flabel=TraditionalForm[g[s]],fname,sigma=0,omega,
          nlocus,list,nlocusplt,omeganorm,PlotRange0,Sens,
          Band,Marg,SensCirc,BandCirc,MargCirc,RR,p1,p2},
	Sens=1/(SensitivityPeak/.{opts}/.SensitivityPeak->Infinity);
	If[Bandwidth/.{opts}/.Bandwidth->False,Band=Sqrt[2],Band=0];
	If[Margins/.{opts}/.Margins->False,Marg=1,Marg=0];
(* Generate the values for the plot. *)
   rangeomega=N[(maxomega-minomega)/delomega];
(* Fix the alignment between the legend color and the frequency value 
    5/25/92 *)
	omeganorm[x_]:=(x-minomega)/Abs[maxomega-minomega];
	Off[Power::infy];
	nlocus=DeleteCases[
        Table[{omeganorm[omega],Re[N[g[sigma+I*omega]]],
            	Im[N[g[sigma+I*omega]]],Abs[N[g[sigma+I*omega]]]},
                {omega,minomega,maxomega,delomega}],{_,_,_,Infinity}];
   On[Power::infy];
	freqcolor[y_,z_]:=If[(z[[2]]-y[[2]])^2+(z[[3]]-y[[3]])^2<=1,
				{Hue[1-.65*y[[1]],1,.75],Thickness[.008],
          		Line[{{y[[2]],y[[3]]},{z[[2]],z[[3]]}}]},
             {{Hue[1-.65*y[[1]],1,.75],Point[{y[[2]],y[[3]]}]},
              {Hue[1-.65*z[[1]],1,.75],Point[{z[[2]],z[[3]]}]}}]; 
	list=MapThread[freqcolor[#1,#2]&,{Drop[nlocus,-1],Drop[nlocus,1]}];
	PlotRange0=(PlotRange/.{opts})/.PlotRange->
          PlotRange[Graphics[list,PlotRange\[Rule]All]];
	RR=MantissaExponent[Max[Transpose[nlocus][[4]]]];
	If[MatrixQ[PlotRange0],
  		RR=MantissaExponent[Sqrt[2]*N[Max[Map[Abs[#]&,Flatten[PlotRange0]]]]]];
	RR=Round[RR[[1]]*10]*10^(RR[[2]]-1);
	BandCirc={RGBColor[1,0,0],Thickness[.0005],
        Line[Table[{Band Cos[2 Pi jj/50]-1,Band Sin[2 Pi jj/50]},{jj,0,50,
              1}]]};
	SensCirc={RGBColor[0,1,0],Thickness[.001],
        Line[Table[{Sens Cos[2 Pi jj/50]-1,Sens Sin[2 Pi jj/50]},{jj,0,50,
              1}]]};
	MargCirc={RGBColor[0,0,1],Thickness[.004],
        Line[Table[{Marg Cos[2 Pi jj/50],Marg Sin[2 Pi jj/50]},{jj,0,50,
              1}]]};
	p1={GrayLevel[0.75],
        Table[Line[{{0.,0.},{2*RR*Cos[i Pi/12],2*RR*Sin[i Pi/12]}}],{i,1,
            24}]};
	p2={GrayLevel[0.75],
        Table[Line[
            Table[{(RR*i/10) Cos[2 Pi jj/50],(RR*i/10) Sin[2 Pi jj/50]},{jj,0,
               50,1}]],{i,1,20}]};
   fname="G[s]=" flabel;
	nlocusplt:=Graphics[{p1,p2,list,SensCirc,BandCirc,MargCirc},
    	GridLines->None,
    	PlotRange->PlotRange0,
   	AspectRatio->GoldenRatio,
   	PlotLabel->fname,
    	(*PlotLabel->FontForm["NYQUIST PLOT",{"Helvetica-Bold",10}],*)
      Frame->True,
      RotateLabel->False,
    	FrameLabel->{"Re[g]","Im[g]"," "," "}
    	];
    maxomegalab=ToString[ maxomega];
    minomegalab=ToString[ minomega];
    ShowLegend[nlocusplt,
        {Hue[.35+.65*#,1,.75]&,rangeomega,(maxomegalab),(minomegalab),
        LegendTextSpace->1,
        LegendShadow->None,
        LegendPosition->{.7,-.5},
        LegendBorderSpace->.05,
        LegendSize->{.1,.5}}]
    ]
ColorNyquist[g_,s_]:=ColorNyquist[g,s,-20,+20,0.4]
SetAttribute[ColorNyquist,ReadProtected];
SetAttribute[ColorNyquist,Locked];

(* Black and white version based on parametric plot *)
Clear[Nyquist];
Nyquist[g_,s_,minomega_,maxomega_,opts___]:=
Module[{flabel=InputForm[g[s]],sigma=0,omega,PlotRange0,Sens,Band,Marg},
PlotRange0=(PlotRange/.{opts})/.PlotRange->All;
Sens=1/(SensitivityPeak/.{opts}/.SensitivityPeak->Infinity);
If[Bandwidth/.{opts}/.Bandwidth->False,Band=Sqrt[2],Band=0];
If[Margins/.{opts}/.Margins->False,Marg=1,Marg=0];
   ParametricPlot[{{Re[g[sigma+I*omega]],Im[g[sigma+I*omega]]},
   		{Sens Cos[2 Pi omega/maxomega]-1,Sens Sin[2 Pi omega/maxomega]},
   		{Band Cos[2 Pi omega/maxomega]-1,Band Sin[2 Pi omega/maxomega]},
   		{Marg Cos[2 Pi omega/maxomega],Marg Sin[2 Pi omega/maxomega]}},
		{omega,minomega,maxomega},
		PlotRange->PlotRange0,
      PlotStyle ->{{RGBColor[0, 0, 0]}, 
      			{RGBColor[0, 1, 0]}, 
              	{RGBColor[1, 0, 0]}, 
      			{RGBColor[0, 0, 1]}},
		(*AspectRatio->Automatic,*)
		GridLines->Automatic,
		(*PlotLabel->FontForm["NYQUIST PLOT",{"Helvetica-Bold",10}],*)
		RotateLabel->False,
		Frame->True,
		FrameLabel->{"Re[g]","Im[g]"," "," "}]
]

Nyquist[g_,s_]:=Nyquist[g,s,-20,+20]
SetAttribute[Nyquist,ReadProtected];
SetAttribute[Nyquist,Locked];


Clear[InverseNyquist];
InverseNyquist[g_,s_,minomega_,maxomega_]:=
Module[{invg,sigma=0,omega,flabel},
invg[ss_]:=1./g[ss];
flabel=InputForm[g[s]];
ParametricPlot[{Re[invg[sigma+I*omega]],Im[invg[sigma+I*omega]]},
                {omega,minomega,maxomega},
PlotRange->All,
(*AspectRatio->Automatic,*)
(*PlotLabel->FontForm["INVERSE NYQUIST PLOT",{"Helvetica-Bold",10}],*)
Frame->True,
RotateLabel->False,
FrameLabel->{"Re[1/g]","Im[1/g]"," "," "}]
]

InverseNyquist[g_,s_]:=InverseNyquist[g,s,-20,+20]
SetAttribute[InverseNyquist,ReadProtected];
SetAttribute[InverseNyquist,Locked];


(* SemiLog Plots *)
Clear[SemiLogPlot]
SetAttributes[SemiLogPlot, HoldAll]

SemiLogPlot[f_List, {x_, xmin_, xmax_}, opts___] :=
        Module[{r,grid,
                g = ParametricPlot[Evaluate[Transpose[Thread[{Log[10,x], f}] ]],
                        {x, xmin, xmax},
                        Ticks->{LogScale, Automatic}, 
                			FrameTicks -> {LogScale, Automatic,LogScale, Automatic},
                        DisplayFunction -> Identity, opts]
					},
      r = PlotRange[g];
		grid=FullOptions[g,GridLines]/.{RGBColor[0.`,0.`,0.5`]\[Rule]GrayLevel[.75]};
     	Show[g, DisplayFunction -> $DisplayFunction,
                Frame->True,
                Axes->False,
                PlotRange -> r, 
				    GridLines\[Rule]grid,
                AxesOrigin -> Map[#[[1]]&,r]]
];

SemiLogPlot[f_, {x_, xmin_, xmax_}, opts___] :=
        Module[{r,grid,
                g = ParametricPlot[{Log[10,x], f}, {x, xmin, xmax},
                        Ticks->{LogScale, Automatic}, 
                        FrameTicks -> {LogScale, Automatic,LogScale, Automatic},
                        DisplayFunction -> Identity, opts]
               },
      r = PlotRange[g];
		grid=FullOptions[g,GridLines]/.{RGBColor[0.`,0.`,0.5`]\[Rule]GrayLevel[.75]};
      Show[g, DisplayFunction -> Identity,
                Frame->True,
                Axes->False,
                PlotRange -> r, 
					 GridLines\[Rule]grid,
                AxesOrigin -> Map[#[[1]]&,r]]
 ];
   
ModPhase::usage = 
"ModPhase[Phase_,Cutoff_:Pi] \n
unrolls phase angles Phase (in radians) \n
by changing absolute jumps greater than CUTOFF \n
to their 2*pi complement.  \n\n
The Cutoff angle defaults to Pi."

ModPhase[Phase_, Cutoff_:Pi]:=Module[{pmin,pi,a,b,c,d,e,f,g},
         pmin = Min[Phase];
         pi=N[Pi];
         a = Mod[Phase - pmin, 2. pi] + pmin;
         b = Drop[RotateLeft[a]-a,-1];        (* List of differences *)
         c = -Map[(If[#>Cutoff,1.,0])&,b]//N; (* Locations large +jumps *)
         d = Map[(If[#<-Cutoff,1.,0])&,b]//N; (* Locations large -jumps *)
         e = (c + d)* (2. pi)//N;
         f = FoldList[Plus,0,e];              (* Cumulative Sum *)
         g = a + f;
        Return[g]
]

Clear[Bode];
SetAttributes[Bode, HoldAll]

Bode[g_,s_,minomega_,maxomega_,opts___]:=Module[{mag,plist,pmod,
   minpmod,otmp,rmag,phase,phase2,flabel=InputForm[g[s]],delta,jj,
   NN,sigma=0,omega,grid},   
   NN=PlotPoints/.{opts}/.PlotPoints\[Rule]100;
   delta=(maxomega-minomega)/NN;
   mag=SemiLogPlot[
        20*Log[10,Abs[g[sigma+I*omega]]],{omega,minomega,maxomega},
      GridLines->Automatic,
		PlotPoints\[Rule]Round[NN/4],
     	AspectRatio->1/3,
      PlotRange->All,
      FrameLabel->{"rad/sec","dB ",FontForm["MAGNITUDE",{"Helvetica-Bold",8}],
            " "},
      RotateLabel->False,
      DisplayFunction->Identity
    ];
    
    rmag=PlotRange[mag];
    plist=Table[Arg[g[sigma+I*(minomega+delta*jj)]],{jj,0,NN}];
    pmod=ModPhase[plist,Pi]*(180./Pi)//N;
    minpmod=Min[pmod,0.01];
    otmp=Table[Log[10,(minomega+delta*jj)],{jj,0,NN}];
	 phase=Graphics[Line[Transpose[{otmp,pmod}]],GridLines\[Rule]Automatic,
        PlotRange->All];
    grid=FullOptions[phase,GridLines]/.{RGBColor[0.`,0.`,0.5`]\[Rule]GrayLevel[.75]};
    phase2=Show[phase,   DisplayFunction->Identity,
    					Axes->False,
                  Ticks->{LogScale, Automatic}, 
                  FrameTicks -> {LogScale, Automatic,LogScale, Automatic},
                  GridLines->grid,
                  AspectRatio->1/3,
                  Frame->True,
                  PlotRange->All,
                  AxesOrigin -> {rmag[[1]][[1]],minpmod},
                  FrameLabel->{"rad/sec","deg ",FontForm["PHASE",{"Helvetica-Bold",8}]," "},
                  RotateLabel->False
               ];
     Map[Show[#,DisplayFunction -> $DisplayFunction]&,{mag,phase2}]
 ];

Bode[g_,s_]:=
Bode[g,s,0.01,1000]
SetAttribute[Bode,ReadProtected];
SetAttribute[Bode,Locked];


Clear[RootLocus];
(* RootLocus can be simplified by combining the plot range 
   computations. 5/23/92 *)

(* Needs[Graphics`Legend] *)
RootLocus[h_, s_, k_,kmin_,kmax_,kdel_] := Module[
                                     {flabel=TraditionalForm[h[s]],
                                      zerorules,zerolist,zeros,
                                      polerules,polelist,openpoles,
                                      xz,yz,xp,yp,xr,yr,
                                      maxx,minx,maxy,miny,del,
                                      list,rlocusplt,krange,kminlab,kmaxlab,
                                      rlocus,fname,a,amax,amin,adel,
                                      red,green,RR,p1,p2},

	 Print["\n                Zeros in green, Open loop poles in red\n"];

(* Computation of the zeros *)

    zerorules=NSolve[Numerator[h[s]] == 0,s];
    If[Length[zerorules]==0,zerolist={},
    zerolist={Re[#],Im[#]} & /@ (s/.zerorules)];
    zeros=Point[#] & /@ zerolist; (* Used in the plot *)

(* Computation of the open loop poles *)

    polerules=NSolve[Denominator[h[s]] == 0,s]; 
    polelist={Re[#],Im[#]} & /@ (s/.polerules);
    openpoles=Point[#] & /@ polelist;  (* Used in the plot *)

(* Computation of the root locus *)
(*
    km=Max[Abs[kmin],Abs[kmax]]; (* Normalize the gain for coloring plot *)
    rlocus=Flatten[Table[{k/km,Re[s], Im[s]} /. 
    NSolve[Denominator[h[s]/(1+k*h[s])] == 0,s], {k,kmin,kmax,kdel}],1];
*)
    amax=Sign[kmax]*(Abs[kmax]^(.333));amin=Sign[kmin]*(Abs[kmin]^(.333));
    adel=kdel*(amax-amin)/(kmax-kmin);am=Abs[amax-amin];
    rlocus=Flatten[Table[{(Abs[a-amin]/am),Re[s], Im[s]} /. 
    NSolve[Denominator[h[s]/(1+(a^3)*h[s])] == 0,s], {a,amin,amax,adel}],1];
(* Compute some maximum values to scale the plot *)
(* Automatic scaling does not include both isolated open loop  values *)
(* and the moving root locus values. *)
(* These computations could be simplified 5/23/92 *)

    xz=First[#]&/@zerolist;  (* Real and imaginary parts of the zeros *)
    yz=Last[#]&/@zerolist;   (* and open loop poles, used to size the *)
    xp=First[#]&/@polelist;  (* plot to include these isolated points *)
    yp=Last[#]&/@polelist;
    
    second[z_]:=z[[2]]/;ListQ[z]; (* Picks out the second entry *)

    xr=second[#]&/@rlocus;
    yr=Last[#]&/@rlocus;

    maxx=Max[xr,xp,xz];  
    minx=Min[xr,xp,xz];
    maxy=Max[yr,yp,yz];
    miny=Min[yr,yp,yz];

    del=1.0;            (* Sizes the plot one unit beyond the 
                           value of the largest points. *)

    fname="G[s]=" flabel;
    green=RGBColor[0,1,0];
    red=RGBColor[1,0,0];

	 gaincolor[z_]:={Hue[1-.65*z[[1]],1,.75],PointSize[0.012],
        Point[{z[[2]],z[[3]]}]};
    list=gaincolor[#]& /@ rlocus;  (* The gain values are translated 
                                  into colors for the plot. *)
	 RR=MantissaExponent[N[Sqrt[2]*Max[Map[Abs[#]&,
                Flatten[{{minx-del,maxx+del},{miny-del,maxy+del}}]]]]];
	 RR=Round[RR[[1]]*10]*10^(RR[[2]]-1);
	 p1={GrayLevel[0.75],
        Table[Line[{{0.,0.},{2*RR*Cos[i Pi/12],2*RR*Sin[i Pi/12]}}],{i,1,24}]};
	 p2={GrayLevel[0.75],
        Table[Line[
            Table[{(RR*i/10) Cos[2 Pi jj/50],(RR*i/10) Sin[2 Pi jj/50]},{jj,0,
                50,1}]],{i,1,20}]};
	 rlocusplt:=Graphics[{p1,p2,list},
            Frame->True,GridLines->None,
				AspectRatio->GoldenRatio,
				PlotLabel->fname,
            (*PlotLabel->FontForm["ROOT LOCUS PLOT",{"Helvetica-Bold",10}],*)
            FrameLabel->{"Re[s]","Im[s]"," "," "}, 
            AxesOrigin->{0,0}, 
            RotateLabel->False,
            Epilog->{{PointSize[1/80],green,zeros},  (* zeros are shown in green *)
                 {PointSize[1/80],red, (* open loop poles *)openpoles}},(* are in red *)
            PlotRange -> {{minx-del,maxx+del},{miny-del,maxy+del}}];

    krange=(kmax-kmin)/kdel;
    kmaxlab=ToString[ kmax];
    kminlab=ToString[ kmin];
    
        ShowLegend[rlocusplt,
        {Hue[.35+.65#,1,.75]&,krange,kmaxlab,kminlab,
        LegendTextSpace->1,
        LegendShadow->None,
        LegendPosition->{.7,-.5},
        LegendBorderSpace->.02,
        LegendSize->{.1,.5}}]
        
    ] (* end of function *)

(* The following versions are used to report defaults *)
(* Automatic variable assignments could be used       *)

RootLocus[h_, k_,kmin_,kmax_,kdel_] :=
RootLocus[h,s,k,kmin,kmax,kdel]

RootLocus[h_, s_, k_,kmin_,kmax_] :=Module[{},
Print["\n                Using default gain increment kdel=1.0\n"];
RootLocus[h,s,k,kmin,kmax,1.0]]  

RootLocus[h_, k_,kmin_,kmax_] :=
RootLocus[h, s, k,kmin,kmax]

RootLocus[h_, s_, k_] :=Module[{},
Print["\n                Using default gain range kmin=0,kmax=10,kdel=1.0\n"];
RootLocus[h,s,k,0,10,1.0]]

RootLocus[h_, s_] :=Module[{},
Print["\n                Using default gain range kmin=0,kmax=10,kdel=1.0\n"];
RootLocus[h,s,k,0,10,1.0]]

RootLocus[h_] :=Module[{},
Print["\n                Using default gain range kmin=0,kmax=10,kdel=1.0\n"];
RootLocus[h,s,k,0,10,1.0]]
SetAttribute[RootLocus,ReadProtected];
SetAttribute[RootLocus,Locked];


Clear[PoleZeroPlot];
PoleZeroPlot[g_,s_]:=
Module[{flabel=InputForm[g[s]],polerules,polelist,numpoles,
        maxsigma,minsigma,maxomega,sigma=0,omega,
        margin,huge},
        
        margin=0.5;  (* Used to expand the plot beyond the min/max poles *)
    huge=10^6;   (* Used to determine if poles are real valued.      *)

    polerules=NSolve[Denominator[g[s]] == 0,s]; (* Find the poles *)
    polelist={Re[#],Im[#]} & /@ (s/.polerules); 
    numpoles=Length[polelist];
    
    maxsigma=N[Max[First[#]&/@polelist]]; (* Maximum real value of poles *)
    minsigma=N[Min[First[#]&/@polelist]]; (* Minimum real value of poles *)
    maxomega=N[Max[Last[#]&/@polelist]];  (* Maximum imag value of poles *)

If[Abs[maxomega]<1/huge,   (* If the poles are real. *)
        Print["There are ",numpoles," real poles located at:\n"];
        Print[polelist];
    Plot[
           Abs[g[sigma]],
                {sigma,minsigma- 5.,
                       maxsigma+ 5.},
    (*PlotLabel->"Singularities of g[s]=" flabel,*)
    AxesLabel->{"Real","Imag"},
    PlotRange->Automatic],             
(* else *)
        Print["There are ",numpoles," poles located at:\n"];
        Print[polelist];
    Plot3D[
           complexToZandColor[g[sigma + I omega]],
                {omega,-(1+margin) maxomega,(1+margin) maxomega},
                {sigma,minsigma-margin Abs[minsigma],
                       maxsigma+margin Abs[maxsigma]},
    (*PlotLabel->"Singularities of g[s]=" flabel,*)
    AxesLabel->{"Imag","Real","Mag"},
    PlotRange->All,AspectRatio->Automatic,PlotPoints->30]
        ] (* end if *)
    ] (* end module *)


(* Two utility functions for plotting functions of a complex variable.*)

maxZ=N[10^30];
nPi=N[Pi];
ntwoPi=N[2Pi];
nE=N[E];

complexToZandColor[z_]:=Block[{nz,az},
nz=N[z];
az=Abs[nz];
{
        Min[az,maxZ],
                Hue[(nPi+If[nz==0,0,Arg[nz/az]])/(ntwoPi)]
                }]


complexToLogLogZandColor[z_]:=Block[{nz,az},
nz=N[z];
az=Abs[nz];
{
        Log[Log[az+nE]],
                Hue[(nPi+If[nz==0,0,Arg[nz/az]])/(ntwoPi)]
                }]
SetAttribute[PoleZeroPlot,ReadProtected];
SetAttribute[PoleZeroPlot,Locked];

(************************************************************************)
(*                      Minimal Realization                             *)
(************************************************************************)

Clear[MinimalRealization];
MinimalRealization[G_,s_]:=
  Module[{GG,PFE,factors,AA={},BB={},CC={},DD,A,B,H},
		(* partial fraction expansion *)
		GG[x_]:=Rationalize[G[x]];
		PFE=Apart[GG[s],s];
		factors=Drop[FactorList[LeastCommonDenominator[GG,s]],1];
		Do[
		{A,B,H}=CreateJordanBlock[PFE,factors[[i]],s];
		AA=DiagJoin[AA,A];
		BB=Join[BB,B];
		CC=Join[CC,H]
		,{i,Length[factors]}];
		DD=GG[Infinity];
		{AA,BB,Transpose[CC],DD}
      ]
MinimalRealization[AA_,BB_,CC_,DD_]:=Module[{A1,B1,C1,D1,T},
		{A1,B1,C1,D1}=
      KalmanDecomposition[AA,BB,CC,DD,IrreducibleRealization->True];
		{T,A1}=JordanDecomposition[A1];
		B1=LinearSolve[T,B1];
		C1=C1.T;
		{A1,B1,C1,D1}
		]
SetAttribute[MinimalRealization,ReadProtected];
SetAttribute[MinimalRealization,Locked];
      
Clear[CreateJordanBlock];
CreateJordanBlock[PFE_,factor_List,s_]:=
  Module[{CoefficientList,BList,	AA={},BB={},CC={},A,B,p,CCT,kk,DD,EE,TT},
		CoefficientList=
      Map[Coefficient[PFE,1/factor[[1]]^#]&,Reverse[Range[factor[[2]]]]];
		BList=Map[QRDecomposition[#][[2]]&,CoefficientList];
		BList=Flatten[BList,1];
		BList=DropLastZeroRows[RowReduce[BList]];
		p=s/.Solve[factor[[1]]==0,s][[1]];
		Do[CCoefficients=
        	Map[CoefficientList[[#]].BList[[i]]&,Range[Length[CoefficientList]]];
		  	CCoefficients=DropFirstZeroRows[CCoefficients];
			{A,B}=JordanBlock[p,Length[CCoefficients],BList[[i]]];
			AA=DiagJoin[AA,A];
			BB=Join[BB,B];
			CC=Join[CC,CCoefficients]
			,{i,Length[BList]}];
		CCT=Transpose[CC];
		If[Not[ObservablePair[AA,CCT]],Print["Eliminating nonobservable states."];
			DD=Span[ObservabilityMatrix[AA,CCT]];
			EE=NullSpace[DD];
			TT=Join[DD,EE];
			AA=Transpose[LinearSolve[Transpose[TT],Transpose[TT.AA]]];
			BB=TT.BB;
			CC=DropLastZeroRows[LinearSolve[Transpose[TT],CC]];
			kk=Length[CC];
			AA=AA[[Range[kk],Range[kk]]];
			BB=BB[[Range[kk]]];
			];
	  {AA,BB,CC}
     ]
SetAttribute[CreateJordanBlock,ReadProtected];
SetAttribute[CreateJordanBlock,Locked];
      
Clear[JordanBlock];
JordanBlock[p_,n_Integer,b_?VectorQ]:=Module[{A={},BB={}},
		If[n===0,Return[A]];
		If[n===1,A={{p}};Return[{A,{b}}]];
		A={Flatten[Append[{{p,1}},Table[0,{n-2}]]]};
		BB=Prepend[{b},Table[0,{Length[b]}]];
	 	Do[
			A=If[i<=n-2,Append[A,RotateRight[A[[-1]]]],A];
			BB=Prepend[BB,Table[0,{Length[b]}]];
		  ,{i,1,n-2}];
		 A=Append[A,Append[Table[0,{n-1}],p]];
		{A,BB}
      ]
SetAttribute[JordanBlock,ReadProtected];
SetAttribute[JordanBlock,Locked];


(************************************************************************)
(*                      Kalman Decomposition                            *)
(************************************************************************)

Clear[ObservableDecomposition];
ObservableDecomposition[AA_,BB_,CC_,DD_]:=
  Module[{OS,EE,TT,AAnew,BBnew,CCnew},
	If[Rationalize[Flatten[CC].Flatten[CC]]===0,Return[{AA,BB,CC,DD}]];
	OS=Span[ObservabilityMatrix[AA,CC]];
	If[Length[OS]===Length[AA],Print["System is observable."];
      Return[{AA,BB,CC,DD}]];
	EE=NullSpace[OS];
	TT=Join[EE,OS];
	AAnew=Transpose[LinearSolve[Transpose[TT],Transpose[TT.AA]]];
	BBnew=TT.BB;
	CCnew=Transpose[LinearSolve[Transpose[TT],Transpose[CC]]];
	Chop[{AAnew,BBnew,CCnew,DD}]
   ]
SetAttribute[ObservableDecomposition,ReadProtected];
SetAttribute[ObservableDecomposition,Locked];
      
Clear[ControllableDecomposition];
ControllableDecomposition[AA_,BB_,CC_,DD_]:=
  Module[{CS,EE,TT,AAnew,BBnew,CCnew},
	If[Rationalize[Flatten[BB].Flatten[BB]]===0,Return[{AA,BB,CC,DD}]];
	CS=Span[Transpose[ControllabilityMatrix[AA,BB]]];
	If[Length[CS]===Length[AA],Print["System is controllable."];
      Return[{AA,BB,CC,DD}]];
	EE=NullSpace[CS];
	TT=Transpose[Join[CS,EE]];
	AAnew=LinearSolve[TT,AA.TT];
	BBnew=LinearSolve[TT,BB];
	CCnew=CC.TT;
	Chop[{AAnew,BBnew,CCnew,DD}]
   ]
SetAttribute[ControllableDecomposition,ReadProtected];
SetAttribute[ControllableDecomposition,Locked];
      
Clear[KalmanDecomposition];
KalmanDecomposition[AA_,BB_,CC_,DD_,opts___]:=
  Module[{Ac,Bc,Cc,Dc,n=Length[AA],m=Dimensions[BB][[2]],p=Length[CC],kk,kir,
      Ac11,Ac12,Ac21,Ac22,Bc1,Bc2,Ao,Bo,Co,Do,Aco,Bco,Cco,Dco,Cc1,Cc2,Aoc,Boc,
      Coc,Doc,Cir,AAnew,BBnew,CCnew,DDnew},
		{Ac,Bc,Cc,Dc}=ControllableDecomposition[AA,BB,CC,DD];
		Print["Decompose into controllable/uncontrollable parts."];
		kk=Length[DropLastZeroRows[Bc]];
		{Ac11,Ac12,Ac21,Ac22}=	{Ac[[Range[kk],Range[kk]]],
        Ac[[Range[kk],Range[kk+1,n]]],Ac[[Range[kk+1,n],Range[kk]]],
        Ac[[Range[kk+1,n],Range[kk+1,n]]]};
		{Bc1,Bc2}={Bc[[Range[kk]]],Bc[[Range[kk+1,n]]]};
		Print["Divide controllable part into observable/unobservable parts."];
		{Ao,Bo,Co,Do}=
      ObservableDecomposition[Ac11,
        Transpose[Join[Transpose[Ac12],Transpose[Bc1]]],
        Cc[[Range[p],Range[kk]]],Dc];
		If[(IrreducibleRealization/.{opts})/.{IrreducibleRealization->False},
      Cir=DropFirstZeroRows[Transpose[Co]];kir=Length[Cir];
      Return[{AppendRows[Ao,Bo[[Range[kk],Range[1,n-kk]]]][[Range[kir],
          Range[kir]]],Bo[[Range[kir],Range[n-kk+1,m+n-kk]]],
          Transpose[Cir],Do}]];
		{Aco,Bco,Cco,Dco}={AppendRows[Ao,Bo[[Range[kk],Range[1,n-kk]]]],
        Bo[[Range[kk],Range[n-kk+1,m+n-kk]]],Co,Do};
		{Cc1,Cc2}={Cco[[Range[p],Range[kk]]],Cc[[Range[p],Range[kk+1,n]]]};
		Print["Divide uncontrollable part into observable/unobservable parts."];
		{Aoc,Boc,Coc,Doc}=ObservableDecomposition[Ac22,Bc2,Cc2,Dco];
	  	Anew=BlockMatrix[{{Aco},{Ac21,Aoc}}];
		Bnew=AppendColumns[Bco,Boc];
		Cnew=AppendRows[Cc1,Coc];
		Dnew=Doc;
	Chop[{Anew,Bnew,Cnew,Dnew}]
   ]
SetAttribute[KalmanDecomposition,ReadProtected];
SetAttribute[KalmanDecomposition,Locked];

(************************************************************************)
(*                      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]]}];
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=Inverse[mat/.ZeroRule];
Inv/.NonZeroRule
]
SetAttribute[MyInverse,ReadProtected];
SetAttribute[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]]
]

(************************************************************************)
(*                      HamiltonianMatExp                                       *)
(************************************************************************)
(*  Finds the resolvent and then takes the inverse laplace transform *)

Clear[MyMatrixExp];
MyMatrixExp[A_,t_]:=Module[{n,s,B},
    n=Length[A];
    B=MyInverse[s*IdentityMatrix[n]-A];
    InverseLaplaceTransform[B[[ Range[1,n],Range[1,n] ]],s,t]
]
SetAttribute[MyMatrixExp,ReadProtected];
SetAttribute[MyMatrixExp,Locked];

Clear[HamiltonianMatExp];
HamiltonianMatExp[A_,t_]:=Module[{n,s,B,lcd,bb,NumB,Num,eigs},
    n=Length[A];
    B[x_]:=MyInverse[x*IdentityMatrix[n]-A];
    lcd=Det[s*IdentityMatrix[n]-DiagonalMatrix[Eigenvalues[A]]];
    bb=InverseLaplaceTransform[1/lcd,s,t];
    Num=Expand[Cancel[lcd*B[s][[ Range[1,n],Range[1,n/2] ]]],s];
    NumB[f_,x_]:=(Num/.{s^m_?IntegerQ->D[f,{x,m}]})/.{s->D[f,x]};
    NumB[bb,t]+(Num/.s->0)*(bb-1)
]
SetAttribute[HamiltonianMatExp,ReadProtected];
SetAttribute[HamiltonianMatExp,Locked];

(* 
EigSort returns a list of sorted eigenvalues in increasing
real part, and a list of corresponding indices.
*)
Clear[EigSort];
EigSort[Eigs_]:=Module[{ReEigs,SE,F,IndexList},
    ReEigs=Re[N[Eigs]];
    SE=Union[Sort[ReEigs]];
    F[x_]:=Position[ReEigs,x];
    IndexList=Flatten[Map[F,SE]];
    {Eigs[[IndexList]],IndexList}
    ]
SetAttribute[EigSort,ReadProtected];
SetAttribute[EigSort,Locked];

Clear[InvAB];
InvAB[A_,B_]:=LinearSolve[A,B];
SetAttribute[InvAB,ReadProtected];
SetAttribute[InvAB,Locked];

Clear[BInvA];
BInvA[A_,B_]:=Transpose[LinearSolve[Transpose[A],Transpose[B]]];
SetAttribute[BInvA,ReadProtected];
SetAttribute[BInvA,Locked];

Clear[DropFirstZeroRows];
DropFirstZeroRows[XX_]:=
If[Rationalize[XX[[1]].XX[[1]]]===0,DropFirstZeroRows[Drop[XX,1]],XX]
   
Clear[DropLastZeroRows];
DropLastZeroRows[XX_]:=
	If[Rationalize[XX[[-1]].XX[[-1]]]===0,DropLastZeroRows[Drop[XX,-1]],XX]
         
Clear[DiagJoin];
DiagJoin[A_,B_]:=Module[{ma,na,mb,nb,Zilch,H0,H1,X},(
                 If[A=={},X=B,(
                 {ma,na}=Dimensions[A];{mb,nb}=Dimensions[B];
                 Zilch=Table[0,{mb},{na}];H0=Join[A,Zilch];
                 Zilch=Table[0,{ma},{nb}];H1=Join[Zilch,B];
                 X=Transpose[Join[Transpose[H0],Transpose[H1]]])];
                 X
                 )];


Print["  *** LinearControl successfully loaded ***"];
End[];        (* end the private context *)



EndPackage[];  (* end the package context *)
If[!spell1, On[General::spell1]];
If[!spell, On[General::spell]];

