(* ::Package:: *)

(* ::Title::Closed:: *)
(*Read here before starting*)


(* ::Section:: *)
(*Software version 1.0.0*)


(* ::Section::Closed:: *)
(*Citation, updates, copyright, URLs*)


(* ::Text:: *)
(*This code provides software associated with the following book.*)
(**)
(*Frank, S. A. 2018. Control theory tutorial: basic concepts illustrated by software examples. Springer, Cham, Switzerland.  *)
(**)
(*This book is open access and freely available at:  https://doi.org/10.1007/978-3-319-91707-8.*)
(**)
(*Software version 1.0.0.*)
(**)
(*This software and updates may be obtained at http://extras.springer.com/2018/978-3-319-91707-8.*)
(**)
(*This work is licensed under a Creative Commons Attribution 4.0 International License, Copyright \[Copyright] 2018 by Steven A. Frank.*)
(**)
(*This software code was originally released on Zenodo at https://doi.org/10.5281/zenodo.1043921 under a Creative Commons Attribution-NonCommercial 4.0 International License, copyright \[Copyright] 2017 by Steven A. Frank. *)


(* ::Section::Closed:: *)
(*Using this code with Mathematica: required programs and files*)


(* ::Text:: *)
(*This code was developed with Mathematica version 11.2, see http://www.wolfram.com/mathematica/. *)
(**)
(*Mathematica is a commercial product that requires a paid license. A free reader program is available that allows one to view Mathematica notebooks and embedded graphics. See https://www.wolfram.com/cdf-player/.*)
(**)
(*At the download site for this software (see previous section), the file with extension .nb is a Mathematica notebook. That notebook includes embedded graphics for most of the figures generated in the manuscript. Because of the graphics, it is a large file. The file with extension .m is a Mathematica m-file. That file does not contain graphics and is much smaller. The m-file can be imported into a notebook file for use. *)


(* ::Section::Closed:: *)
(*Notes on using the code, settings that must be changed*)


(* ::Text:: *)
(*I originally developed this code for my own use rather than for distribution. Please send any comments, bug fixes, or rewritten code to me at safrank AT uci.edu [replace AT with @]. Thanks.*)


(* ::Text:: *)
(*In the first section "Generic transfer function and other functions for initialization" there are settings for the directory location for output graphics files. Change the setting for gdir and gdirlocal to be appropriate for your system.*)
(**)
(*That first section also includes code that automatically loads whenever code in any part of the file is used. Most sections in the file do not depend on the initialization code, but some sections do depend on that code. You can change the requirement for loading the initialization cell if you wish. See the Mathematica documentation.*)
(**)
(*Problems in execution can develop if you use the code from different sections without clearing all variables. I have added Clear[] statements in several places. However, I have not carefully cleared all variables, and clashes will likely occur in various places. The simplest quick fix is to quit the Mathematica kernel [Menu -> Evaluation -> Quit Kernel -> Make correct choice for kernel]. Then restart in the place you want to work, and things should work well for a while, until another clash arises. Clashes are not common, but they can happen, so be aware of this problem before considering whether or not the code works correctly.*)
(**)
(*The sections in this file match the order of the presentation in the manuscript. In many places in this file, there is additional code that was not used in the text. You can use that additional code or ignore it.*)
(**)
(*The best way to begin with the code is to find a graphic in the manuscript and match it with the code in this file that generated the graphic. Work through the presentation in the text and how that presentation corresponds to the code and the associated graphic. *)
(**)
(*The code to handle relatively complex control problems can often be fairly simple in Mathematica, because of the great power of the Mathematica control library. So do not be misled by the simplicity of the code, which can often be used to analyze reasonably complex problems. The code here provides the basis for research studies in many disciplines. *)
(**)
(*You will probably have to spend a significant amount of time reading the Mathematica documentation to understand how the code works. That time investment is worthwhile, because mastery of the Mathematica control library will be repaid by the subsequent ease with which one can solve complex control problems.*)


(* ::Section::Closed:: *)
(*Send bug fixes and comments on the code to the author*)


(* ::Text:: *)
(*I originally developed this code for my own use rather than for distribution. Please send any comments, bug fixes, or rewritten code to me at safrank AT uci.edu [replace AT with @]. Thanks.*)


(* ::Title:: *)
(*Control theory tutorial*)


(* ::Section::Closed:: *)
(*Generic transfer function and other functions for initialization*)


(* ::Subsection:: *)
(*Key functions*)


(* ::Text::Closed:: *)
(*Notes, open to see.*)


(* ::Text:: *)
(*Generic transfer function to be used as plant. Provides general form for many common transfer functions. Can always combine transfer functions with SystemsModelSeriesConnect[]. Note use of optional arguments, can skip starting with end. Must specify exponents, otherwise will not accept as TF. *)
(**)
(*Parameter c provides a sum of proportional and derivative response, whereas c=0 is pure derivative response. For step input, pure derivative response does not track, whereas with a proportional term c not zero, tracking is possible.*)


(* ::Input::Initialization:: *)
gtf[z_,n_,m_,a_:a,b_:b,\[Zeta]_:\[Zeta],\[Omega]_:\[Omega],c_:c]:=TransferFunctionCancel[TransferFunctionModel[(s+c)^z/((s+a)^n (s+b)^n (\[Omega]^-2 (s^2+2\[Zeta] \[Omega] s+\[Omega]^2))^m),s]];
FS[x_]:=FullSimplify[x];

TF[model_]:=TransferFunctionCancel[TransferFunctionModel[model,s]];
TFconnect[tf1_,tf2_]:=TransferFunctionCancel[SystemsModelSeriesConnect[tf1,tf2]];
TFloop[tf_]:=FS[TransferFunctionCancel[TF[(tf[s]/(1+tf[s]))[[1,1]]]]];

gtfn[z_,n_,m_,a_:a,b_:b,\[Zeta]_:\[Zeta],\[Omega]_:\[Omega],c_:c]:=TransferFunctionModel[(s+c)^z/((s+a)^n (s+b)^n (\[Omega]^-2 (s^2+2\[Zeta] \[Omega] s+\[Omega]^2))^m),s];

TFn[model_]:=TransferFunctionModel[model,s];
TFconnectn[tf1_,tf2_]:=SystemsModelSeriesConnect[tf1,tf2];
TFloopn[tf_]:=TFn[(tf[s]/(1+tf[s]))[[1,1]]];

PlotTF[tf_,freq_,tmax_]:=
Module[{input,yout,t,i,p1,p2,p3},
input={DiracDelta[t],UnitStep[t],Sin[freq*t]};
yout=Table[OutputResponse[tf,input[[i]],{t,0,tmax}],{i,3}];
p1=Plot[yout,{t,0,tmax},PlotRange->All];p2=BodePlot[tf,PlotLayout->"Magnitude"];
p3=BodePlot[tf,PlotLayout->"Phase"];
GraphicsColumn[{p1,p2,p3},Spacings->{0,50},ImageSize->Large]
]

PlotTFsin[tf_,freq_,tmax_,opts_:Null]:=
Module[{input,yout,t,i,tfa,popt},
tfa=If[ArrayQ[tf],tf,{tf}];
input=Sin[freq*t];
popt=If[opts===Null,PlotStyle->Automatic,opts];
yout=Table[OutputResponse[tfa[[i]],input,{t,0,2\[Pi]*tmax/freq}],{i,Length[tfa]}];
Plot[{yout,input},{t,0,2\[Pi]*tmax/freq},PlotRange->All,Evaluate@popt]
]

PlotTFstep[tf_,tmax_,opts_:Null]:=
Module[{input,yout,t,i,tfa,popt},
tfa=If[ArrayQ[tf],tf,{tf}];
popt=If[opts===Null,PlotStyle->Automatic,opts];
input=UnitStep[t];
yout=Table[OutputResponse[tfa[[i]],input,{t,0,tmax}],{i,Length[tfa]}];
Plot[yout,{t,0,tmax},PlotRange->All,Evaluate@popt]
]

H2[tf_]:=(1/(2\[Pi]) NIntegrate[Abs[tf[I w]]^2,{w,-\[Infinity],\[Infinity]}])^(1/2)
Hinf[tf_]:=Quiet[FindMaximum[{Abs[tf[I w][[1,1]]],w>=0},w]]
BodeDB[x_]:=20Log[10,x];

(* returns a polynomial that is a function, thus aa=den[tf], then a[s] is the denominator as polynomial of s *)
den[tf_]:=(Denominator[tf[#]][[1,1]])&
num[tf_]:=(Numerator[tf[#]][[1,1]])&

(* create a polynomial variables of vars_List_, w/coefficients a_ and order_ of polynomial, ie for one variable, poly[{s},q,3] = Subscript[q, 0]+s Subscript[q, 1]+s^2 Subscript[q, 2]+s^3 Subscript[q, 3] *)
poly[vars_List,a_,order_]:=Module[{n=Length@vars,idx,z},idx=Cases[Tuples[Range[0,order],n],x_/;Plus@@x<=order];
z=Times@@@(vars^#&/@idx);
z.((Subscript[a,Row[#]])&/@idx)]

(* OutputResponse can be numerically unstable, so sometimes will need to get the StateResponse and then recreate the output from the state model, as with the following function *)

StateOutputResponse[tf_,input_,tmax_]:=
Module[{ssm,ssr,c,y},
ssm=StateSpaceModel[tf];
ssr=StateResponse[ssm,input,{t,0,100}];
c=Normal[ssm][[3]];
y=c.ssr;
Return[y]
];

(* set default directory for graphics output *)
gdir="~/Desktop/";
gdirlocal="~/text/Submit/Signals/Tracking/Figures/";


(* ::Section::Closed:: *)
(*Frequency, gain and phase*)


(* ::Subsection::Closed:: *)
(*Dynamics and Bode plot*)


(* ::Input:: *)
(*llabelx=0.083;*)
(*rlabelx=0.633;*)
(*flabelx=0.233;*)
(*tf=TF[a/(s+a)];*)
(*gr=Table[Null,{i,3},{j,2}];*)
(*gr[[1,1]]=PlotTFsin[{tf/.a->1,tf/.a->10},1,4,{Frame->{True,True,False,False},FrameLabel->"",FrameTicks->{Range[0,25,5],Automatic}}];*)
(*gr[[2,1]]=PlotTFsin[{tf/.a->1,tf/.a->10},10,4,{Frame->{True,True,False,False},FrameLabel->"",FrameTicks->{Range[0,2.5,.5],Automatic}}];*)
(*gr[[3,1]]=PlotTFsin[{tf/.a->1,tf/.a->10},100,4,{Frame->{True,True,False,False},FrameLabel->Style["Time",FontSize->12],FrameTicks->{Range[0,.25,.05],Automatic}}];*)
(*gr[[1,2]]=PlotTFstep[{tf/.a->1,tf/.a->10},5,{Frame->{True,True,False,False},FrameLabel->{Style["Time",FontSize->12],Style["Response",FontSize->12]}}];*)
(*gr[[2,2]]=BodePlot[{TF[1/(s+1)],TF[10/(s+10)]},PlotLayout->"Magnitude",{Frame->{True,True,False,False},FrameLabel->{Style["",FontSize->12],Style["Gain",FontSize->12]}}];*)
(*gr[[3,2]]=BodePlot[{TF[1/(s+1)],TF[10/(s+10)]},PlotLayout->"Phase",{Frame->{True,True,False,False},FrameLabel->{Style["Frequency",FontSize->12],Style["Phase",FontSize->12]}}];*)
(*gout=GraphicsGrid[gr,Spacings->{50,20},ImageSize->Large,Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{llabelx,0.77}]],Inset[Text["(b)"],ImageScaled[{llabelx,0.427}]],Inset[Text["(c)"],ImageScaled[{llabelx,0.087}]],Inset[Text["(d)"],ImageScaled[{rlabelx,0.77}]],Inset[Text["(e)"],ImageScaled[{rlabelx,0.427}]],Inset[Text["(f)"],ImageScaled[{rlabelx,0.087}]],*)
(*Inset[Text["\[Omega] = 1"],ImageScaled[{flabelx,0.99}]],*)
(*Inset[Text["\[Omega] = 10"],ImageScaled[{flabelx-0.002,0.65}]],*)
(*Inset[Text["\[Omega] = 100"],ImageScaled[{flabelx-0.004,0.31}]]*)
(*}]*)
(*Export[gdir<>"lowpass.eps",gout];*)


(* ::Input:: *)
(*gout=BodePlot[{TF[1/(s+1)],TF[10/(s+10)]}]*)
(*Export[gdir<>"bodeExp.pdf",gout];*)


(* ::Input:: *)
(*gtest=PlotTFsin[{tf/.a->1,tf/.a->10},100,4,Axes->False,Frame->True]*)
(**)


(* ::Input:: *)
(*Show[gtest,Graphics[Text[ "Frequency",Bottom]]]*)


(* ::Subsection::Closed:: *)
(*Gain and phase angle for low pass filter*)


(* ::Input:: *)
(*(Abs[a/(I w +a)]==a/Sqrt[w^2+a^2])/.a->3/.w->7*)


(* ::Input:: *)
(*FullSimplify[ArcTan[Im[a/(I w +a)]/Re[a/(I w + a)]]/.a-> 3,Assumptions->w>0]*)


(* ::Section::Closed:: *)
(*Open loop control*)


(* ::Input:: *)
(*L[\[Omega]_,\[Omega]\[Epsilon]_]:=TF[\[Omega]\[Epsilon]/\[Omega] \[Omega]^3/(s+\[Omega])^3 (s^2+\[Omega]^2)/\[Omega] \[Omega]\[Epsilon]/(s^2+\[Omega]\[Epsilon]^2)]*)
(*Abs[L[\[Omega],\[Omega]\[Epsilon]][0]][[1,1]]*)
(**)
(*gout=BodePlot[{TF[((1/(s +1))^3)],L[1,1.2]},FrameLabel->{{None,Style["Gain",FontSize->12]},{Style["Frequency",FontSize->12],Style["Phase",FontSize->12]}}]*)
(*Export[gdir<>"openL.pdf",gout];*)


(* ::Section::Closed:: *)
(*Feedback control*)


(* ::Subsection::Closed:: *)
(*PID control of exponential cascade*)


(* ::Input:: *)
(*tf=TF[1/((s+.1)(s+10))];*)
(*tfalt=TF[1/((s+.01)(s+100))];*)
(*BodePlot[{tf,tfalt}]*)


(* ::Input:: *)
(*pid=PIDTune[tf,{"PID","ZieglerNichols"}];*)
(*PIDTune[tf,"PID","TuningRule"];*)
(*Print["PID controller = ",Expand[pid[s]][[1,1]], " \[TildeTilde] ",Rationalize[Expand[pid[s]][[1,1]],0.1]," \[TildeTilde] ", Rationalize[pid[s][[1,1]],0.0001], " \[TildeTilde] ",Rationalize[pid[s][[1,1]],0.001], " \[TildeTilde] ",Rationalize[pid[s][[1,1]],0.01]]*)
(*pidff=PIDTune[tf,{"PID","ZieglerNichols"},"Feedforward",PIDFeedforward->Automatic];*)
(*Print["PID FF control = ",pidff[s][[1,1]], " \[TildeTilde] ",Rationalize[pidff[s][[1,1]],0.0001], " \[TildeTilde] ",Rationalize[pidff[s][[1,1]],0.01]]*)
(**)
(*olp=FS[TFconnect[tf,pid]];*)
(*olpalt=FS[TFconnect[tfalt,pid]];*)
(*rout=FS[TFloop[olp]];*)
(*routalt=TFloop[olpalt];*)
(*NSolve[Denominator[rout[s]][[1,1]]==0,s];*)
(*NSolve[Denominator[routalt[s]][[1,1]]==0,s];*)
(*routff=TFconnect[pidff,rout];*)
(*routaltff=TFconnect[pidff,routalt];*)
(**)
(*tmp=FS[PIDTune[tf,{"PID","ZieglerNichols"},"ReferenceOutput"]];*)
(*(* test equivalence of my closed loop and MMA ReferenceOutput *)*)
(*FS[Expand[rout[s]]]==Rationalize[FS[Expand[tmp[s]]]]*)
(**)
(*ppairs={{tf,tfalt},{olp,olpalt},{rout,routalt},{routff,routaltff}};*)
(*popts={{PlotLayout->"Magnitude",PlotRange->{{0.006,100},Automatic}},{PlotLayout->"Phase",PlotRange->{{0.01,100},{-180,0}}}};*)
(**)
(*labely1=0.925;*)
(*labely2=0.41;*)
(*labely3=0.655;gout=GraphicsGrid[Table[With[{i=i,j=j},BodePlot[Evaluate[ppairs[[j]]],{0.01,100},Evaluate[popts[[i]]]]],{i,2},{j,Length[ppairs]}],Spacings->{0,10},ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.215,labely1}]],Inset[Text["(b)"],ImageScaled[{0.215,labely2}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.465,labely1}]],Inset[Text["(d)"],ImageScaled[{0.465,labely2}]],*)
(*Inset[Text["(e)"],ImageScaled[{0.715,labely1}]],Inset[Text["(f)"],ImageScaled[{0.715,labely2}]],*)
(*Inset[Text["(g)"],ImageScaled[{0.965,labely1}]],Inset[Text["(h)"],ImageScaled[{0.965,labely2}]],*)
(*Inset[Text["Process"],ImageScaled[{0.085,labely3}],BaseStyle->FontSize->11],*)
(*Inset[Text["PID open loop"],ImageScaled[{0.352,labely3}],BaseStyle->FontSize->11],Inset[Text["PID feedback"],ImageScaled[{0.61,labely3}],BaseStyle->FontSize->11],*)
(*Inset[Text["PID w/filter"],ImageScaled[{0.845,labely3}],BaseStyle->FontSize->11]*)
(*}]*)
(*Export[gdir<>"pidLoops.pdf",gout];*)


(* ::Input:: *)
(*dcolor=ColorData[97,"ColorList"];*)
(**)
(*(* measure inputs in relation to output r-\[Eta], yielding sensitivities *)*)
(*dout=TF[-tf[s]/(1+olp[s])];*)
(*sout=rout;*)
(*rerr=TF[1/(1+olp[s])];*)
(*doutalt=TF[-tfalt[s]/(1+olp[s])];*)
(*soutalt=routalt;*)
(*rerralt=TF[1/(1+olpalt[s])];*)
(*bin1m=BodePlot[{rerr,dout,sout},{0.01,1000},PlotRange->{-120,10},PlotLayout->"Magnitude",PlotStyle->{dcolor[[1]],dcolor[[4]],dcolor[[3]]}];*)
(*bin1p=BodePlot[{rerr,dout,sout},{0.01,1000},PlotLayout->"Phase"];*)
(*bin2m=BodePlot[{rerralt,doutalt,soutalt},{0.01,1000},PlotRange->{-120,10},PlotLayout->"Magnitude",PlotStyle->{dcolor[[1]],dcolor[[4]],dcolor[[3]]}];*)
(*bin2p=BodePlot[{rerralt,doutalt,soutalt},{0.01,1000},PlotLayout->"Phase"];*)
(*GraphicsGrid[{{bin1m,bin2m},{bin1p,bin2p}},Spacings->{0,10},ImageSize->Medium];*)
(**)
(*gout=GraphicsGrid[{{bin1m,bin2m}},Spacings->{0,10},ImageSize->Medium,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.10,0.235}]],Inset[Text["(b)"],ImageScaled[{0.60,0.235}]]*)
(*}]*)
(*Export[gdir<>"pidInputs.pdf",gout];*)
(**)
(*bin1m=BodePlot[{sout,soutalt},{0.01,1000},PlotRange->{-120,10},PlotLayout->"Magnitude"];*)
(*bin2m=BodePlot[{dout,doutalt},{0.01,1000},PlotRange->{-120,10},PlotLayout->"Magnitude"];*)
(*gout=GraphicsGrid[{{bin1m,bin2m}},Spacings->{0,10},ImageSize->Medium,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.10,0.80}]],Inset[Text["(b)"],ImageScaled[{0.60,0.80}]]*)
(*}];*)
(**)


(* ::Input:: *)
(**)
(*yout1=OutputResponse[rout,UnitStep[t],{t,0,30}];*)
(*yout2=OutputResponse[routalt,UnitStep[t],{t,0,30}];*)
(*youtff1=OutputResponse[TFconnect[pidff,rout],UnitStep[t],{t,0,30}];*)
(*youtff2=OutputResponse[TFconnect[pidff,routalt],UnitStep[t],{t,0,30}];*)
(*youttf=OutputResponse[tf,UnitStep[t],{t,0,100}];*)
(*youttfalt=OutputResponse[tfalt,UnitStep[t],{t,0,100}];*)
(**)
(*p1=Plot[{youttf,youttfalt},{t,0,30},PlotRange->{0,1.6}];*)
(*p2=Plot[{yout1,yout2},{t,0,5},PlotRange->{0,1.6}];*)
(*p3=Plot[{youtff1,youtff2},{t,0,5},PlotRange->{0,1.6}];*)
(**)
(*labely1=0.93;*)
(*labely2=0.96;*)
(*gout=GraphicsRow[{p1,p2,p3},ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.06,labely1}]],Inset[Text["(b)"],ImageScaled[{0.40,labely1}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.74,labely1}]],*)
(*Inset[Text["Process"],ImageScaled[{0.175,labely2}],BaseStyle->FontSize->12],*)
(*Inset[Text["PID feedback"],ImageScaled[{0.52,labely2}],BaseStyle->FontSize->12],*)
(*Inset[Text["PID w/filter"],ImageScaled[{0.85,labely2}],BaseStyle->FontSize->12]*)
(*}]*)
(**)
(*Export[gdir<>"pidStep.pdf",gout];*)
(**)


