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

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

Estimator with GPS

Problem Data

m = 100. (* 100 kg *);

A = {
{0,1},

{0,0}
};
Bw = {
{0},
{(1/m)}
};
Cy={
{1,0},
{0,0}
};
Dyw={
{0},
{0}
};

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

Ww={{1^2}};
Wv={{10^2,0},{0,(10/1000)^2}}; (* 1 mg *)

Filter design using Kalman

filterGPS= KalmanEstimator[linearizedSystem, {Ww, Wv}]

F = -Normal[filterGPS][[2]];
MatrixForm[F]

Error Dynamics

Eigenvalues[A+ F.Cy]

BodePlot[SystemsModelExtract[filterGPS,{1},{1}],GridLines->Automatic]

Analysis of the filter

Augment system with measurement noise input

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

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

Simulate

h=.01;

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

Estimate stationary position

x0 = {0,0};
xhat0 = {0,0};

sysDiscreteGPS = ToDiscreteTimeModel[augmentedSystemGPS,h];
filterDiscreteGPS = ToDiscreteTimeModel[filterGPS,h];

{ySys,tSys,xSys}= {OutputResponse[{sysDiscreteGPS,x0},Transpose[w]],T,StateResponse[{sysDiscreteGPS,x0},Transpose[w]]};

{yFilterGPS,tFilterGPS,xFilterGPS}= {OutputResponse[{filterDiscreteGPS,xhat0},ySys],T,StateResponse[{filterDiscreteGPS,xhat0},ySys]};

plots={{tSys,xSys[[1]]},{tFilterGPS,xFilterGPS[[1]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 1: Position and estimate",Joined->True,PlotStyle->{{Red,Dashed},Blue},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{tSys,xSys[[2]]},{tFilterGPS,xFilterGPS[[2]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 2: Velocity and estimate",Joined->True,PlotStyle->{{Red,Dashed},Blue},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{t,xSys[[1]]-xFilterGPS[[1]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 1: Position error",Joined->True,GridLines->Automatic,PlotStyle->{Blue},PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{t,xSys[[2]]-xFilterGPS[[2]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 2: Velocity error",Joined->True,PlotStyle->{Blue},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

Estimator with Accelerometer

Problem Data

m = 100. (* 100 kg *);

A = {
{0,1},
{0,0}
};
Bw = {
{0},
{(1/m)}
};
CyGPS={
{1,0}
};
DywGPS={
{0}
};

Accelerometer

CyA={{0,1}}.A;
DywA={{0,1}}.Bw;

Cy=ArrayFlatten[{{CyGPS},{CyA}}];
Dyw=ArrayFlatten[{{DywGPS},{DywA}}];

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

Ww={{1^2}};
Wv={{10^2,0},{0,(10/1000)^2}}; (* 1 mg *)

Filter design using Kalman

filterA= KalmanEstimator[linearizedSystem, {Ww, Wv}]

F = -Normal[filterA][[2]];
MatrixForm[F]

Error Dynamics

Eigenvalues[A+ F.Cy]

BodePlot[SystemsModelExtract[filterA,{1,2},{1,2}],GridLines->Automatic]

Analysis of the filter

Augment system with measurement noise input

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

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

Simulate

x0 = {0,0};
xhat0 = {0,0};

sysDiscreteA = ToDiscreteTimeModel[augmentedSystemA,h];
filterDiscreteA = ToDiscreteTimeModel[filterA,h];

{ySys,tSys,xSys}= {OutputResponse[{sysDiscreteA,x0},Transpose[w]],T,StateResponse[{sysDiscreteA,x0},Transpose[w]]};

{yFilterA,tFilterA,xFilterA}= {OutputResponse[{filterDiscreteA,xhat0},ySys],T,StateResponse[{filterDiscreteA,xhat0},ySys]};

plots={{tSys,xSys[[1]]},{tSys,xFilterGPS[[1]]},{tSys,xFilterA[[1]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 1: Position and estimate",Joined->True,PlotStyle->{{Red,Dashed},Blue,Magenta},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{tSys,xSys[[2]]},{tSys,xFilterGPS[[2]]},{tSys,xFilterA[[2]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 2: Velocity and estimate)",Joined->True,PlotStyle->{{Red,Dashed},Blue,Magenta},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{tSys,xSys[[1]]-xFilterGPS[[1]]},{tSys,xSys[[1]]-xFilterA[[1]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 1: Position error",Joined->True,PlotStyle->{Blue,Magenta},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]

plots={{tSys,xSys[[2]]-xFilterGPS[[2]]},{tSys,xSys[[2]]-xFilterA[[2]]}};
plots=Map[Transpose,plots];
ListPlot[plots,PlotLabel->"Figure 2: Velocity error",Joined->True,PlotStyle->{Blue,Magenta},GridLines->Automatic,PerformanceGoal->"Speed",MaxPlotPoints->2000]