Undamped noisy oscillator (Problem 8.7)

• revised, Oct. 25, 2022:
  – use prediction estimator to conform to numbers given in text (and is simpler)
  – compare explicit construction with LQGRegulator command for closed loop
  – miscellaneous code cleanup

Define the (discretized) system, with noisy inputs

Clear["Global`*"];
Ts=0.1;  Tend = 15; Nt =Round[Tend/Ts];  (* # time steps to simulate *)

G1ss=StateSpaceModel[{x''[t]+x[t]==u[t]+w[t]},{{x[t],0},{x'[t],0}},{{u[t],0},{w[t],0}},{x[t]},t,SystemsModelLabels->{{"u","w"},{"y"},{"x","(Overscript[x, •])"}}] (* undamped oscillator; separate inputs for feedback and process noise *);
G1dss = ToDiscreteTimeModel[G1ss,Ts,z,Method -> "ZeroOrderHold"];
zeroIn= Table[0,{i,Nt}] (* list of zero inputs *);
ν=0.3; ξ=0.3         (* process & measurement noise *);
SeedRandom[2];    (* for reproducible plots *) 
processNoise=RandomReal[NormalDistribution[0,ν],Nt];
measurementNoise=RandomReal[NormalDistribution[0,ξ],Nt];
{G1ss,G1dss}

Open loop disturbance response, using Kalman filter (prediction observer)

kalEst=KalmanEstimator[{G1dss,1,2},{(({{(ν^2)}})),(({{(ξ^2)}}))},Method->"PredictionEstimator"];
ym=Flatten[OutputResponse[{G1dss,{0,1}},{zeroIn,processNoise}]]+measurementNoise         (* noisy position measurement *);
{xs,vs}=StateResponse[{G1dss,{0,1}},{zeroIn,processNoise}] (* physical states *);
{xe,ve,ye}=OutputResponse[kalEst,{zeroIn,ym}];  (* Kalman estimate *)

dr=DataRange->{0,Tend}; lRed=Lighter[Red,0.7];plotmax=1.5;plot0a=ListPlot[{ym,xe,xs},dr,Joined->True,InterpolationOrder->0,PlotStyle ->{lRed,Black,Blue},PlotRange->{-plotmax,plotmax}];

LQG controller for disturbance response  (explicit construction)

q=(({{1, 0}, {0, 1}})); r={{0.1}} (* weight matrices for state, input *);k=LQRegulatorGains[{G1dss,1},{q,r}]

l=LQEstimatorGains[{G1dss,All,1},{(({{(ν^2)}})),(({{(ξ^2)}}))},Method->"PredictionEstimator"]

{a,b,c,d}=Normal[SystemsModelExtract[G1dss,1]];
{Eigenvalues[a],Eigenvalues[a-KroneckerProduct[b,k]]}

a1= ArrayFlatten[(({{a, -b.k}, {l.c,     a-b.k-l.c}}))]  ;b1=ArrayFlatten[(({{b, 0}, {0,   l}}))]  (* inputs = (ν,ξ) *);c1=(({{1, 0, 0, 0}}));
closedLoop2=StateSpaceModel[{a1,b1,c1},SamplingPeriod->Ts];

{xs2,vs2,xe2,ve2}=StateResponse[{closedLoop2,{0,1,0,0}},{processNoise,measurementNoise}];
ym2=xs2+measurementNoise;
pclosed2=ListPlot[{ym2,xe2,xs2},dr,Joined->True,InterpolationOrder->0,PlotStyle ->{lRed,Black,Blue},PlotRange->{-plotmax,plotmax}];
pclosed3=ListPlot[{xs-xe,xs2-xe2,xs-xe-xs2+xe2 },dr,Joined->True,InterpolationOrder->0,PlotRange->{-0.3,0.6}];

Graphs

Grid[{{plot0a,pclosed2,pclosed3}},Spacings->{4}]

StandardDeviation/@{Take[xs2,-50],Take[xe2,-50]}  (* sd of last 50 elements *)

Compare the above closed-loop system with the one-step routine LQGRegulator

sspec=<|"InputModel"->G1dss,"FeedbackInputs"-> 1,"MeasuredOutputs"->1,SystemsModelLabels->{{"u","w"},{"y"},{"x","(Overscript[x, •])"}}|>;
closedLoop1=LQGRegulator[sspec,{(({{(ν^2)}})),(({{(ξ^2)}}))},{q,r},"ClosedLoopSystem",Method->"PredictionEstimator"];
{closedLoop1,closedLoop2}

Export data

(* 
SetDirectory[NotebookDirectory[]];
Export["UndampedNoisyOscillator.dat", {ym,xe,xs,ym2,xe2,xs2}\[Transpose]];
*)