(* ::Input:: *)
(*yout1=OutputResponse[sout,UnitStep[t],{t,0,30}];*)
(*yout2=OutputResponse[soutalt,UnitStep[t],{t,0,30}];*)
(*p1=Plot[{yout1,yout2},{t,0,5},PlotRange->{-0.96,1.6},AxesOrigin->{0,-0.96}];*)
(**)
(*yout1=OutputResponse[dout,UnitStep[t],{t,0,30}];*)
(*yout2=OutputResponse[doutalt,UnitStep[t],{t,0,30}];*)
(*p2=Plot[{yout1,yout2},{t,0,2},PlotRange->{-0.012,0.02},AxesOrigin->{0,-.012}];*)
(**)
(*yout1=OutputResponse[sout,DiracDelta[t],{t,0,30}];*)
(*yout2=OutputResponse[soutalt,DiracDelta[t],{t,0,30}];*)
(*p3=Plot[{yout1,yout2},{t,0,5},PlotRange->{-7,7},AxesOrigin->{0,-7}];*)
(**)
(*yout1=OutputResponse[dout,DiracDelta[t],{t,0,30}];*)
(*yout2=OutputResponse[doutalt,DiracDelta[t],{t,0,30}];*)
(*p4=Plot[{yout1,yout2},{t,0,2.04},PlotRange->{-.05,.05},AxesOrigin->{0,-.05},ImageMargins->{{10.0,0},{0,0}}];*)
(**)
(*llabelx=0.07;*)
(*rlabelx=0.585;*)
(*gout=GraphicsGrid[{{p1,p2},{p3,p4}},ImageSize->Large,Spacings->{0, 30},*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{llabelx,0.60}]],Inset[Text["(c)"],ImageScaled[{rlabelx,0.60}]],Inset[Text["(b)"],ImageScaled[{llabelx,0.070}]],Inset[Text["(d)"],ImageScaled[{rlabelx,0.070}]],Inset[Text["Sensor noise"],ImageScaled[{0.265,0.98}],BaseStyle->FontSize->11],*)
(*Inset[Text["Process disturbance"],ImageScaled[{0.763,0.98}],BaseStyle->FontSize->11]*)
(*}]*)
(**)
(*Export[gdir<>"pidNoise.pdf",gout];*)


(* ::Input:: *)
(*freq={0.1,1,10,100};*)
(*main={tf,rout,sout,dout};*)
(*alt={tfalt,routalt,soutalt,doutalt};pl=Table[PlotTFsin[{main[[i]],alt[[i]]},freq[[j]],3,Axes->False],{i,1,Length[main]-2},{j,Length[freq]}];*)
(**)
(*labely=0.972;*)
(*labelx=0.014;*)
(*gout=GraphicsGrid[pl,ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["\[Omega] = 0.1"],ImageScaled[{0.136,labely}]],Inset[Text["\[Omega] = 1"],ImageScaled[{0.392,labely}]],*)
(*Inset[Text["\[Omega] = 10"],ImageScaled[{0.646,labely}]],*)
(*Inset[Text["\[Omega] = 100"],ImageScaled[{0.900,labely}]],*)
(*Inset[Text["Pr"],ImageScaled[{labelx,0.68}],BaseStyle->FontSize->14],*)
(*Inset[Text["Rf"],ImageScaled[{labelx,0.155}],BaseStyle->FontSize->14],*)
(*Inset[Text[""],ImageScaled[{labelx,0.09}],BaseStyle->FontSize->14]*)
(*}]*)
(*Export[gdir<>"/pidFreq.pdf",gout];*)


(* ::Subsection::Closed:: *)
(*PID control of internal oscillator*)


(* ::Input:: *)
(*tf=TF[1/(s^2+1)];*)
(*tfalt=TF[1/(s^2+33)];*)
(*pid=PIDTune[tf,{"PID"}];*)
(*PIDTune[tf,"PID","TuningRule"]*)
(*olp=FS[TFconnect[tf,pid]];*)
(*clp=TFloop[olp];*)
(*olpalt=FS[TFconnect[tfalt,pid]];*)
(*clpalt=TFloop[olpalt];*)
(*NSolve[Denominator[clp[s]][[1,1]]==0,s];*)
(*NSolve[Denominator[clpalt[s]][[1,1]]==0,s]*)
(*BodePlot[{olp,olpalt}]*)
(*BodePlot[{clp,clpalt}]*)
(*PlotTFsin[clp,10,30]*)
(*PlotTFsin[clpalt,1,30]*)


(* ::Section::Closed:: *)
(*Regulation*)


(* ::Subsection::Closed:: *)
(*Qiu & Zhou 2013: example 4 and generalize to other TF*)


