Control Systems in Mathematica 8

Wolfram Research, Inc.

System Models

Transfer-function model:

TransferFunctionModel[(1/(s^2)), s]

State - space model:

StateSpaceModel[%]

Inverted pendulum

eqns={Fx==m xc''[t],Fy-m g==m yc''[t],fx[t]-Fx==M X''[t]};

xc[t_]=X[t]+(1/2) L Sin[θ[t]];yc[t_]=(1/2) L Cos[θ[t]];J=((m (L^2))/12);

Solve[eqns,{Fx,Fy,X''[t]}]

(1/2) Fy L Sin[θ[t]]-(1/2) Fx L Cos[θ[t]]==J θ''[t];

StateSpaceModel[%/.%%,{θ[t],θ'[t]},fx[t],θ[t],t]

(Subscript[(({{0, 1, 0}, {((6 γ (m+M))/(L (m+4 M))), 0, -(6/(L (m+4 M)))}, {1, 0, 0}})), ""]^("𝒮"))

MatrixForm/@Normal[%]

Constructing from matrices

({{(OverDot[x])(t)=Ax(t)+Bu(t)}, {y(t)=Cx(t)+Du(t)}})

StateSpaceModel[{(({{0, 1}, {0, -1}})),(({{0}, {1}})),(({{1, 0}}))}]

SIS0, MIMO, MISO, SIMO...

TransferFunctionModel[{{(1/(s+a)),(1/(s+b))}},s]

StateSpaceModel[%]

TransferFunctionModel[%]//Simplify

The polynomial variable is optional.

Frequency response

%[1+I]

%%[I Range[10]ω]

State and output responses

Symbolic:

ss0=StateSpaceModel[(Subscript[(({{(1/(10+2 s+(s^2)))}})), ""]^("𝒯"))]

StateResponse[%,UnitStep[t],t]//Simplify

Plot[%,{t,0,10}]

StateResponse[{(Subscript[(({{0, 1, 0}, {-10, -2, 1}, {1, 0, 0}})), ""]^("𝒮")),{.5,-.5}},UnitStep[t],t];

Plot[%,{t,0,5},PlotRange->All]

OutputResponse[ss0,DiracDelta[t],t]

Plot[%,{t,0,10},PlotRange->All]

... and numeric:

OutputResponse[(Subscript[(({{(1/(10+2 s+(s^2)))}})), ""]^("𝒯")),t,{t,2}]

Plot[%,{t,0,2}]

Model manipulation

Transfer-function model

tf = TransferFunctionModel[((s+c)/((s+a)(s+b))), s]

TransferFunctionExpand[%]

TransferFunctionFactor[%]

TransferFunctionPoles[%]

TransferFunctionZeros[%%]

tf/.c->a

Simplify[%]

tf/.{c->999/1000,a->1}

TransferFunctionCancel[%]

TransferFunctionCancel[%,Tolerance->.01]

State-space model

ss=StateSpaceModel[tf]

StateSpaceModel[tf,StateSpaceRealization->"Observable"]

StateSpaceTransform[%,{{1,0},{0,-1}}]

TransferFunctionModel/@{%,%%,%%%}

Decompositions

JordanModelDecomposition[ss]

MinimalStateSpaceModel[ss]

MinimalStateSpaceModel[ss/.c->a]

Simplify[%,a>0]

ss1=ss/.c->a

ControllableDecomposition[ss1]

ControllableModelQ[ss1]

ObservableModelQ[ss1]

{(Subscript[s, o]),(Subscript[ss, o])}=Simplify[ObservableDecomposition[ss1],a>0]

StateSpaceTransform[ss1,{(Subscript[s, o]),Transpose[(Subscript[s, o])]}]//Simplify[#,a>0]&

ss2=StateSpaceModel[{(({{0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {-50, -79, -33, -5}})),(({{0}, {0}, {0}, {1}})),(({{50.5, 15.25, 1.5, 0}}))}]

{(Subscript[s, b]),(Subscript[ss, b])}=InternallyBalancedDecomposition[ss2]

MatrixForm/@{ControllabilityGramian[#],ObservabilityGramian[#]}&[(Subscript[ss, b])]//Chop

The underlying solvers are also available (LyapunovSolve, DiscreteLyapunovSolve).

Model Conversions

TransferFunctionModel[(a/(s+a)),s]

ToDiscreteTimeModel[%,τ,z]

ToContinuousTimeModel[%,s]

Continuous-time state-space systems:

({{(OverDot[x])(t)=Ax(t)+Bu(t)}, {y(t)=Cx(t)+Du(t)}})

and discrete-time state-space systems:

({{x(k+1)=Ax(k)+Bu(k)}, {y(k)=Cx(k)+Du(k)}})

StateSpaceModel[%]

Grid[Table[{m,ToDiscreteTimeModel[%,τ,Method->m]//Simplify},{m,{"ForwardRectangularRule","BackwardRectangularRule","BilinearTransform","ZeroOrderHold","FirstOrderHold"}}],Background->{None,{{White,LightGray}}},Frame->All]

Connections

Series, parallel, feedback,state feedback...

SystemsModelSeriesConnect[TransferFunctionModel[(1/(s-a)),s],TransferFunctionModel[((s-a)/(s+b)),s]]

Simplify[%]

SystemsModelSeriesConnect[TransferFunctionModel[(1/(s-a)),s]//StateSpaceModel,TransferFunctionModel[((s-a)/(s+b)),s]//StateSpaceModel]

Control Design

Modern and classical.

Pole placement, Ackermann

ss3=StateSpaceModel[{(({{1, 1, -2}, {0, 1, 1}, {0, 0, 1}})),(({{1}, {0}, {1}}))}]

kk=StateFeedbackGains[%,{(Subscript[p, 1]),(Subscript[p, 2]),(Subscript[p, 3])}]

SystemsModelStateFeedbackConnect[ss3,kk]

Eigenvalues[First[Normal[%]]]

Robust pole placement (Kautsky-Nichols-Van Dooren)

aircraft=StateSpaceModel[{{{-10,0,-10,0},{0,-0.7,9,0},{0,-1,-0.7,0},{1,0,0,0}},{{20,2.8},{0,-3.13},{0,0},{0,0}}}]

StateFeedbackGains[aircraft,{-10,-8,-5,-2}]

Root locus plot

A feedback system

sys=TransferFunctionModel[k(( 297.25+186 s+59.25 (s^2)+9 (s^3)+(s^4))/(11.25+34.5 s+32.75 (s^2)+21 (s^3)+7.5 (s^4)+(s^5))),s];

RootLocusPlot[sys,{k,0,10}]

TransferFunctionFactor[sys]//Quiet

RootLocusPlot[sys,{k,0,10},PoleZeroMarkers->{"ParameterValues"->{8}}]

Manipulate[RootLocusPlot[sys,{k,0,10},PoleZeroMarkers->{"ParameterValues"->{kVal}}],{{kVal,5,"k"},0,10},ContinuousAction->False]

A lead compensator

RootLocusPlot[((s+z)/(s+z/0.5))((10 )/(s(s+2))),{z,0,5}]

RootLocusPlot[((s+3)/(s+3/α))((10 )/(s(s+2))),{α,0.01,1}]

The first example revisited

sys=TransferFunctionModel[k(( 297.25+186 s+59.25 (s^2)+9 (s^3)+(s^4))/(11.25+34.5 s+32.75 (s^2)+21 (s^3)+7.5 (s^4)+(s^5))),s];

closedLoopSystem=SystemsModelFeedbackConnect[sys]

RootLocusPlot[closedLoopSystem,{k,0,10},FeedbackType->None]

Nyquist plot

Nyquist contours

A continuous-time system

NyquistPlot[tf1=TransferFunctionModel[((10 s)/(10s+1)),s]]

Nyquist’s stability criterion

Z = N + P

number of unstable closed-loop poles = number of cw encirclements of -1. +  number of unstable open-loop poles

Z=0+0

TransferFunctionPoles[SystemsModelFeedbackConnect[tf1]]

NyquistPlot[tf2=TransferFunctionModel[(z/((z+1.2)(z+0.4))),z,SamplingPeriod->1],PlotRange->{{-2,9},All}]

Z=0+1

TransferFunctionPoles[SystemsModelFeedbackConnect[tf2]]

NyquistPlot[tf3=TransferFunctionModel[((s+6)/((s^2)+4)),s]]

Z=0+0

TransferFunctionPoles[SystemsModelFeedbackConnect[tf3]]//N

M and N circles

((X + I Y)/(1 + f (X + I Y) )) == M (E^(I N))

NyquistMCircle[M_/;M==-1, f_, X_, Y_]:= X == f (1/2)
NyquistMCircle[M_, f_, X_, Y_]:= (((X + (((M^2)(f^2))/((M^(2 ))- 1))))^2) + (Y^2) == ((M^2)/((((M^2) - 1))^2))

NyquistNCircle[N_/;Tan[N]==0, f_, X_, Y_]:= Y == 0
NyquistNCircle[N_, f_, X_, Y_]:= (((X + (f/2)))^2) + (((Y - (f/(2 Tan[N]))))^2) == (1/4) + (1/(4 ((Tan[N])^2)))

tf=TransferFunctionModel[(100/((s^2)+2 s +100)),s];

NyquistPlot[tf,NyquistGridLines->Automatic,PlotRange->5,AspectRatio->1,Mesh->{{8}},MeshStyle->Directive[PointSize[Medium],Red]]

Manipulate[NyquistPlot[tf,NyquistGridLines->{M,None},Mesh->{{f}},MeshStyle->Directive[PointSize[Medium],Red],AspectRatio->1],{M,1.3,5},{f,10,20},ContinuousAction->False]

Nichols plot

Example

NicholsPlot[((100(2 s+1))/((s+1)((s^2)+2s+7))),Frame->True,FrameLabel->{"Phase (degrees)","Mag (dB)"}]

Nichols Chart

((Y (E^(I X)))/(1 + f Y (E^(I X)))) == M (E^(I N))

NicholsMContour[M_/;M==-1, f_, X_, Y_]:= (M^2)(1 + (Y^2) + 2 f Y Cos[X]) == (Y^2)

NicholsNContour[N_/;Tan[N]==0, f_, X_, Y_]:= Sin[X] ==0
NicholsNContour[N_, f_, X_, Y_]:= Y + Cos[X] == ((Sin[X])/(Tan[N]))

NicholsPlot[(6.5/(s(s+1)(3s+1))),{0.05,10},NicholsGridLines->Automatic,Mesh->{Table[10^ii,{ii,Log10@0.05,Log10@10,(Log10@10-Log10@0.05)/(10-1)}]},MeshStyle->Directive[Red,PointSize[Large]],Frame->True,FrameLabel->{"Phase (Degrees)","Magnitude (dB)"}]

Bode and singular value plot

Bode plot

BodePlot[TransferFunctionModel[(4/((s^2)+2 0.05 2 s+4)),s],ImageSize->Medium]

Singular value plot

SingularValuePlot[TransferFunctionModel[{{((2 s)/((s^2)+0.7)),(1/(s+4)),(1/s)},{(2/(s+1)),((s-1 )/(s+2)),(s/((s^2)+5))},{(1/(s+3)),(2/(s+4)),s}},s],Frame->True]

Relative stability

StabilityMargins option

tf=TransferFunctionModel[(10/((s+0.1)(s+1.2)(s+4))),s];

NyquistPlot[tf,StabilityMargins->True]

NyquistPlot[tf,PlotRange->1,StabilityMargins->True,StabilityMarginsStyle->{Directive[Thick, Green],Blend[{Red,Blue}]},Epilog->{Dashed,Circle[]}]

BodePlot[tf,StabilityMargins->True,ImageSize->Medium]

NicholsPlot[tf,StabilityMargins->True]

Gain and phase margins

GainPhaseMargins[tf]

Optimal Control

Infinite horizon LQR

Compute feedback control that regulates initial state perturbations: Minimize J=(∫_0^∞)((Transpose[x])(t)Q x(t)+(Transpose[u])(t) R u(t))dt , subject to :  x'(t)=A x(t)+B u(t)

LQRegulatorGains[(Subscript[(({{-8.2, -6.1, 0, -2.4, 1, -1}, {-6.1, -14.7, -1, -6.3, 0, 1}, {0, -1, -26.04, -2, -1, 1}, {-2.92, -6.5, -2.08, -20, 0, 0}, {1, 0, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}})), ""]^("𝒮")),{(({{10, 0, 0, 0}, {0, 8, 0, 0}, {0, 0, 2, 0}, {0, 0, 0, 0}})),(({{1, 0}, {0, 1}}))}]//MatrixForm

Flatten[StateResponse[{SystemsModelStateFeedbackConnect[(Subscript[(({{-8.2, -6.1, 0, -2.4, 1, -1}, {-6.1, -14.7, -1, -6.3, 0, 1}, {0, -1, -26.04, -2, -1, 1}, {-2.92, -6.5, -2.08, -20, 0, 0}, {1, 0, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}})), ""]^("𝒮")),%],{1,-1,0.5,-0.4}},0,{t,0,2}]][[1;;4]];
Plot[%,{t,0,1},PlotRange-> All,Frame-> True, FrameLabel-> {"Time","States"}]

Optimal Control

Infinite horizon LQR

Cross-product term in cost penalizes “power” into a system:

J=(∫_0^∞)((Transpose[x])(t)Q x(t)+(Transpose[u])(t)R u(t)+2 (Transpose[x])(t)P(t)u(t))dt

LQRegulatorGains[(Subscript[(({{0, 1, 0, 0, 0}, {0, -0.01, 0.3, 0, -1}, {0, -0.003, -10, 0.1, 0}, {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}})), ""]^("𝒮")),{(({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}})),(({{1, 0}, {0, 1}})),(({{0.1, 0}, {-0.2, 0}, {0.15, 0.2}}))}]//MatrixForm

LQ gain computation is achieved using RiccatiSolve[{a,b},{q,r,p}] or DiscreteRiccatiSolve[{a,b},{q,r,p}]

RiccatiSolve[{(({{-3, 2}, {1, 1}})),(({{0}, {1}}))},{(({{1.0, -1}, {-1, 1}})),(({{3}}))}]//MatrixForm

Optimal Control

Optimal design by emulation

Match the performance of a continuous-time optimal design with a discrete controller

DiscreteLQRegulatorGains[(Subscript[(({{0, 1, 0, 0}, {0, -5, 1, 0.1}, {1, 0, 0, 0}})), ""]^("𝒮")),{(({{100, 0}, {0, 10}})),(({{10, 0}, {0, 1}}))},0.1]//MatrixForm

The State feedback gain matrix is obtained by solving the discrete algebraic Riccati equation.

Advantage: Execute control design in continuous-time and implement with a digital computer in the loop.

Optimal Estimation with a Kalman Filter

Optimal steady-state estimation of the states and outputs in presence of white noise, zero-mean stochastic processes:

x(k+1)=A x(k)+(Subscript[B, d]) (Subscript[u, d])(k)+(Subscript[B, w]) (Subscript[u, w])(k)
(SphericalBesselY[n, k])=(Subscript[C, n]) x(k)+(Subscript[D, n]) u(k)+(SphericalBesselY[v, k])

Best state estimate (OverHat[x])(k), in the least mean-square sense:

(OverHat[x])(k+1)=(A-L (Subscript[C, n])) (OverHat[x])(k)+((Subscript[B, d])-L (Subscript[D, nd])) (Subscript[u, d])(k)+L (SphericalBesselY[n, k])

Estimator model construction from covariance matrices is achieved in a single step with KalmanEstimator[ss,{w,v}]:

antenna=(Subscript[(({{0.5, 0.07869, 0.0042, 0.0104}, {0, -0.60653, 0.0786, 0.00786}, {1, 0, 0, 0}})), 0.1]^("𝒮"));
w={{0.01}};v={{0.001}};

u=Table[Sin[((2 π i)/20.0)],{i,100}];processNoise= Table[RandomReal[NormalDistribution[0,Sqrt[w[[1,1]]]]],{100}];
measurementNoise=Table[RandomReal[NormalDistribution[0,Sqrt[v[[1,1]]]]],{100}];
y=Flatten[OutputResponse[antenna,{u,processNoise}]]+measurementNoise;
ListLinePlot[y,ImageSize-> 200, PlotLabel->"Sensor Output"]

kalmanFilter=SystemsModelExtract[KalmanEstimator[{antenna,All,{1}},{w,v}],All,{1}];ListLinePlot[OutputResponse[kalmanFilter,{u,y}],ImageSize-> 200, PlotLabel->"Filter Output"]

LQG Problem

Optimize the performance measure for a stochastic system:

J = (lim_(T->∞)) (1/T)(EllipticE[(∫_0^T)((Transpose[x])(t)Q x(t) + (Transpose[u])(t)R u(t))dt])

Solved by combining the solutions of deterministic LQR and Kalman filtering problems

LQGRegulator[{ss, sensors, finputs},{w,v,h},{q,r,p}]  assembles optimal regulator model from covariance and weighting matrices based on the separation principle:

LQGRegulator[{StateSpaceModel[{(({{2, 0, -1}, {0, -2, 1}, {1, -1, 1}})),(({{1, 1}, {0, 1}, {1, -1}})),(({{1, -1, 0}, {0, 1, -1}}))}],1,1},{(({{1}})),(({{0.01}}))},{(({{0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1}})),(({{10}}))}]

Other Statespace Design Functions

LQOutputRegulatorGains[ss, {q,r}] : Output-weighted LQ state-feedback

LQEstimatorGains[ss, {w, v}] : Kalman gain computation

DiscreteLQEstimatorGains[ss, {w, v}, τ] :  Optimal estimation using discrete equivalents of noise matrices

StateOutputEstimator[ss, l] : Statespace model of a generic state estimator with pre - computed gain matrix

EstimatorRegulator[ss, {l, k}] : Statespace model of a generic regulator based on separation property