MAE 280 B - Linear Control Design
Mauricio de Oliveira
State Estimation

Hyperlink["http://fccr.ucsd.edu/~mauricio/courses/mae280b"]

Problem Data

m = 100. (* 100 kg *);
r = 300. (10^3) (* 300 km *);
R0 = 6.37 (10^6) (* 6.37 10^3 km *);
G = 6.673 (10^(-11)) (* 6.673 N m^2/kg^2 *);
M = 5.98 (10^24) (* 5.98 10^24 kg *);

Gravitational force constant

k = G M;

Angular velocity (rad/s)

ω = Sqrt[k/((R0+r)^3)]

“Ground” velocity (m/s)

v = ω*(R0+r)

Noise variances

Ww = 0.1 IdentityMatrix[2];

Linearized System Realization (radial and tangential position measurement)

A = {
{0,0,1,0},
{0,0,0,1},
{3(ω^2),0,0,2(r+R0)ω},
{0,0,((-2ω)/(r+R0)),0}
};
Bw=Bu = {
{0,0},
{0,0},
{(1/m),0},
{0,(1/(m(r+R0)))}
};
Cy={
{0,r+R0,0,0},
{1,0,0,0}
};

Scaling

linearizedSystem=StateSpaceModel[{A,Bw,Cy}]

T = DiagonalMatrix[{1,r+R0,1,r+R0}]

Apply the similarity transformation

transformedLinearizedSystem=StateSpaceTransform[linearizedSystem,Inverse[T]]

Bwt = T.Bw;

Filter design using dual state feedback

dualSystem = StateSpaceModel[{Transpose[A],Transpose[Cy],Transpose[Bw]}]

transformedDualSystem=StateSpaceTransform[dualSystem,Transpose[T]]

Wv = 0.1IdentityMatrix[2];

Ft=-Transpose[LQRegulatorGains[transformedDualSystem,{Bwt.Ww.Transpose[Bwt],Wv}]];
MatrixForm[Ft]

Filter design using Kalman

filter= KalmanEstimator[transformedLinearizedSystem, {Ww, Wv}]

Ft = -Normal[filter][[2]];
MatrixForm[Ft]

Analysis of the filter

Filter gain in original coordinates :

F=Inverse[Transpose[T]].Ft;
MatrixForm[F]

Error Dynamics

Eigenvalues[A+ F.Cy]

Augment system with measurement noise input

Bwa=ArrayFlatten[{{Bw,ConstantArray[0,{4,2}]}}];
Dywa=ArrayFlatten[{{ConstantArray[0,{2,2}],IdentityMatrix[2]}}];

augmentedSystem=StateSpaceModel[{A,Bwa,Cy,Dywa}]

transformedAugmentedSystem=StateSpaceTransform[augmentedSystem,Inverse[T]]

Wt = ArrayFlatten[{{ArrayFlatten[{{Ww,ConstantArray[0,{2,2}]}}]},
{ArrayFlatten[{{ConstantArray[0,{2,2}],Wv}}]}}];
MatrixForm[Wt]

Connect system and filter

sys = SystemsModelSeriesConnect[transformedAugmentedSystem,filter]

Simulate

h=.01;

Tmax = 200;
T = Table[i,{i,0,Tmax,h}];
w = MapThread[Flatten[{#1,#2}]&,{RandomVariate[NormalDistribution[],{Length[T],2}].MatrixPower[Ww,.5],RandomVariate[NormalDistribution[],{Length[T],2}].MatrixPower[Wv,.5]}];

Estimate stationary position

x0 = {0,0,0,0};
xhat0 = {-6,20,0,0};

sysDiscrete = ToDiscreteTimeModel[sys,h];
{y,t,x}= {OutputResponse[{sysDiscrete,Join[x0,xhat0]},Transpose[w]],T,StateResponse[{sysDiscrete,Join[x0,xhat0]},Transpose[w]]};

plots={{t,x[[1]]},{t,x[[5]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 1: Radial position and estimator (x[1] and x[5])",Joined->True,PlotStyle->{{Red,Dashed},Blue},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{t,x[[2]]},{t,x[[6]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 2: Tangential position and estimate (x[2] and x[6])",Joined->True,PlotStyle->{{Red,Dashed},Blue},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

Linearized System Realization (tangential position and radial velocity measurement)

A = {
{0,0,1,0},
{0,0,0,1},
{3(ω^2),0,0,2(r+R0)ω},
{0,0,((-2ω)/(r+R0)),0}
};
Bw=Bu = {
{0,0},
{0,0},
{(1/m),0},
{0,(1/(m(r+R0)))}
};
Cy={
{0,r+R0,0,0},
{0,0,1,0}
};

Scaling

linearizedSystem=StateSpaceModel[{A,Bw,Cy}]

T = DiagonalMatrix[{1,r+R0,1,r+R0}]

Apply the similarity transformation

transformedLinearizedSystem=StateSpaceTransform[linearizedSystem,Inverse[T]]

Bwt = T.Bw;

Filter design using Kalman

Wv = 0.1IdentityMatrix[2];

filter= KalmanEstimator[transformedLinearizedSystem, {Ww, Wv}]

Ft = -Normal[filter][[2]];
MatrixForm[Ft]

Analysis of the filter

Filter gain in original coordinates :

F=Inverse[Transpose[T]].Ft;
MatrixForm[F]

Error Dynamics

Eigenvalues[A+ F.Cy]

Augment system with measurement noise input

Bwa=ArrayFlatten[{{Bw,ConstantArray[0,{4,2}]}}];
Dywa=ArrayFlatten[{{ConstantArray[0,{2,2}],IdentityMatrix[2]}}];

augmentedSystem=StateSpaceModel[{A,Bwa,Cy,Dywa}]

transformedAugmentedSystem=StateSpaceTransform[augmentedSystem,Inverse[T]]

Wt = ArrayFlatten[{{ArrayFlatten[{{Ww,ConstantArray[0,{2,2}]}}]},
{ArrayFlatten[{{ConstantArray[0,{2,2}],Wv}}]}}];
MatrixForm[Wt]

Connect system and filter

sys = SystemsModelSeriesConnect[transformedAugmentedSystem,filter]

Simulate

h=.01;

Tmax = 200;
T = Table[i,{i,0,Tmax,h}];
w = MapThread[Flatten[{#1,#2}]&,{RandomVariate[NormalDistribution[],{Length[T],2}].MatrixPower[Ww,.5],RandomVariate[NormalDistribution[],{Length[T],2}].MatrixPower[Wv,.5]}];

Estimate stationary position

x0 = {0,0,0,0};
xhat0 = {-6,20,0,0};

sysDiscrete = ToDiscreteTimeModel[sys,h];
{y,t,x}= {OutputResponse[{sysDiscrete,Join[x0,xhat0]},Transpose[w]],T,StateResponse[{sysDiscrete,Join[x0,xhat0]},Transpose[w]]};

plots={{t,x[[1]]},{t,x[[5]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 1: Radial position and estimator (x[1] and x[5])",Joined->True,PlotStyle->{{Red,Dashed},Blue},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{t,x[[2]]},{t,x[[6]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 2: Tangential position and estimate (x[2] and x[6])",Joined->True,PlotStyle->{{Red,Dashed},Blue},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]