(* ::Input:: *)
(*(* Choose one to use *)*)
(*(**)
(*P=TFn[1/s^2];  (* used by Zhou in example 4 *)*)
(*P=TFn[1/(s^2+s+1)]; (* 2nd order TF *)*)
(*(* use whole numbers for following *)*)
(*P=TFn[10/(10s^2+101s+10)]; (*equiv to TF[1/((s+0.1)(s+10))]*)*)
(*P=TFn[10/(10s^2+s+10)]           (*equiv to TF[1/(s+0.1s+1)]*)*)
(**)*)
(**)
(*P=TFn[1/s^2]*)
(**)
(*Cn=TFn[(Subscript[q, 1]s+Subscript[q, 2])/(Subscript[p, 0] s^2+Subscript[p, 1]s+Subscript[p, 2])];*)
(**)
(*den[tf_]:=(Denominator[tf[#]][[1,1]])&*)
(*num[tf_]:=(Numerator[tf[#]][[1,1]])&*)
(**)
(*aa=den[P];*)
(*bb=num[P];*)
(*pp=den[Cn];*)
(*qq=num[Cn];*)
(**)
(*n=Exponent[aa[s],s];*)
(*dd[s_]:=poly[{s},r,n];*)
(*(* use \[Pi] as a placeholder for \[Rho], then substitute back later, problem here is not clear how to define \[Rho] a constant *)*)
(*system=aa[-s]aa[s]+\[Pi]^2 bb[-s]bb[s]==dd[-s]dd[s];*)
(*system/.\[Pi]->\[Rho]*)
(**)
(*sol=ToRules[FullSimplify[Reduce[ForAll[s,system],Reals],Assumptions-> Subscript[r, Row[{0}]]>0&&Subscript[r, Row[{1}]]>0&&Subscript[r, Row[{2}]]>0]/.\[Pi]->\[Rho]]*)
(*FullSimplify[system/.\[Pi]->\[Rho] /.sol]*)
(*Subscript[d, \[Rho]]=(dd[s]/.sol)*)
(*Subscript[d, \[Mu]]=Subscript[d, \[Rho]]/.\[Rho]->\[Mu]*)
(**)
(*system=aa[s]pp[s]+bb[s]qq[s]==Subscript[d, \[Rho]] Subscript[d, \[Mu]]*)
(*cleft=CoefficientList[Collect[aa[s]pp[s]+bb[s]qq[s],s],s];*)
(*cright =CoefficientList[Collect[Subscript[d, \[Rho]] Subscript[d, \[Mu]],s],s];*)
(**)
(*eqs=Table[cleft[[i]]==cright[[i]],{i,Length[cleft]}];*)
(*sub=Solve[eqs,{Subscript[p, 0],Subscript[p, 1],Subscript[p, 2],Subscript[q, 1],Subscript[q, 2]}];*)
(**)
(*FullSimplify[system/.sub]*)
(**)
(*FullSimplify[Cn/.sub]*)
(*Cn11=Cn/.sub/.\[Mu]->1/.\[Rho]->1*)
(*FS[N[Cn11]]*)
(**)
(*(* TFxy for input x to output y in Figure 1 of Zhou13.pdf *)*)
(*TF11=TFn[(P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*TF12=TFn[(P[s])/(1+P[s]Cn[s])];*)
(*TF21=TFn[(Cn[s])/(1+P[s]Cn[s])];*)
(*TF22=TFn[(-P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*T={TF11,TF12,TF21,TF22};*)
(**)
(*(* get max eigenvalue of closed loop, which is TF11 *)*)
(*tf11d=FS[Denominator[N[TF11[s]/.sub/.\[Mu]->1/.\[Rho]->1]]];*)
(*Print["Max real eigenvalue = ", Max[Re[s/.NSolve[tf11d==0,s]]]]*)
(**)
(*J[tf_,\[Mu]\[Mu]_,\[Rho]\[Rho]_]:=Total[(H2/@(tf/.sub/.\[Rho]->\[Rho]\[Rho]/.\[Mu]->\[Mu]\[Mu])[[1]])^2];*)
(**)
(*jmin=J[{TFconnectn[TFn[\[Mu]] ,TF11],TFconnect[TFn[\[Rho] \[Mu]] ,TF12],TF21,TFconnectn[TFn[\[Rho]] ,TF22]},1,1][[1,1]];*)
(*RtA=RootApproximant[jmin,2];*)
(*Print["Minimized cost J = ",jmin, "; given as root approx = ",RtA]*)


(* ::Input:: *)
(*Clear[\[Rho],P,Cn,aa,bb,pp,qq,n,system,sol,str,d,cleft,cright,sub,jmin,TF11,TF12,TF21,TF22,T,J]*)


(* ::Input:: *)
(*(* look at tradeoffs for sensitivities for various {\[Mu],\[Rho]} *)*)
(*Table[J[{T[[i]]},1,1][[1,1]],{i,Length[T]}]/Total[Table[J[{T[[i]]},1,1][[1,1]],{i,Length[T]}]]*)
(**)
(*Quiet[Plot[Evaluate[Table[J[{T[[i]]},1,\[Rho]][[1,1]],{i,Length[T]}]/Total[Table[J[{T[[i]]},1,\[Rho]][[1,1]],{i,Length[T]}]]],{\[Rho],0.01,4},PlotPoints->10]]*)
(*(* This second one w/\[Mu] appears to produce the same values as the one above for \[Rho], check *)*)
(*(*Quiet[Plot[Evaluate[Table[J[{T[[i]]},\[Mu],1][[1,1]],{i,Length[T]}]/Total[Table[J[{T[[i]]},\[Mu],1][[1,1]],{i,Length[T]}]]],{\[Mu],0.01,4},PlotPoints\[Rule]10]]*)*)
(**)
(*PlotTF[(TF11/.sub/.\[Rho]->1./.\[Mu]->1)[[1]],30] *)
(*PlotTF[(TF12/.sub/.\[Rho]->1./.\[Mu]->1)[[1]],30] *)
(*PlotTF[(TF21/.sub/.\[Rho]->1./.\[Mu]->1)[[1]],30] *)
(*PlotTF[(TF22/.sub/.\[Rho]->1./.\[Mu]->1)[[1]],30] *)
(**)
(*PlotTF[P,30]*)
(*PlotTF[TFloop[P],30]*)
(*(* can do similar for others *)*)


(* ::Subsection::Closed:: *)
(*Resonance peak example in SAF tutorial article*)


(* ::Input:: *)
(*(* Choose one to use *)*)
(*(**)
(*P=TFn[1/s^2];  (* used by Zhou in example 4 *)*)
(*P=TFn[1/(s^2+s+1)]; (* 2nd order TF *)*)
(*(* use whole numbers for following *)*)
(*P=TFn[10/(10s^2+101s+10)]; (*equiv to TF[1/((s+0.1)(s+10))]*)*)
(*P=TFn[10/(10s^2+s+10)]           (*equiv to TF[1/(s+0.1s+1)], resonance peak *)*)
(**)*)
(**)
(*P=TFn[10/(10s^2+s+10)]*)
(**)
(*Cn=TFn[(Subscript[q, 1]s+Subscript[q, 2])/(Subscript[p, 0] s^2+Subscript[p, 1]s+Subscript[p, 2])];*)
(**)
(*den[tf_]:=(Denominator[tf[#]][[1,1]])&*)
(*num[tf_]:=(Numerator[tf[#]][[1,1]])&*)
(**)
(*aa=den[P];*)
(*bb=num[P];*)
(*pp=den[Cn];*)
(*qq=num[Cn];*)
(**)
(*n=Exponent[aa[s],s];*)
(*dd[s_]:=poly[{s},r,n];*)
(*(* use \[Pi] as a placeholder for \[Rho], then substitute back later, problem here is not clear how to define \[Rho] a constant *)*)
(*system=aa[-s]aa[s]+\[Pi]^2 bb[-s]bb[s]==dd[-s]dd[s];*)
(*system/.\[Pi]->\[Rho]*)
(**)
(*sol=ToRules[FullSimplify[Reduce[ForAll[s,system],Reals],Assumptions-> Subscript[r, Row[{0}]]>0&&Subscript[r, Row[{1}]]>0&&Subscript[r, Row[{2}]]>0]/.\[Pi]->\[Rho]]*)
(*FullSimplify[system/.\[Pi]->\[Rho] /.sol]*)
(*Subscript[d, \[Rho]]=(dd[s]/.sol)*)
(*Subscript[d, \[Mu]]=Subscript[d, \[Rho]]/.\[Rho]->\[Mu]*)
(**)
(*system=aa[s]pp[s]+bb[s]qq[s]==Subscript[d, \[Rho]] Subscript[d, \[Mu]]*)
(*cleft=CoefficientList[Collect[aa[s]pp[s]+bb[s]qq[s],s],s];*)
(*cright =CoefficientList[Collect[Subscript[d, \[Rho]] Subscript[d, \[Mu]],s],s];*)
(**)
(*eqs=Table[cleft[[i]]==cright[[i]],{i,Length[cleft]}];*)
(*sub=Solve[eqs,{Subscript[p, 0],Subscript[p, 1],Subscript[p, 2],Subscript[q, 1],Subscript[q, 2]}];*)
(**)
(*FullSimplify[system/.sub]*)
(**)
(*FullSimplify[Cn/.sub]*)
(*Cn11=Cn/.sub/.\[Mu]->1/.\[Rho]->1*)
(*FS[N[Cn11]]*)
(**)
(*(* TFxy for input x to output y in Eq optTF of manuscript, based on closed loop fig *)*)
(*TF11=TFn[(-P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*TF12=TFn[(P[s])/(1+P[s]Cn[s])];*)
(*TF21=TFn[(-Cn[s])/(1+P[s]Cn[s])];*)
(*TF22=TFn[(-P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*T={TF11,TF12,TF21,TF22};*)
(**)
(*(* get max eigenvalue of closed loop, which is TF11 *)*)
(*tf11d=FS[Denominator[N[TF11[s]/.sub/.\[Mu]->1/.\[Rho]->1]]];*)
(*Print["Max real eigenvalue = ", Max[Re[s/.NSolve[tf11d==0,s]]]]*)
(**)
(*J[tf_,\[Mu]\[Mu]_,\[Rho]\[Rho]_]:=Total[(H2/@(tf/.sub/.\[Rho]->\[Rho]\[Rho]/.\[Mu]->\[Mu]\[Mu])[[1]])^2];*)
(**)
(*jmin=J[{TFconnectn[TFn[\[Mu]] ,TF11],TFconnect[TFn[\[Rho] \[Mu]] ,TF12],TF21,TFconnectn[TFn[\[Rho]] ,TF22]},1,1][[1,1]];*)
(*RtA=RootApproximant[jmin,2];*)
(*Print["Minimized cost J = ",jmin, "; given as root approx = ",RtA]*)


(* ::Input:: *)
(*Clear[\[Rho],P,Cn,aa,bb,pp,qq,n,system,sol,str,d,cleft,cright,sub,jmin,TF11,TF12,TF21,TF22,T,J]*)


(* ::Input:: *)
(*(* look at tradeoffs for sensitivities for various {\[Mu],\[Rho]} *)*)
(**)
(*T={TF11,TF12,TF21,TF22};*)
(**)
(*Table[J[{T[[i]]},1,1][[1,1]],{i,Length[T]}]/Total[Table[J[{T[[i]]},1,1][[1,1]],{i,Length[T]}]]*)
(**)
(*gout=Quiet[Plot[Evaluate[Table[J[{T[[i]]},1,\[Rho]][[1,1]],{i,Length[T]}]/Total[Table[J[{T[[i]]},1,\[Rho]][[1,1]],{i,Length[T]}]]],{\[Rho],0.01,4},PlotPoints->10,Axes->False,Frame->True,FrameTicks->True,FrameLabel->{{Style["Relative \!\(\*SubscriptBox[\(H\), \(2\)]\)",FontSize->14],None},{Style[\[Rho],FontSize->14],None}},BaseStyle->{FontSize->12}]]*)
(**)
(*Export[gdir<>"relH2.pdf",gout];*)
(**)
(*(* This second one w/\[Mu] appears to produce the same values as the one above for \[Rho], check *)*)
(*(*Quiet[Plot[Evaluate[Table[J[{T[[i]]},\[Mu],1][[1,1]],{i,Length[T]}]/Total[Table[J[{T[[i]]},\[Mu],1][[1,1]],{i,Length[T]}]]],{\[Mu],0.01,4},PlotPoints\[Rule]10]]*)*)


(* ::Input:: *)
(*T={TF12,TF21,TF22}; (* do not use TF11 because identical to TF22 in plots *)*)
(**)
(*pl=Table[Null,{i,2},{j,3}];*)
(**)
(*BodePlot[P,ImageSize->Medium];*)
(*bpr={{0.048,30},{-90,22}};*)
(*(* use following to generate comparison w/freq weighting below *)*)
(*(*bpr={{0.048,30},{-21,11}};*)*)
(**)
(*pl[[1,1]]=BodePlot[{P,T}/.sub/.\[Mu]->1/.\[Rho]->0.25,ImageSize->Small,PlotLayout->"Magnitude",PlotRange->bpr];*)
(*pl[[1,2]]=BodePlot[{P,T}/.sub/.\[Mu]->1/.\[Rho]->1,ImageSize->Small,PlotLayout->"Magnitude",PlotRange->bpr];*)
(*pl[[1,3]]=BodePlot[{P,T}/.sub/.\[Mu]->1/.\[Rho]->4,ImageSize->Small,PlotLayout->"Magnitude",PlotRange->bpr];*)
(*tout=Table[OutputResponse[T[[i]]/.sub/.\[Mu]->1/.\[Rho]->0.25,DiracDelta[t],{t,0,10}],{i,Length[T]}];*)
(*pout=OutputResponse[P,DiracDelta[t],{t,0,10}];*)
(*pl[[2,1]]=Plot[{pout,tout},{t,0,10},PlotRange->{-1.2,1.05},ImageSize->Small,AxesOrigin->{0,-1.2}];*)
(*tout=Table[OutputResponse[T[[i]]/.sub/.\[Mu]->1/.\[Rho]->1,DiracDelta[t],{t,0,10}],{i,Length[T]}];*)
(*pl[[2,2]]=Plot[{pout,tout},{t,0,10},PlotRange->{-1.2,1.05},ImageSize->Small,AxesOrigin->{0,-1.2}];*)
(*tout=Table[OutputResponse[T[[i]]/.sub/.\[Mu]->1/.\[Rho]->4,DiracDelta[t],{t,0,10}],{i,Length[T]}];*)
(*pl[[2,3]]=Plot[{pout,tout},{t,0,10},PlotRange->{-1.2,1.05},ImageSize->Small,AxesOrigin->{0,-1.2}];*)
(**)
(*labely1=0.955;*)
(*labely2=0.47;*)
(*gout=GraphicsGrid[pl,Spacings->{0,20},*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.055,labely1}]],Inset[Text["(b)"],ImageScaled[{0.055,labely2}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.39,labely1}]],Inset[Text["(d)"],ImageScaled[{0.39,labely2}]],*)
(*Inset[Text["(e)"],ImageScaled[{0.72,labely1}]],Inset[Text["(f)"],ImageScaled[{0.72,labely2}]]*)
(*}]*)
(**)
(*Export[gdir<>"optH2.pdf",gout];*)


(* ::Subsection::Closed:: *)
(*Frequency weighting*)


(* ::Input:: *)
(*P=TFn[10/(10s^2+s+10)];*)
(*Cn=TFn[(q1 s+q2)/( s^2+p1 s+p2)];*)
(*(* TFxy for input x to output y in Eq optTF of manuscript, based on closed loop fig *)*)
(*TFud=TFn[(-P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*TF\[Eta]d=TFn[(P[s])/(1+P[s]Cn[s])];*)
(*TFun=TFn[(-Cn[s])/(1+P[s]Cn[s])];*)
(*TF\[Eta]n=TFn[(-P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*T={TFud,TF\[Eta]d,TFun,TF\[Eta]n};*)
(**)
(*Jw[\[Mu]_,\[Rho]_,qq1_,qq2_,pp1_,pp2_]:=\[Mu]^2 H2[TFconnectn[Wd,TFud]]^2+\[Mu]^2 \[Rho]^2 H2[TFconnectn[Wd,TF\[Eta]d]]^2+H2[TFconnectn[Wn,TFun]]^2+H2[TFconnectn[Wn,TF\[Eta]n]]^2/.{q1->qq1,q2->qq2,p1->pp1,p2->pp2}*)
(**)
(*MaxEig[qq1_,qq2_,pp1_,pp2_]:=*)
(*Module[*)
(*{Cmin},*)
(*Cmin=Cn/.{q1->qq1,q2->qq2,p1->pp1,p2->pp2};*)
(*Max[Re[s/.NSolve[1+TFconnect[P,Cmin][s]==0,s]]]*)
(*];*)


(* ::Input:: *)
(*(* Controller in manuscript obtained by Qiu & Zhou method, here just show match for cost J and eigenvalues, approx values used *)*)
(*Quiet[Jw[1,1,0.60918,-0.49329,1.73,2.49]]*)
(*Cmin=Cn/.{q1->0.609,q2->-0.49329,p1->1.7313,p2->2.4937}*)
(*(* For four TF of regulator problem, eigenvalues can be analyzed by following *)*)
(*MaxEig[0.60918,-0.49329,1.73,2.49]*)


(* ::Input:: *)
(*(* Here, show that numerical minimization matches analytic approach of Qiu & Zhou *)*)
(*(* Weighting functions of 1 leave original problem unchanged *)*)
(*Wd=TFn[1];*)
(*Wn=TFn[1];min=Quiet[NMinimize[{Jw[1,1,qq1,qq2,pp1,pp2][[1,1]],MaxEig[qq1,qq2,pp1,pp2]<0},{qq1,qq2,pp1,pp2}]]*)
(*Cmin=Cn/.{q1->qq1,q2->qq2,p1->pp1,p2->pp2}/.min[[2]]*)
(*(* For four TF of regulator problem, eigenvalues can be analyzed by following *)*)
(*MaxEig[qq1,qq2,pp1,pp2]/.min[[2]]*)


(* ::Input:: *)
(*(* Now weight inputs with low and high pass filters to shape optimization *)*)
(*Wd=TFn[1/(s+0.1)];*)
(*Wn=TFn[s/(s+10)];min=Quiet[NMinimize[{Jw[1,1,qq1,qq2,pp1,pp2][[1,1]],MaxEig[qq1,qq2,pp1,pp2]<0},{qq1,qq2,pp1,pp2}]]*)
(*Cmin=Cn/.{q1->qq1,q2->qq2,p1->pp1,p2->pp2}/.min[[2]]*)
(*(* For four TF of regulator problem, eigenvalues can be analyzed by following *)*)
(*MaxEig[qq1,qq2,pp1,pp2]/.min[[2]]*)
(*(* Calculate unweighted cost, should be higher than for case in which unweighted system optimized directly *)*)
(*Wd=TFn[1];*)
(*Wn=TFn[1];*)
(*Quiet[Jw[1,1,qq1,qq2,pp1,pp2]/.min[[2]]]*)


(* ::Input:: *)
(*bp1=BodePlot[{P,TF\[Eta]d,TFun,TF\[Eta]n}/.{q1->qq1,q2->qq2,p1->pp1,p2->pp2}/.min[[2]],PlotLayout->"Magnitude",PlotRange->{{0.048,30},{-21,11}}]*)


(* ::Input:: *)
(*bp2=pl[[1,2]]; (* generate from larger array of bode plots above *)*)


(* ::Input:: *)
(*gout=GraphicsRow[{bp2,bp1},*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.080,0.25}]],Inset[Text["(b)"],ImageScaled[{0.595,0.25}]]*)
(*}]*)
(**)
(*Export[gdirlocal<>"freqWt.pdf",gout];*)


(* ::Section::Closed:: *)
(*Stabilization*)


(* ::Subsection::Closed:: *)
(*Small gain theorem calculations*)


(* ::Input:: *)
(*SmallGainTest[G_,\[CapitalDelta]_]:=*)
(*Module[{hinfg, hinfd,hinfgd,maxEig,sol},*)
(*sol=NSolve[1-G[s]\[CapitalDelta][s]==0,s];*)
(*maxEig=Max[Re[s/.sol]];*)
(*hinfg=Hinf[G][[1]];*)
(*hinfd=Hinf[\[CapitalDelta]][[1]];*)
(*hinfgd=Hinf[TFconnectn[G,\[CapitalDelta]]][[1]];*)
(*Print["Hinf(G*\[CapitalDelta]) = ", hinfgd, "; Hinf(G) = ", hinfg,"; Hinf(\[CapitalDelta]) = ", hinfd];*)
(*Print["max eigenvalue = ", maxEig, "; ||G||*||\[CapitalDelta]|| = ", hinfg*hinfd]*)
(*]*)
(**)
(*SmallGainTest[TFn[1.01/(s+1)],TFn[5s/(s+2)^2]]*)
(*SmallGainTest[TFn[1/(s+1)],TFn[0.99/(s+1)]]*)


(* ::Subsection::Closed:: *)
(*Examples of distance and stability*)


(* ::Input:: *)
(*(* Vinnicombe distance, Subscript[\[Delta], \[Nu], ]see page 34 of Qiu and Zhou 2013 *)*)
(*vDist[tf1_,tf2_]:=((1+Hinf[TFn[(tf1[s]-tf2[s])/(1+tf1[s]tf2[-s])]]^-2)^(-1/2))[[1]]*)


(* ::Input:: *)
(*PlotTFstep2[tf_,tmax_,opts_:Null]:=*)
(*Module[{input,yout,t,i,tfa,popt},*)
(*tfa=If[ArrayQ[tf],tf,{tf}];*)
(*popt=If[opts===Null,PlotStyle->Automatic,opts];*)
(*input=UnitStep[t];*)
(*yout=Table[OutputResponse[tfa[[i]],input,{t,0,tmax}],{i,Length[tfa]}];*)
(*Plot[yout,{t,0,tmax},Evaluate@popt]*)
(*]*)
(**)
(*P11=TFn[100/(s+1)];*)
(*P12=TFn[100/((s+1)(0.025s+1)^2)];*)
(*P21=TFn[100/(s+1)];*)
(*P22=TFn[100/(s-1)];*)
(*vDist[P11,P12]*)
(*vDist[P21,P22]*)
(**)
(*opts=PlotRange->{0,400};*)
(*p11=PlotTFstep2[{P11,P12},5,{BaseStyle->{FontSize->12},PlotRange->{0,104}}];*)
(*p12=PlotTFstep2[{TFloop[P11],TFloop[P12]},0.5,{BaseStyle->{FontSize->12},PlotRange->{-1,3.1},AxesOrigin->{0,-1}}];*)
(*p21=PlotTFstep2[{P21,P22},2,{BaseStyle->{FontSize->12},PlotRange->{0,418}}];*)
(*p22=PlotTFstep2[{TFloop[P21],TFloop[P22]},0.1,{BaseStyle->{FontSize->12},PlotRange->{{-0.0060,0.1},{0,1.03}}}];*)
(*gout=GraphicsGrid[{{p11,p21},{p12,p22}},ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.070,0.98}]],Inset[Text["(b)"],ImageScaled[{0.070,0.45}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.590,0.98}]],Inset[Text["(d)"],ImageScaled[{0.590,0.45}]]*)
(*},BaseStyle->{FontSize->12},Spacings->{20,20}]*)
(**)
(*Export[gdir<>"openClosedDist.pdf",gout]*)
(**)


(* ::Subsection::Closed:: *)
(*Controller design for robust stability*)


(* ::Input:: *)
(*bpc[P_,C_]:=NMinimize[{cDist[P[I \[Omega]][[1,1]],-1/C[I \[Omega]][[1,1]]],\[Omega]>-1000000},\[Omega]]*)
(*cDist[c1_?NumericQ,c2_?NumericQ]:=Abs[c1-c2]/Sqrt[(1+Abs[c1]^2)(1+Abs[c2]^2)]*)
(**)
(*bpc[TF[1/s^2],TF[((1+Sqrt[2])s+1)/(s+1+Sqrt[2])]]*)
(*(4+2Sqrt[2])^(-1./2)*)
(*bpc[TF[1/s^2],TF[(2Sqrt[2]s+1)/(s^2+2Sqrt[2]s+4)]]*)
(*bpc[TF[1/s^2],TF[(2.5456s+1)/(0.28s^2+1.5274s+2.88)]]*)
(*1./Sqrt[10]*)


(* ::Input:: *)
(*J2[tf_]:=Total[(H2/@tf)^2];*)
(**)
(*P=TFn[1/s^2];*)
(*Clist={TF[((1+Sqrt[2])s+1.)/(s+1+Sqrt[2])],*)
(*TF[(2Sqrt[2]s+1.)/(s^2+2Sqrt[2]s+4)],TF[(2.5456s+1)/(0.28s^2+1.5274s+2.88)]};*)
(**)
(*(* for i=1, TF21 > 0 as \[Omega] \[Rule] \[Infinity], so J is infinite, although system is stable *)*)
(*For[i=1,i<=3,i++,*)
(*Cn=Clist[[i]];*)
(*TF11=TF[(P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*TF12=TF[(P[s])/(1+P[s]Cn[s])];*)
(*TF21=TF[(Cn[s])/(1+P[s]Cn[s])];*)
(*TF22=TF[(-P[s]Cn[s])/(1+P[s]Cn[s])];*)
(*T={TF11,TF12,TF21,TF22};*)
(*Print["Cost J = ", J2[T][[1,1]]]*)
(*]*)


(* ::Section::Closed:: *)
(*Tracking*)


(* ::Subsection::Closed:: *)
(*Basic functions*)


(* ::Input:: *)
(*\[Psi]=10;*)
(*T=2*\[Psi]*2\[Pi];*)
(*\[Psi]List={1/\[Psi],1,\[Psi]};*)
(*\[Psi]Weight={1,1,0.2};*)
(*P=TFn[1/((s+0.1)(s+10))];*)
(*(*P=TFn[10/(10s^2+s+10)];*)*)
(**)
(*Cn[q0_,q1_,q2_,p0_,p1_,p2_]:=TFn[(q0 s^2+q1 s+q2)/(p0 s^2 + p1 s + p2)];*)
(*R=TFn[Sum[\[Psi]Weight[[i]]\[Psi]List[[i]]/(s^2+\[Psi]List[[i]]^2),{i,Length[\[Psi]List]}]];*)
(*W=TFn[(s+0.85)(Sqrt[\[Psi]]/(s+Sqrt[\[Psi]]))^4];*)
(*RW=TFn[R[s]W[s]];*)
(*U[q0_,q1_,q2_,p0_,p1_,p2_]:=TFn[Cn[q0,q1,q2,p0,p1,p2][s]R[s]/(1+Cn[q0,q1,q2,p0,p1,p2][s]P[s])];*)
(*Y[q0_,q1_,q2_,p0_,p1_,p2_]:=TFn[Cn[q0,q1,q2,p0,p1,p2][s]P[s]R[s]/(1+Cn[q0,q1,q2,p0,p1,p2][s]P[s])];*)
(*En[q0_,q1_,q2_,p0_,p1_,p2_]:=TF[RW[s]-Y[q0,q1,q2,p0,p1,p2][s]];*)
(**)
(*e[q0_?NumericQ,q1_?NumericQ,q2_?NumericQ,p0_?NumericQ,p1_?NumericQ,p2_?NumericQ,T_?NumericQ]:=OutputResponse[En[q0,q1,q2,p0,p1,p2],DiracDelta[t],{t,0,T}];*)
(*u[q0_?NumericQ,q1_?NumericQ,q2_?NumericQ,p0_?NumericQ,p1_?NumericQ,p2_?NumericQ,T_?NumericQ]:=OutputResponse[U[q0,q1,q2,p0,p1,p2],DiracDelta[t],{t,0,T}];*)
(*y[q0_?NumericQ,q1_?NumericQ,q2_?NumericQ,p0_?NumericQ,p1_?NumericQ,p2_?NumericQ,T_?NumericQ]:=OutputResponse[Y[q0,q1,q2,p0,p1,p2],DiracDelta[t],{t,0,T}];*)
(*rw[T_]:=OutputResponse[RW,DiracDelta[t],{t,0,T}];*)
(*r[T_]:=OutputResponse[R,DiracDelta[t],{t,0,T}];*)
(**)
(*Jtrack[q0_?NumericQ,q1_?NumericQ,q2_?NumericQ,p0_?NumericQ,p1_?NumericQ,p2_?NumericQ,\[Rho]_?NumericQ,T_?NumericQ]:=*)
(*Module[{eout,uout},*)
(*eout=e[q0,q1,q2,p0,p1,p2,T];*)
(*uout=u[q0,q1,q2,p0,p1,p2,T];*)
(*NIntegrate[uout^2+\[Rho]^2 eout^2,{t,0,T}][[1]]/((1+\[Rho]^2)T)*)
(*];*)
(**)
(*PlotTrack[q0_,q1_,q2_,p0_,p1_,p2_,T_]:=*)
(*Module[{eout,uout,yout,rout,rwout,pl},*)
(*pl=Table[Null,{i,4}];*)
(*eout=e[q0,q1,q2,p0,p1,p2,T];*)
(*uout=u[q0,q1,q2,p0,p1,p2,T];*)
(*yout=y[q0,q1,q2,p0,p1,p2,T];*)
(*rout=r[T];*)
(*rwout=rw[T];*)
(*Print["Average energy for u, e = ", NIntegrate[{uout^2,eout^2},{t,0,T}]/T];*)
(*Print[pl[[1]]=Plot[{rwout,rout},{t,0,T}]];*)
(*Print[pl[[2]]=Plot[{rwout,eout},{t,0,T}]];*)
(*Print[pl[[3]]=Plot[{rwout,yout},{t,0,T}]];*)
(*Print[pl[[4]]=Plot[{,,uout,eout},{t,0,T}]];*)
(*Return[pl];*)
(*];*)


(* ::Subsection::Closed:: *)
(*Optimize \[Rho] = 1*)


(* ::Input:: *)
(*rho=1;sol=Quiet[NMinimize[{Jtrack[q0,q1,q2,p0,p1,p2,rho,T],q0>=0&&q1>=0&&q2>0.00001&&p0>=0&&p1>=0&&p2>0.00001},{q0,q1,q2,p0,p1,p2},StepMonitor:> Print["J = ", Jtrack[q0,q1,q2,p0,p1,p2,rho,T],"; param = ", {q0,q1,q2,p0,p1,p2}]]]*)


(* ::Input:: *)
(*pl1=PlotTrack[0.5141516041166867`,0.003163748340049146`,0.3241221878154297`,3.3674161392413144`,0.3003430108916596`,0.613363378007872`,T];*)


(* ::Subsection::Closed:: *)
(*Optimize \[Rho] = 10*)


(* ::Input:: *)
(*rho=10;sol=Quiet[NMinimize[{Jtrack[q0,q1,q2,p0,p1,p2,rho,T],q0>=0&&q1>=0&&q2>0.00001&&p0>=0&&p1>=0&&p2>0.00001},{q0,q1,q2,p0,p1,p2},StepMonitor:> Print["J = ", Jtrack[q0,q1,q2,p0,p1,p2,rho,T],"; param = ", {q0,q1,q2,p0,p1,p2}]]]*)


(* ::Input:: *)
(*pl10=PlotTrack[0.24102485065096793`,3.070111817846051`,1.6460212433612864`,0.045258857304346214`,0.08498826845260446`,0.4132130274421302`,T];*)


(* ::Subsection::Closed:: *)
(*Optimize \[Rho] = 100*)


(* ::Input:: *)
(*rho=100;sol=Quiet[NMinimize[{Jtrack[q0,q1,q2,p0,p1,p2,rho,T],q0>=0&&q1>=0&&q2>0.00001&&p0>=0&&p1>=0&&p2>0.00001},{q0,q1,q2,p0,p1,p2},StepMonitor:> Print["J = ", Jtrack[q0,q1,q2,p0,p1,p2,rho,T],"; param = ", {q0,q1,q2,p0,p1,p2}]]]*)


(* ::Input:: *)
(*pl100=PlotTrack[2.773157663129925`,1.0259327225228918`,1.2442819164840477`,0.12086812688064708`,0.0243874649173403`,0.058765807455279706`,T];*)


(* ::Subsection::Closed:: *)
(*Graphics*)


(* ::Input:: *)
(*Export[gdir<>"trackSignal.pdf",pl1[[1]]];*)


(* ::Input:: *)
(*gout=GraphicsGrid[{pl1[[3;;4]],pl10[[3;;4]],pl100[[3;;4]]},ImageSize->Full]*)
(*GraphicsGrid[{{pl1[[3]],pl10[[3]],pl100[[3]]},{pl1[[4]],pl10[[4]],pl100[[4]]}},ImageSize->Full];*)
(*Export[gdir<>"trackPerform.pdf",gout];*)


(* ::Subsection::Closed:: *)
(*Stability margins*)


(* ::Input:: *)
(*bpc[P,Cn[0.5141516041166867`,0.003163748340049146`,0.3241221878154297`,3.3674161392413144`,0.3003430108916596`,0.613363378007872`]]*)
(*bpc[P,Cn[0.24102485065096793`,3.070111817846051`,1.6460212433612864`,0.045258857304346214`,0.08498826845260446`,0.4132130274421302`]]*)
(*bpc[P,Cn[2.773157663129925`,1.0259327225228918`,1.2442819164840477`,0.12086812688064708`,0.0243874649173403`,0.058765807455279706`]]*)
(*bpc[P,Cn[0.19253502653153443`,0.9448082685598449`,0.4932955517287573`,0.7686331626195105`,0.2807095076478491`,0.9798069233794141`]]*)


(* ::Subsubsection::Closed:: *)
(*Optimize \[Rho] = 100 w/stability margin*)


(* ::Input:: *)
(*rho=100;*)
(*(* NOT WORKING CORRECTLY *)*)
(**)
(*(* In NMinimize, need to prevent evaluation before numbers feed in *)*)
(*Clear[Cn];*)
(*Cn[q0_?NumericQ,q1_?NumericQ,q2_?NumericQ,p0_?NumericQ,p1_?NumericQ,p2_?NumericQ]:=TFn[(q0 s^2+q1 s+q2)/(p0 s^2 + p1 s + p2)];*)
(**)
(*sol=Quiet[NMinimize[{Jtrack[q0,q1,q2,p0,p1,p2,rho,T],(bpc[P,Cn[q0,q1,q2,p0,p1,p2]][[1]])>0.25},{q0,q1,q2,p0,p1,p2},StepMonitor:> Print["J = ", Jtrack[q0,q1,q2,p0,p1,p2,rho,T],"; bpc = ",bpc[P,Cn[q0,q1,q2,p0,p1,p2]][[1]] ,"; param = ", {q0,q1,q2,p0,p1,p2}]]]*)


(* ::Input:: *)
(*Clear[Cn];*)
(*Cn[q0_?NumericQ,q1_?NumericQ,q2_?NumericQ,p0_?NumericQ,p1_?NumericQ,p2_?NumericQ]:=TFn[(q0 s^2+q1 s+q2)/(p0 s^2 + p1 s + p2)];*)
(**)
(*rho=100;*)
(*sol=Quiet[NMinimize[{Jtrack[q0,q1,q2,p0,p1,p2,rho,T],(bpc[P,Cn[q0,q1,q2,p0,p1,p2]][[1]])>0.25},{q0,q1,q2,p0,p1,p2},StepMonitor:> Print["J = ", Jtrack[q0,q1,q2,p0,p1,p2,rho,T],"; bpc = ",bpc[P,Cn[q0,q1,q2,p0,p1,p2]][[1]] ,"; param = ", {q0,q1,q2,p0,p1,p2}],Method->{"DifferentialEvolution","RandomSeed"->121}]]*)


(* ::Input:: *)
(**)
(**)


(* ::Input:: *)
(*pl100sb=PlotTrack[0.19253502653153443`,0.9448082685598449`,0.4932955517287573`,0.7686331626195105`,0.2807095076478491`,0.9798069233794141`,T];*)


(* ::Input:: *)
(*gout=GraphicsRow[{pl100sb[[3]],pl100sb[[4]]},ImageSize->Full]*)
(*Export[gdir<>"trackStability.pdf", gout];*)


(* ::Section::Closed:: *)
(*State space*)


(* ::Subsection::Closed:: *)
(*Basic functions*)


(* ::Input:: *)
(*StateSpaceMatrix[ssm_]:=Normal[ssm];*)


(* ::Subsection::Closed:: *)
(*LQ regulation with Riccati solution, P=1/(s+0.1)(s+10)*)


(* ::Subsubsection::Closed:: *)
(*Single input model, disturbance to input 1*)


(* ::Input:: *)
(*\[Alpha]\[Alpha]=10.1;*)
(*\[Beta]\[Beta]=1;*)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},{{1},{0}},{{1,0}},{{0}}}]*)
(*TransferFunctionModel[ssme]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Only one input into state 1, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*rho={1,Sqrt[10],10};*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* in this case, input only comes to state 2, see state matrix BB *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2]];*)
(*];*)
(*(* code for pl: # inputs, input used for disturbance, state response plotted *)*)
(*pl111=Plot[sysout1,{t,0,5},PlotRange->All];*)
(*pl112=Plot[sysout2,{t,0,5},PlotRange->{All,{-0.098,0}},AxesOrigin->{0,-0.098}];*)


(* ::Subsubsection::Closed:: *)
(*Single input model, disturbance to input 2*)


(* ::Input:: *)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},{{0},{1}},{{1,0}},{{0}}}]*)
(*TransferFunctionModel[ssme]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Only one input into state 2, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*rho={1,Sqrt[10],10};*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* in this case, input only comes to state 2, see state matrix BB *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2]];*)
(*];*)
(*(* code for pl: # inputs, input used for disturbance, state response plotted *)*)
(*pl121=Plot[sysout1,{t,0,20},PlotRange->{All,{0,0.098}}];*)
(*pl122=Plot[sysout2,{t,0,1},PlotRange->All];*)


(* ::Subsubsection::Closed:: *)
(*Two input model, disturbance to input 1*)


(* ::Input:: *)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},IdentityMatrix[2],{{1,0}},{{0,0}}}]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Two inputs, one into each state, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*rho={1,Sqrt[10],10};*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* examine response to input 2 only, to match prior case *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1,1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1,2]];*)
(*];*)
(*pl211=Plot[sysout1,{t,0,5},PlotRange->All];*)
(*pl212=Plot[sysout2,{t,0,5},PlotRange->{All,{-0.098,0}},AxesOrigin->{0,-0.098}];*)


(* ::Subsubsection:: *)
(*Two input model, disturbance to input 2*)


(* ::Input:: *)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},IdentityMatrix[2],{{1,0}},{{0,0}}}]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Two inputs, one into each state, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*rho={1,Sqrt[10],10};*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* examine response to input 2 only, to match prior case *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2,1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2,2]];*)
(*];*)
(*pl221=Plot[sysout1,{t,0,20},PlotRange->{All,{0,0.098}}];*)
(*pl222=Plot[sysout2,{t,0,1},PlotRange->All];*)
(**)
(*GraphicsGrid[{{pl111,pl211},{pl112,pl212}},ImageSize->Large]*)
(*GraphicsGrid[{{pl121,pl221},{pl122,pl222}},ImageSize->Large]*)


(* ::Subsection::Closed:: *)
(*LQ regulation with Riccati solution, P=1/(s^2+0.1s+1)*)


(* ::Input:: *)
(*\[Alpha]\[Alpha]=0.1;*)
(*\[Beta]\[Beta]=1;*)
(*rho={0.25,1,4};*)
(*yr1={-0.65,1.07};*)
(*yr2={-0.51,0.82};*)
(*yr3={-0.82,0.51};*)


(* ::Subsubsection::Closed:: *)
(*Single input model, disturbance to input 1*)


(* ::Input:: *)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},{{1},{0}},{{1,0}},{{0}}}]*)
(*TransferFunctionModel[ssme]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Only one input into state 1, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* in this case, input only comes to state 2, see state matrix BB *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2]];*)
(*];*)
(*(* code for pl: # inputs, input used for disturbance, state response plotted *)*)
(*pl111=Plot[sysout1,{t,0,10},PlotRange->{All,yr1}];*)
(*pl112=Plot[sysout2,{t,0,10},PlotRange->{All,yr3}];*)


(* ::Subsubsection::Closed:: *)
(*Single input model, disturbance to input 2*)


(* ::Input:: *)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},{{0},{1}},{{1,0}},{{0}}}]*)
(*TransferFunctionModel[ssme]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Only one input into state 2, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* in this case, input only comes to state 2, see state matrix BB *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2]];*)
(*];*)
(*(* code for pl: # inputs, input used for disturbance, state response plotted *)*)
(*pl121=Plot[sysout1,{t,0,10},PlotRange->{All,yr2}];*)
(*pl122=Plot[sysout2,{t,0,10},PlotRange->{All,yr1}];*)


(* ::Subsubsection::Closed:: *)
(*Two input model, disturbance to input 1*)


(* ::Input:: *)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},IdentityMatrix[2],{{1,0}},{{0,0}}}]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Two inputs, one into each state, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* examine response to input 2 only, to match prior case *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1,1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[1,2]];*)
(*];*)
(*pl211=Plot[sysout1,{t,0,10},PlotRange->{All,yr1}];*)
(*pl212=Plot[sysout2,{t,0,10},PlotRange->{All,yr3}];*)


(* ::Subsubsection::Closed:: *)
(*Two input model, disturbance to input 2*)


(* ::Input:: *)
(*ssme=StateSpaceModel[{{{0,1},{-\[Beta],-\[Alpha]}},IdentityMatrix[2],{{1,0}},{{0,0}}}]*)
(*{AA,BB,CC,DD}=Normal[ssme];*)
(*{AA//MatrixForm,BB//MatrixForm,CC//MatrixForm};*)
(*Print["Two inputs, one into each state, from B matrix: ", BB //MatrixForm];*)
(*R=IdentityMatrix[Dimensions[BB][[2]]];*)
(*sysout1={Null,Null,Null};*)
(*sysout2={Null,Null,Null};*)
(*For[i=1,i<=3,i++,*)
(*Q=\[Rho]^2 ({*)
(* {1, 0},*)
(* {0, 1}*)
(*})/.\[Rho]->rho[[i]];*)
(*(* the next function solves the Riccati Matrix equation *)*)
(*K=LQRegulatorGains[ssme/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],{Q,R}]; *)
(*(* Print[K]; *)*)
(*sys=SystemsModelStateFeedbackConnect[ssme,K];*)
(*(* examine response to input 2 only, to match prior case *)*)
(*(* look at both state responses, state 1 is equivalent to output response *)*)
(*sysout1[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2,1]];*)
(*sysout2[[i]]=StateResponse[sys/.\[Alpha]->\[Alpha]\[Alpha]/.\[Beta]->\[Beta]\[Beta],DiracDelta[t],{t,0,20}][[2,2]];*)
(*];*)
(*pl221=Plot[sysout1,{t,0,10},PlotRange->{All,yr2}];*)
(*pl222=Plot[sysout2,{t,0,10},PlotRange->{All,yr1}];*)
(**)
(*GraphicsGrid[{{pl111,pl211},{pl112,pl212}},ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.065,0.55}]],Inset[Text["(b)"],ImageScaled[{0.580,0.55}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.065,0.04}]],Inset[Text["(d)"],ImageScaled[{0.580,0.04}]]*)
(*}];*)
(*gout=GraphicsGrid[{{pl121,pl221},{pl122,pl222}},ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.065,0.55}]],Inset[Text["(b)"],ImageScaled[{0.580,0.55}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.065,0.04}]],Inset[Text["(d)"],ImageScaled[{0.580,0.04}]]*)
(*}]*)
(**)
(*Export[gdir<>"lqrReg.pdf",  gout];*)


(* ::Subsection::Closed:: *)
(*LQ tracking with Riccati solution*)


(* ::Input:: *)
(*(* Make a system with two transfer functions. One function for the system process, and one function for the reference signal to be tracked. Here, the upper left tf is the process, and the lower right tf is the reference signal *)*)
(*tfa=TFn[{{1/(s^2+\[Alpha] s+\[Beta]),0},{0,Subscript[\[Omega], 0]/(s^2+Subscript[\[Omega], 0]^2)+Subscript[\[Omega], 1]/(s^2+Subscript[\[Omega], 1]^2)}}];*)
(*sstmp=StateSpaceModel[tfa]*)
(*Normal[sstmp];  (* use this as basis to create State Space models *)*)
(*(* Rewrite the output C matrix so that the output is the error difference between the process output and the reference signal. One input, which is the standard input into the second state of the process, as occurs in the standard state space form for the transfer function given above for the process *)*)
(**)
(*(* Note that by allowing state feedback, the embedded reference signal is separated into four components for the number of states of that signal, with each component available as a state that can be used to create the feedback input. On the one hand, the information in the signal is the same for a single signal output value and for the four states, on the other hand, here providing the system with the equivalent of perfect state estimators for each of the four state underlying the single reference signal. Here, done this way to provide easy use of standard LQ regulator theory and available MMA functions. Would be interesting to compare this classic result with various assumptions about the sensors, associated costs,  and estimators required to measure each state. Rich literature exists about state estimators and related topics. See for example Anderson and Moore (1989). *)*)
(**)
(*(* outputs are error, e, the process output, y, and the reference, r *)*)
(*ssme1=StateSpaceModel[{{{0,1,0,0,0,0},{-\[Beta],-\[Alpha],0,0,0,0},{0,0,0,1,0,0},{0,0,0,0,1,0},{0,0,0,0,0,1},{0,0,-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\) \!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0,-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\)-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0}},{{0,0},{1,0},{0,0},{0,0},{0,0},{0,1}},{{1,0,-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\) Subscript[\[Omega], 1]-Subscript[\[Omega], 0] \!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0,-Subscript[\[Omega], 0]-Subscript[\[Omega], 1],0},{1,0,0,0,0,0},{0,0,\!\( *)
(*\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\ *)
(*\*SubscriptBox[\(\[Omega]\), \(1\)]\)+Subscript[\[Omega], 0] \!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0,Subscript[\[Omega], 0]+Subscript[\[Omega], 1],0}},{{0,0},{0,0},{0,0}}}]*)
(*(* Create inputs for both internal states of the process, so that state feedback can be used for both states, note that the text reverses the first and second columns of the input, B, but maintains the same logic *)*)
(*ssme2=StateSpaceModel[{{{0,1,0,0,0,0},{-\[Beta],-\[Alpha],0,0,0,0},{0,0,0,1,0,0},{0,0,0,0,1,0},{0,0,0,0,0,1},{0,0,-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\) \!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0,-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\)-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0}},{{0,1,0},{1,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,1}},{{1,0,-\!\(\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\) Subscript[\[Omega], 1]-Subscript[\[Omega], 0] \!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0,-Subscript[\[Omega], 0]-Subscript[\[Omega], 1],0},{1,0,0,0,0,0},{0,0,\!\( *)
(*\*SubsuperscriptBox[\(\[Omega]\), \(0\), \(2\)]\ *)
(*\*SubscriptBox[\(\[Omega]\), \(1\)]\)+Subscript[\[Omega], 0] \!\(\*SubsuperscriptBox[\(\[Omega]\), \(1\), \(2\)]\),0,Subscript[\[Omega], 0]+Subscript[\[Omega], 1],0}},0*IdentityMatrix[3]}]*)
(*(* use only first output (the error) for performance index *)*)
(*Q=0*IdentityMatrix[3];*)
(*Q[[1,1]]=\[Rho]^2;*)
(**)
(*\[Alpha]=10.1;*)
(*\[Beta]=1;*)
(*\[Rho]=1;*)
(*Subscript[\[Omega], 0]=0.1;*)
(*Subscript[\[Omega], 1]=1;*)
(**)
(*(* inWeight is relative weighting of inputs in performance *)CalcK[inWeight_]:=Module[{zz,K1,K2},  *)
(*(* "All" for all outputs available, but weighting only first output in performance index via Q, and {1} or {1,2} for using only first or first and second inputs for feedback, ignore last input, which is embedded reference input and not part of the response model *)*)
(*K1=LQOutputRegulatorGains[{ssme1,All,{1}},{Q,inWeight*IdentityMatrix[1]}];*)
(*K2=LQOutputRegulatorGains[{ssme2,All,{1,2}},{Q,inWeight*IdentityMatrix[2]}];*)
(*(* put back the third input as zero to make K matrices compatible with model, but avoid perturbing the embedded reference input *)*)
(*zz=Table[0,Length[K1[[1]]]];*)
(*K1=Append[K1,zz];*)
(*K2=Append[K2,zz];*)
(*Return[{K1,K2}];*)
(*]*)
(**)
(*PlotTrack[inWeight_,showK_:False]:=Module[{K1,K2,sys1,sys2,sysout11,sysout13,sysout21,sysout23,p1,p2},*)
(*{K1,K2}=CalcK[inWeight];*)
(*If[showK,Print["K1 = ", K1];Print["K2 = ", K2];];*)
(*sys1=SystemsModelStateFeedbackConnect[ssme1,K1];*)
(*sys2=SystemsModelStateFeedbackConnect[ssme2,K2];*)
(**)
(*(* This system has one input and three outputs: e=y-r, y, r *)*)
(*sysout11=OutputResponse[sys1,{DiracDelta[t],DiracDelta[t]},{t,0,100}][[1]];*)
(*sysout13=OutputResponse[sys1,{DiracDelta[t],DiracDelta[t]},{t,0,100}][[3]];*)
(*(* This system has two inputs which must be set explicitly, use only input 1 to match prior system *)*)
(*sysout21=OutputResponse[sys2,{DiracDelta[t],0,DiracDelta[t]},{t,0,100}][[1]];*)
(*sysout23=OutputResponse[sys2,{DiracDelta[t],0,DiracDelta[t]},{t,0,100}][[3]];*)
(**)
(*p1=Plot[{sysout13,sysout11},{t,0,100},PlotRange->{-2.2,2.2},Axes->{False,True}];*)
(*p2=Plot[{sysout23,sysout21},{t,0,100},PlotRange->{-2.2,2.2},Axes->{False,True}];*)
(*Return[{p1,p2}];*)
(*]*)
(**)
(*{p1,p2}=PlotTrack[0.1,True];*)
(*{p3,p4}=PlotTrack[0.01];*)
(**)
(*gout=GraphicsGrid[{{p1,p3},{p2,p4}},ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.045,0.98}]],Inset[Text["(b)"],ImageScaled[{0.560,0.98}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.045,0.465}]],Inset[Text["(d)"],ImageScaled[{0.560,0.465}]]*)
(*}]*)
(**)
(*Export[gdir<>"stateTrack.pdf",gout];*)
(**)
(*Clear[\[Alpha],\[Beta]]*)
(*Subscript[\[Omega], 0]=.; Subscript[\[Omega], 1]=.;*)


(* ::Section::Closed:: *)
(*Nonlinearity*)


(* ::Subsection::Closed:: *)
(*Linear approximations*)


(* ::Input:: *)
(*(* Equilibrium point is x1=1 and x2=1/2\[Gamma], which is given here as the "operating point" for analysis of the state space model, note output set to deviation from equilibrium, Subscript[x, 2]-1/(2\[Gamma]), to make the transformed model show the same output as deviation from the equilibrium *)*)
(*af1=AffineStateSpaceModel[{{-Subscript[x, 1]+1,\!\( *)
(*\*SubsuperscriptBox[\(x\), \(1\), \(n\)]/\((1 + *)
(*\*SubsuperscriptBox[\(x\), \(1\), \(n\)])\)\)-\[Gamma] Subscript[x, 2]},{{1},{0}},{Subscript[x, 2]-1/(2\[Gamma])}},{{Subscript[x, 1],1},{Subscript[x, 2],1/(2\[Gamma])}}]*)
(**)
(*(* transform affine model to have (0, 0) equilibrium as new operating point *)*)
(*af=StateSpaceTransform[af1,{{Subscript[x, 1]->Subscript[z, 1]+1,Subscript[x, 2]->Subscript[z, 2]+1/(2\[Gamma])},{Subscript[z, 1],Subscript[z, 2]}}]*)
(**)
(*(* linearized around the operating point, so zero is equivalent to equilibrium operating point in af model *)*)
(*ssm=StateSpaceModel[af]  *)
(*TransferFunctionModel[ssm]*)
(**)
(*(* af system initialized at operating point, which, for transformed system is (0, 0), so results give deviation from zero which is deviation from equilibrium; MMA seems to want offset on Dirac delta, so give a very small offset *)*)
(**)
(*ddWeight={0.1,0.1\[Sqrt]10,1};*)
(*g=Table[Null,Length[ddWeight]];*)
(*For[i=1,i<=Length[ddWeight],i++,out1=OutputResponse[af/.n->2/.\[Gamma]->1,ddWeight[[i]]*DiracDelta[t-0.000001],{t,0,100}];*)
(*out2=OutputResponse[ssm/.n->2/.\[Gamma]->1,ddWeight[[i]]*DiracDelta[t],{t,0,100}];*)
(*g[[i]]=Plot[{out1,out2},{t,0,8}];*)
(*];*)
(*gout=GraphicsRow[g,ImageSize->Full]*)
(*Export[gdir<>"linApprox.pdf", gout];*)


(* ::Subsection::Closed:: *)
(*Linear approximation: regulation w/state feedback*)


(* ::Input:: *)
(*\[Rho]={1,\[Sqrt]10,10};*)
(*n=2;*)
(*\[Gamma]=1;*)
(*g=Table[Null,Length[ddWeight]];*)
(*(* set weighting for single input to 1 *)*)
(*R={{1.}};*)
(*For[i=1,i<=Length[\[Rho]],i++,*)
(*(* set weighting for states to \[Rho]^2 *)*)
(*Q=\[Rho][[i]]^2 IdentityMatrix[2];*)
(*K=LQRegulatorGains[ssm,{Q,R}];*)
(*lin=SystemsModelStateFeedbackConnect[ssm,K];*)
(*nonlin=SystemsModelStateFeedbackConnect[af,K];*)
(*out1=OutputResponse[nonlin,DiracDelta[t-0.000001],{t,0,100}];*)
(*out2=OutputResponse[lin,DiracDelta[t],{t,0,100}];*)
(*g[[i]]=Plot[{out1,out2},{t,0,8},PlotRange->{0,0.16}];*)
(*];*)
(*gout=GraphicsRow[g,ImageSize->Full]*)
(*Export[gdir<>"linApproxFeedback.pdf", gout];*)
(*Clear[\[Rho],n,\[Gamma]];*)


(* ::Section::Closed:: *)
(*Adaptive control*)


(* ::Subsection::Closed:: *)
(*Reference model plot*)


(* ::Input:: *)
(*\[Omega]1=0.1;*)
(*tmax=100; (*2\[Pi]/ \[Omega]1;*)*)
(*am={.3,3,3};*)
(*bm={.6,6,1.5};*)
(*pl=Table[Null,3];*)
(**)
(*For[i=1,i<=3,i++,*)
(*r[t_]:=Sin[\[Omega]1*t]+Sin[t];*)
(*ymf[t_]:=(ymm[t]/.DSolve[ymm'[t]==- am[[i]]*ymm[t]+bm[[i]]*r[t]&&ymm[0]==0,ymm[t],t])[[1]];*)
(*ym[t_]:=Evaluate[ymf[t]]; *)
(**)
(*pl[[i]]=Plot[{r[t],ym[t]},{t,0,tmax},PlotRange->{-4.2,4.2},AxesOrigin->{0,-4.2}];*)
(*]*)
(**)
(*gout=GraphicsRow[pl,ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text["(a)"],ImageScaled[{0.048,0.92}]],Inset[Text["(b)"],ImageScaled[{0.391,0.92}]],*)
(*Inset[Text["(c)"],ImageScaled[{0.731,0.92}]]*)
(*}]*)
(*Export[gdir<>"mracModel.pdf", gout];*)
(*Clear[\[Omega]1,tmax,am,bm,pl];*)


(* ::Subsection::Closed:: *)
(*Model reference adaptive control*)


(* ::Input:: *)
(*\[Omega]1=0.1;*)
(*tm={3,1.5,1.5};*)
(*tmax=2\[Pi]/ \[Omega]1;*)
(*am=3;*)
(*bm=6;*)
(*a=4;*)
(*b=2;*)
(*k1s=-a/b;*)
(*k2s=-am/b;*)
(*ws=bm/b;*)
(*\[Gamma]={0.01,0.1,1};*)
(*rng={5,5,5};*)
(*gr=Table[Null,{3},{3}];*)
(**)
(*f[y_]:=y^2;*)
(*g[y_]:=1;r[t_]:=Sin[\[Omega]1*t]+Sin[t];*)
(*ymf[t_]:=(ymm[t]/.DSolve[ymm'[t]==- am*ymm[t]+bm*r[t]&&ymm[0]==0,ymm[t],t])[[1]]*)
(*ym[t_]:=Evaluate[ymf[t]]; *)
(**)
(*For[i=1,i<=3,i++,*)
(*(* err = y - ym and thus y = ym+err *)*)
(*\[Gamma]1=\[Gamma]2=\[Gamma]3=\[Gamma][[i]];*)
(*soln=NDSolve[*)
(*{err'[t]==-am*err[t]+b((k1[t]-k1s)f[ym[t]+err[t]]+(k2[t]-k2s)(ym[t]+err[t])+(w[t]-ws)r[t]),*)
(*k1'[t]==-\[Gamma]1*err[t]*f[ym[t]+err[t]],*)
(*k2'[t]==-\[Gamma]2*err[t]*(ym[t]+err[t]),*)
(*w'[t]==-\[Gamma]3*err[t]*r[t],*)
(*err[0]==0,*)
(*k1[0]==-.1,*)
(*k2[0]==-.1,*)
(*w[0]==.1},*)
(*{err[t],k1[t],k2[t],w[t]},{t,0,tm[[i]]*tmax}];*)
(**)
(*{err[t],k1[t],k2[t],w[t]}={err[t],k1[t],k2[t],w[t]}/.soln[[1]];*)
(**)
(**)
(*gr[[1,i]]=Plot[{ym[t]+err[t],ym[t]},{t,0,tm[[i]]*tmax},PlotRange->{-rng[[i]],rng[[i]]},AxesOrigin->{0,-rng[[i]]}];*)
(*gr[[2,i]]=Plot[{,,err[t]},{t,0,tm[[i]]*tmax},PlotRange->{-rng[[i]],rng[[i]]},AxesOrigin->{0,-rng[[i]]}];*)
(*gr[[3,i]]=Plot[{k1[t],k2[t],w[t]},{t,0,tm[[i]]*tmax},PlotRange->{-4.2,3.2},AxesOrigin->{0,-4.2}];*)
(*Clear[err,k1,k2,w];*)
(*]*)
(**)
(*gout=GraphicsGrid[gr,ImageSize->Large]*)
(*Export[gdir<>"mracMain.pdf",gout];*)
(**)
(*Clear[am,bm,\[Omega]1,a,b,k1s,k2s,ws,\[Gamma]1,\[Gamma]2,\[Gamma]3,err,k1,k2,w,tmax,tm,\[Gamma],gr,rng];*)
(**)


(* ::Input:: *)
(*Plot[r[t],{t,0,100}]*)


(* ::Section::Closed:: *)
(*Model predictive control*)


(* ::Subsection::Closed:: *)
(*Duffing equations*)


(* ::Input:: *)
(*(* see MMA help: tutorial/FunctionsWithSensitiveDependenceOnTheirInput *)*)
(*(* Duffing equation, see https://en.wikipedia.org/wiki/Duffing_equation *)*)
(**)
(*(* MMA help page parameters *)*)
(*\[Delta]=0.15;*)
(*\[Alpha]=-1;*)
(*\[Beta]=1;*)
(*\[Gamma]=0.3;*)
(*\[Omega]=1.0;*)
(**)
(*(* Wikipedia parameters: \[Gamma]=0.2,0.28,0.29,0.37,0.50,0.65 *)*)
(*(*\[Delta]=0.3;*)
(*\[Alpha]=-1;*)
(*\[Beta]=1;*)
(*\[Gamma]=0.37;*)
(*\[Omega]=1.2;*)*)
(**)
(*ssm2=NonlinearStateSpaceModel[{x''[t]+\[Delta] x'[t]+\[Alpha] x[t]+\[Beta] x[t]^3==u[t]},{{x[t],-1},{x'[t],1.001},{x''[t]}},{u[t]},{x[t]},t]*)
(*tmax=100;*)
(*state=StateResponse[ssm2,\[Gamma] Cos[\[Omega] t],{t,0,tmax}];*)
(*(* position and velocity, state 1 is also the "output" *)*)
(*Plot[state,{t,0,tmax}]*)
(*Clear[\[Alpha],\[Beta],\[Delta],\[Gamma],\[Omega],tmax];*)


(* ::Subsection::Closed:: *)
(*Optimize*)


(* ::Input:: *)
(*uu[t_]:=Sum[(t-\[Alpha])u[\[Alpha]],{\[Alpha],0,t-1}]*)
(*y[t_,y0_,yv0_]:=y0+yv0*t*)
(*x[t_,x0_,xv0_]:=x0+xv0*t+uu[t]*)
(*J[t_,x0_,xv0_,y0_,yv0_,\[Rho]_,\[Tau]_]:=Sum[(x[t,x0,xv0]-y[t,y0,yv0])^2+\[Rho] u[t]^2,{t,0,\[Tau]}]*)
(**)
(*J[t,x0,xv0,y0,yv0,\[Rho],\[Tau]]*)


(* ::Input:: *)
(*(* symbol result *)*)
(*\[Tau]=3;*)
(*derivs=Table[D[J[t,x0,xv0,y0,yv0,\[Rho],\[Tau]],u[i]]==0,{i,0,\[Tau]}];*)
(*soln=FullSimplify[Solve[derivs,Table[u[i],{i,0,\[Tau]}]]];*)
(*soln[[1,1]]*)
(*Clear[\[Tau]]*)


(* ::Input:: *)
(*(* numeric results *)*)
(*\[Tau]=10;*)
(*x0=0;*)
(*xv0=0;*)
(*y0=2.;*)
(*yv0=3;*)
(*\[Rho]=1;*)
(*derivs=Table[D[J[t,x0,xv0,y0,yv0,\[Rho],\[Tau]],u[i]]==0,{i,0,\[Tau]}];*)
(*soln=FullSimplify[Solve[derivs,Table[u[i],{i,0,\[Tau]}]]]*)
(*derivs=Table[D[J[t,x0,xv0,y0,yv0,\[Rho],\[Tau]],u[i]]==0,{i,0,\[Tau]}];*)
(*soln=FullSimplify[Solve[derivs,Table[u[i],{i,0,\[Tau]}]]];FullSimplify[J[t,x0,xv0,y0,yv0,\[Rho],\[Tau]]/.soln];*)
(*Flatten[Table[(x[i,x0,xv0]-y[i,y0,yv0])/.soln,{i,0,\[Tau]}]]*)
(*Clear[\[Tau],x0,xv0,y0,yv0,\[Rho]]*)


(* ::Input:: *)
(*(* plots showing how well MPC tracks Duffing equation chaos *)*)
(*\[Delta]=0.15;*)
(*\[Alpha]=-1;*)
(*\[Beta]=1;*)
(*\[Gamma]=0.3;*)
(*\[Omega]=1.0;*)
(*tmax=100;*)
(**)
(*(* Duffing equation from above *)*)
(*ssm2=NonlinearStateSpaceModel[{x''[t]+\[Delta] x'[t]+\[Alpha] x[t]+\[Beta] x[t]^3==u[t]},{{x[t],-1},{x'[t],1.001},{x''[t]}},{u[t]},{x[t]},t];*)
(*state=StateResponse[ssm2,\[Gamma] Cos[\[Omega] t],{t,0,tmax}];*)
(*Clear[\[Delta],\[Alpha],\[Beta]];*)
(**)
(*\[Tau]=10;*)
(*x0=0;*)
(*xv0=0;*)
(*y0=state[[1]]/.t->0;*)
(*yv0=state[[2]]/.t->0;*)
(*\[Rho]={1,10,100};*)
(*xx=Table[0,{j,tmax+1}];*)
(*yy=Table[0,{j,tmax+1}];*)
(*xx[[1]]=x0;*)
(*yy[[1]]=y0;*)
(*pl=Table[Null,Length[\[Rho]]];*)
(*For[k=1,k<=Length[\[Rho]],k++,*)
(*For[j=1,j<=tmax,j++,*)
(*derivs=Table[D[J[t,x0,xv0,y0,yv0,\[Rho][[k]],\[Tau]],u[i]]==0,{i,0,\[Tau]}];*)
(*soln=FullSimplify[NSolve[derivs,Table[u[i],{i,0,\[Tau]}]]];*)
(*FullSimplify[J[t,x0,xv0,y0,yv0,\[Rho][[k]],\[Tau]]/.soln];*)
(*diff=Flatten[Table[(x[i,x0,xv0]-y[i,y0,yv0])/.soln,{i,0,\[Tau]}]];*)
(*(*Print["j = ", j];Print[soln];Print[diff];*)*)
(*x0=Flatten[(x[1,x0,xv0]/.soln),\[Infinity]][[1]];*)
(*xx[[j+1]]=x0;*)
(*xv0=xv0+u[0]/.soln;*)
(*yy[[j+1]]=y0=state[[1]]/.t->j;*)
(*yv0=state[[2]]/.t->j;*)
(*];*)
(*(* Print[xx,"\n", yy]; *)*)
(*pl[[k]]=ListLinePlot[{yy,xx},InterpolationOrder->2,Axes->False,PlotRange->{{-0.12*tmax,1.08*tmax},{-2,2.07}}];*)
(*];*)
(**)
(*gout=GraphicsRow[pl,ImageSize->Large,Spacings->{10,10},*)
(*Epilog->{*)
(*Inset[Text[Style["\[Rho] = 1",FontSize->11]],ImageScaled[{0.085,0.92}]],Inset[Text[Style["\[Rho] = 10",FontSize->11]],ImageScaled[{0.420,0.92}]],*)
(*Inset[Text[Style["\[Rho] = 100",FontSize->11]],ImageScaled[{0.758,0.92}]],*)
(*Inset[Text[Style["Time",FontSize->18]],ImageScaled[{0.5,0.06}]],*)
(*Rotate[Inset[Text[Style["Signal value",FontSize->14]],ImageScaled[{0.01,0.55}]],90Degree]}]*)
(*(*gout=Graphics[Rotate[Text[Style["Location",FontSize\[Rule]16]],90Degree]]*)*)
(*Export[gdir<>"mpcDuffing.pdf",gout];*)
(*Clear[\[Tau],x0,xv0,y0,yv0,\[Rho],tmax]*)


(* ::Section::Closed:: *)
(*Time delays*)


(* ::Subsection::Closed:: *)
(*Basic delay differential equations*)


(* ::Input:: *)
(*(* simple first order diff eq with delay, x^\[Prime](t)\[LongEqual]-k x(t-\[Delta]), \[Delta] = 0 => \[Lambda] = -k *)*)
(*SetupDDE[]:=Module[{},*)
(*ssm=StateSpaceModel[{x'[t]+ k x[t-\[Delta]]==k u[t]},{x[t]},{u[t]},{x[t]},t];*)
(*tf=Simplify[TransferFunctionModel[ssm]];*)
(**)
(*(* approximation of delay system, use 5th order which should be accurate *)*)
(*ssmA=SystemsModelDelayApproximate[ssm,5];*)
(*tfA=TransferFunctionModel[ssmA];*)
(*];*)


(* ::Subsubsection::Closed:: *)
(*Variable \[Delta], fixed k*)


(* ::Input:: *)
(*SetupDDE[];*)
(*k=5;*)
(*\[Delta]\[Delta]={0,0.25,0.33};*)
(*eig=Table[0,Length[\[Delta]\[Delta]]];*)
(*eigA=Table[0,Length[\[Delta]\[Delta]]];*)
(*tfT=Table[Null,Length[\[Delta]\[Delta]]];*)
(*(* check max eigenvalue for exact and approximate systems *)*)
(*For[i=1,i<=Length[\[Delta]\[Delta]],i++,*)
(*\[Delta]=\[Delta]\[Delta][[i]];*)
(*tfT[[i]]=tf;*)
(*den=FullSimplify[Denominator[tf[s]]];*)
(*denA=If[\[Delta]==0,{{k+s}},FullSimplify[Denominator[tfA[s]]]];*)
(*(* Solution of exact tf cannot find all eigenvalues, because may be infinite number *)*)
(*eig[[i]]=Max[Re[s/.(Quiet[NSolve[den[[1,1]]==0,s]])]];*)
(*eigA[[i]]=Max[Re[s/.(NSolve[denA[[1,1]]==0,s])]];*)
(*];*)
(*Print["Time delay values  = ",\[Delta]\[Delta]];*)
(*Print["Exact eigenvalues  = ",eig];*)
(*Print["Approx eigenvalues = ",eigA,"\n"];*)
(**)
(*plT=Table[Null,3];*)
(*plT[[1]]=PlotTFstep[tfT,6,{Frame->True,Axes->False}];*)
(*plT[[2]]=BodePlot[tfT,PlotLayout->"Magnitude"];*)
(*plT[[3]]=BodePlot[tfT,PlotLayout->"Phase",PlotRange->All];*)
(*goutT=GraphicsRow[plT,ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text[Style["(a)",FontSize->10]],ImageScaled[{0.055,0.86}]],Inset[Text[Style["(b)",FontSize->10]],ImageScaled[{0.398,0.86}]],*)
(*Inset[Text[Style["(c)",FontSize->10]],ImageScaled[{0.743,0.86}]]*)
(*}]*)
(**)
(*Export[gdir<>"delayDiff.pdf",goutT];*)
(*Clear[k,\[Delta],\[Delta]\[Delta],tfT,tf,tfA,ssm,ssmA,den,denA,eig,eigA,plT,goutT];*)


(* ::Subsubsection::Closed:: *)
(*Fixed \[Delta], variable k*)


(* ::Input:: *)
(*SetupDDE[];*)
(*kk={3,4,5};*)
(*\[Delta]=0.33;*)
(*eig=Table[0,Length[kk]];*)
(*eigA=Table[0,Length[kk]];*)
(*tfB=Table[Null,Length[kk]];*)
(*(* check max eigenvalue for exact and approximate systems *)*)
(*For[i=1,i<=Length[kk],i++,*)
(*k=kk[[i]];*)
(*tfB[[i]]=tf;*)
(*den=FullSimplify[Denominator[tf[s]]];*)
(*denA=If[\[Delta]==0,{{k+s}},FullSimplify[Denominator[tfA[s]]]];*)
(*(* Solution of exact tf cannot find all eigenvalues, because may be infinite number *)*)
(*eig[[i]]=Max[Re[s/.(Quiet[NSolve[den[[1,1]]==0,s]])]];*)
(*eigA[[i]]=Max[Re[s/.(NSolve[denA[[1,1]]==0,s])]];*)
(*];*)
(*Print["k values  = ",kk];*)
(*Print["Exact eigenvalues  = ",eig];*)
(*Print["Approx eigenvalues = ",eigA,"\n"];*)
(**)
(*plb=Table[Null,3];*)
(*plb[[1]]=PlotTFstep[tfB,6,{Frame->True,Axes->False}];*)
(*plb[[2]]=BodePlot[tfB,PlotLayout->"Magnitude"];*)
(*plb[[3]]=BodePlot[tfB,PlotLayout->"Phase",PlotRange->All];*)
(*gout=GraphicsRow[plb,ImageSize->Large,*)
(*Epilog->{*)
(*Inset[Text[Style["(a)",FontSize->10]],ImageScaled[{0.055,0.86}]],Inset[Text[Style["(b)",FontSize->10]],ImageScaled[{0.398,0.86}]],*)
(*Inset[Text[Style["(c)",FontSize->10]],ImageScaled[{0.743,0.86}]]*)
(*}]*)
(**)
(*(*Export[gdir<>"delayDiff.pdf",gout];*)*)
(*Clear[k,kk,\[Delta],tfB,tf,tfA,ssm,ssmA,den,denA,eig,eigA,plb,gout];*)


(* ::Subsubsection::Closed:: *)
(*Find curve of instability transition*)


(* ::Input:: *)
(*SetupDDE[];*)
(*MaxE[denA_,kk_,\[Delta]\[Delta]_]:=Max[Re[s/.(NSolve[(denA[[1,1]]/.k->kk/.\[Delta]->\[Delta]\[Delta])==0,s])]]*)
(*denA=Denominator[tfA[s]];*)
(*kmin=FindMinimum[{Abs[MaxE[denA,k,0.33]],k>=1},{k,20}]*)
(*\[Delta]\[Delta]=Table[i,{i,0.2,1,.05}];*)
(*kval=Table[0,Length[\[Delta]\[Delta]]];*)
(*For[i=1,i<=Length[\[Delta]\[Delta]],i++,*)
(*kmin=FindMinimum[{Abs[MaxE[denA,k,\[Delta]\[Delta][[i]]]],k>=1},k];*)
(*kval[[i]]=k/.kmin[[2,1]];*)
(*(*Print["k = ", kval[[i]],"; \[Delta] = ", \[Delta]\[Delta][[i]]];*)*)
(*];*)
(*xy=Table[{kval[[i]],\[Delta]\[Delta][[i]]},{i,Length[\[Delta]\[Delta]]}];*)
(*gout=ListLinePlot[xy,InterpolationOrder->2,Frame->True,ImageSize->Small,FrameLabel->{Gain k,"Feedback lag \[Delta]",None,None},LabelStyle->Directive[FontSize->11],FrameTicksStyle->Directive[FontSize->9],PlotRange->{{Automatic,8.15},Automatic}]*)
(**)
(*Export[gdir<>"delayDifftrans.pdf",gout];*)
(**)
(*Clear[k,\[Delta],\[Delta]\[Delta],tf,tfA,ssm,ssmA,denA,kval,kmin,xy,\[Delta]\[Delta],gout];*)


(* ::Subsubsection::Closed:: *)
(*Max eigenvalue as function of \[Delta] for fixed k*)


(* ::Input:: *)
(*SetupDDE[];*)
(*MaxE[denA_,kk_,\[Delta]\[Delta]_]:=Max[Re[s/.(NSolve[(denA[[1,1]]/.k->kk/.\[Delta]->\[Delta]\[Delta])==0,s])]]*)
(*denA=Denominator[tfA[s]];*)
(*k=5;*)
(*\[Delta]\[Delta]=Table[i,{i,0.2,20,.05}];*)
(*max\[Delta]=Table[0,Length[\[Delta]\[Delta]]];*)
(*For[i=1,i<=Length[\[Delta]\[Delta]],i++,*)
(*max\[Delta][[i]]=MaxE[denA,k,\[Delta]\[Delta][[i]]];*)
(*];*)
(*xy=Table[{\[Delta]\[Delta][[i]],max\[Delta][[i]]},{i,Length[\[Delta]\[Delta]]}];*)
(*ListLinePlot[xy,InterpolationOrder->2,PlotRange->All]*)
(*Clear[k,kk,\[Delta],\[Delta]\[Delta],tf,tfA,ssm,ssmA,denA,xy,max\[Delta]];*)


(* ::Subsubsection::Closed:: *)
(*Notes on process delay*)


(* ::Input:: *)
(*(* simple first order diff eq with delay, x^\[Prime](t)\[LongEqual]-k x(t-\[Delta])+k u[t-\[Delta]], \[Delta] = 0 => \[Lambda] = -k *)*)
(*SetupDDE2[]:=Module[{},*)
(*ssm=StateSpaceModel[{x'[t]+ k x[t-\[Delta]]==k u[t-\[Delta]]},{x[t]},{u[t]},{x[t]},t];*)
(*tf=Simplify[TransferFunctionModel[ssm]];*)
(**)
(*(* approximation of delay system, use 5th order which should be accurate *)*)
(*ssmA=SystemsModelDelayApproximate[ssm,5];*)
(*tfA=TransferFunctionModel[ssmA];*)
(*];*)
(**)
(*SetupDDE2[]*)
(*tf*)
(*ssm*)
(**)
(*Max[Re[s/.(NSolve[(Denominator[tf[s]][[1,1]]/.k->1/.\[Delta]->.9)==0,s])]]*)
(*Max[Re[s/.(NSolve[(Denominator[tfA[s]][[1,1]]/.k->1/.\[Delta]->.9)==0,s])]]*)
(**)
(*Plot[Max[Re[s/.(NSolve[(Denominator[tfA[s]][[1,1]]/.k->1/.\[Delta]->\[Delta]\[Delta])==0,s])]],{\[Delta]\[Delta],0.001,5}]*)
(*Clear[ssm,tf,tfA,ssmA];*)


(* ::Subsection::Closed:: *)
(*Model predictive control*)


(* ::Subsubsection::Closed:: *)
(*Optimal inputs to a process to match given reference trajectory [not used in text]*)


(* ::Text:: *)
(*This section has some sample code that might be useful elsewhere. Uses numerical minimization to obtain sequence of inputs to drive a process with time delays to match a reference input*)


(* ::Input:: *)
(*f[t_,tmax_]:=-(t-tmax/2)^2+(tmax/2)^2;*)
(*UU[U_,i_]:=U[[i+1]];*)
(*g[U_,p_,q_,\[Tau]_,tmax_]:=(x[t]/.NDSolve[x''[t]+p x'[t-\[Tau]]+q x[t-\[Tau]]==Sum[UU[U,i]*DiracDelta[t-i],{i,0,tmax-1}]&&x[t/;t<-1]==0&&x'[t/;t<-1]==0,{x[t],x'[t]},{t,-1,tmax},Method->{"TimeIntegration"->"ExplicitRungeKutta"}])[[1]];*)
(*quad[U_/;VectorQ[U,NumericQ],p_,q_,\[Tau]_,tmax_]:=Module[{out},*)
(*out=NIntegrate[(f[t,tmax]-g[U,p,q,\[Tau],tmax])^2,{t,0,tmax}];*)
(*If[out<minJ,*)
(*minJ=out;*)
(*Print["J = ", out, "; ", U, "; step = ", step];*)
(*];*)
(*step++;*)
(*Return[out]*)
(*];*)
(**)
(*p=-0.5;*)
(*q=1;*)
(*\[Tau]=0;*)
(*tmax=4;*)
(*step=1;*)
(*minJ=10^8;*)
(**)
(*U=Table[Subscript[u, i],{i,0,tmax-1}];*)
(*solU=Quiet[NMinimize[{quad[U,p,q,\[Tau],tmax],Sum[Abs[U[[i]]],{i,1,tmax}]<10*Length[U]},U,Method->"NelderMead",AccuracyGoal->4,PrecisionGoal->4]]*)
(*solD=NDSolve[x''[t]+p x'[t-\[Tau]]+q x[t-\[Tau]]==Sum[UU[U/.solU[[2]],i]*DiracDelta[t-i],{i,0,tmax-1}]&&x[t/;t<-1]==0&&x'[t/;t<-1]==0,{x[t],x'[t]},{t,-0.5,tmax},Method->{"TimeIntegration"->"ExplicitRungeKutta"}];*)
(*Plot[Evaluate[{x[t],x'[t],f[t,tmax]}/.solD],{t,-.5,tmax}]*)
(*Clear[p,q,\[Tau],tmax,g,quad,U,solU,solD,step,minJ]*)


(* ::Title::Closed:: *)
(*Notes and Miscellany*)


(* ::Text:: *)
(*Haphazard bits of code. No order or particular goal. Some tests here may be useful for learning about the code or experimenting with examples.*)


(* ::Section::Closed:: *)
(*Open loop control vs feedback [old version]*)


(* ::Subsection:: *)
(*Tests*)


(* ::Input:: *)
(*tf=TF[(6)/((s-0.2)(s+2))];*)
(*tfc=TF[(tf[s]/(1+tf[s]))[[1,1]]];*)
(*PlotTFsin[tf,8\[Pi],30]*)
(*BodePlot[tf,ImageSize->Medium,StabilityMargins->True]*)
(*PlotTFsin[tfc,8\[Pi],30]*)
(*BodePlot[tfc,ImageSize->Medium,StabilityMargins->True]*)
(*Clear[tf,tfc]*)


(* ::Input:: *)
(*tf=TF[((5(s^2+2))/(s(s^2+4)))];*)
(*tfc=TFloop[tf]*)
(*PlotTF[tf,20]*)
(*PlotTFsin[tf,1.0,10]*)
(*PlotTFsin[tfc,100,10]*)


(* ::Subsection:: *)
(*Stable open loop: Pole cancellation vs Bode shaping vs feedback*)


(* ::Input:: *)
(*tfSin[w_,s_]:=TF[w/(s^2+w^2)];*)


(* ::Input:: *)
(*P=FS[tfSin[w0,s]];*)
(*R=FS[TF[tfSin[we,s][s]+tfSin[wn,s][s]]];*)
(*Y=FS[tfSin[we,s]];*)
(*K=FS[TF[Y[s]/(P[s]R[s])]];*)
(*Print["K = ", K, ";  Match to Y is ", Y==FS[TF[P[s]K[s]R[s]]]]*)


(* ::Input:: *)
(**)
(*(* daily cycle, houring noise, weekly signal*)*)
(*param={w0->1,wn->24,we->1/7};*)
(**)
(*Kh=K/.param ;*)
(*Ph=P/.param;*)
(*Ph2=tfSin[2,s];*)
(*lowp=TF[(1)/(s+1)];*)
(*gr=Table[Null,{i,3},{j,3}];*)
(*gr[[1,1]]=BodePlot[Kh,PlotLayout->"Magnitude"];*)
(*gr[[1,2]]=BodePlot[TF[Ph[s]Kh[s]],PlotLayout->"Magnitude"];*)
(*gr[[1,3]]=BodePlot[TF[Ph2[s]Kh[s]],PlotLayout->"Magnitude"];*)
(*gr[[2,1]]=BodePlot[lowp,PlotLayout->"Magnitude"];*)
(*gr[[2,2]]=BodePlot[TF[Ph[s]lowp[s]],PlotLayout->"Magnitude"];*)
(*gr[[2,3]]=BodePlot[TF[Ph2[s]lowp[s]],PlotLayout->"Magnitude"];*)
(*gr[[3,1]]=BodePlot[lowp,PlotLayout->"Phase"];*)
(*gr[[3,2]]=BodePlot[TF[Ph[s]lowp[s]],PlotLayout->"Phase"];*)
(*gr[[3,3]]=BodePlot[TF[Ph2[s]lowp[s]],PlotLayout->"Phase"];*)
(*gout=GraphicsGrid[gr,Spacings->{0,50},ImageSize->Large]*)
(*Export[gdir<>"openCancel.pdf",gout];*)


(* ::Input:: *)
(*(* on design *)*)
(*tmax= 2\[Pi] 7/we/.param;*)
(*input=Sin[we t]+Sin[wn t]/.param; *)
(*yout=OutputResponse[TF[Ph[s]lowp[s]],input,{t,0,tmax}];*)
(*Plot[{yout,Sin[we t]/.param},{t,0,tmax},PlotRange->All]*)
(**)
(*(* internal clock freq doubled *)*)
(*tmax= 2\[Pi] 7/we/.param;*)
(*input=Sin[we t]+Sin[wn t]/.param; *)
(*yout=OutputResponse[TF[Ph2[s]lowp[s]],input,{t,0,tmax}];*)
(*Plot[{yout,Sin[we t]/.param},{t,0,tmax},PlotRange->All]*)
(**)
(*(* environment freq quadrupled *)*)
(*tmax= 2\[Pi] 7/(4we)/.param;*)
(*input=Sin[4we t]+Sin[wn t]/.param; *)
(*yout=OutputResponse[TF[Ph[s]lowp[s]],input,{t,0,tmax}];*)
(*Plot[{yout,Sin[4we t]/.param},{t,0,tmax},PlotRange->All]*)


(* ::Input:: *)
(*k=1;*)
(*K=FS[Rationalize[N[PIDTune[Ph,"PID"]]]]*)
(*K2=FS[TF[(0.85/1.7+s(2.4/1.7+s))/(2s)]];*)
(*L=FS[TF[k*Ph[s]K[s]]];*)
(*G=TFloop[L];*)
(*Max[Re[s/.NSolve[Denominator[G[s]][[1,1]]==0,s]]]*)
(*BodePlot[{L,G}]*)
(*PlotTFsin[G,.14,7]*)
(*PlotTFsin[G,1,7]*)
(*PlotTFsin[G,2,7]*)
(*PlotTFsin[G,10,7]*)
(*PlotTFsin[G,20,7]*)
(*NyquistPlot[{L}];*)
(*Clear[k];*)



