%
%   Plate_Est_ApplyNPlateSRP
%
%   Solve for the Cartesian state, applying NPlate SRP.
%
%   10/10/2019 SES Initial version
%

%
%   Spacecraft
%

Create Spacecraft SimSat;

SimSat.DateFormat             = UTCGregorian;
SimSat.Epoch                  = '10 Jun 2012 00:00:00.000';
SimSat.CoordinateSystem       = EarthMJ2000Eq;
SimSat.DisplayStateType       = Cartesian;
SimSat.X                      = -4009.814
SimSat.Y                      = -41239.052
SimSat.Z                      = -7779.138
SimSat.VX                     =  3.038104
SimSat.VY                     = -0.215458
SimSat.VZ                     = -0.423910
SimSat.DryMass                = 10;
SimSat.Cd                     = 2.2;
SimSat.Cr                     = 1.8;
SimSat.DragArea               = 15;
SimSat.SRPArea                = 15;
SimSat.Attitude 			  = NadirPointing;
SimSat.AttitudeReferenceBody  = Sun;
SimSat.AttitudeConstraintType = 'OrbitNormal';
SimSat.BodyAlignmentVectorX   = 1;
SimSat.BodyAlignmentVectorY   = 0;
SimSat.BodyAlignmentVectorZ   = 0;
SimSat.BodyConstraintVectorX  = 0;
SimSat.BodyConstraintVectorY  = 0;
SimSat.BodyConstraintVectorZ  = 1;
SimSat.Id                     = 'GEOSAT';
SimSat.AddHardware            = {Transponder1, SpacecraftAntenna};

Create Spacecraft EstSat;

EstSat.DateFormat             = UTCGregorian;
EstSat.Epoch                  = '10 Jun 2012 00:00:00.000';
EstSat.CoordinateSystem       = EarthMJ2000Eq;
EstSat.DisplayStateType       = Cartesian;
EstSat.X                      = -4009.814
EstSat.Y                      = -41239.052
EstSat.Z                      = -7779.138
EstSat.VX                     =  3.038104
EstSat.VY                     = -0.215458
EstSat.VZ                     = -0.423910
EstSat.DryMass                = 10;
EstSat.Cd                     = 2.2;
EstSat.Cr                     = 1.8;
EstSat.DragArea               = 15;
EstSat.SRPArea                = 15;
EstSat.Attitude 			  = NadirPointing;
EstSat.AttitudeReferenceBody  = Sun;
EstSat.AttitudeConstraintType = 'OrbitNormal';
EstSat.BodyAlignmentVectorX   = 1;
EstSat.BodyAlignmentVectorY   = 0;
EstSat.BodyAlignmentVectorZ   = 0;
EstSat.BodyConstraintVectorX  = 0;
EstSat.BodyConstraintVectorY  = 0;
EstSat.BodyConstraintVectorZ  = 1;
EstSat.Id                     = 'GEOSAT';
EstSat.AddHardware            = {Transponder1, SpacecraftAntenna};
EstSat.SolveFors              = {CartesianState};     

%
%   N-Plate models
%

Create Plate SimPlate1 SimPlate2 

SimPlate1.Type             = FixedInBody
SimPlate1.PlateNormal      = [1.0, 0.0, 0.0] 					
SimPlate1.LitFraction      = 1.0                          				
SimPlate1.AreaCoefficient  = 1.0
SimPlate1.Area             = 500
SimPlate1.SpecularFraction = 0.5
SimPlate1.DiffuseFraction  = 0.5

SimPlate2.Type             = FixedInBody
SimPlate2.PlateNormal      = [-1.0, 0.0, 0.0] 					
SimPlate2.LitFraction      = 1.0                          				
SimPlate2.AreaCoefficient  = 1.0			
SimPlate2.Area             = 500
SimPlate2.SpecularFraction = 0.5;
SimPlate2.DiffuseFraction  = 0.5;

SimSat.AddPlates = {SimPlate1, SimPlate2}; 

Create Plate EstPlate1 EstPlate2 

EstPlate1.Type             = FixedInBody
EstPlate1.PlateNormal      = [1.0, 0.0, 0.0] 					
EstPlate1.LitFraction      = 1.0                          				
EstPlate1.AreaCoefficient  = 1.0
EstPlate1.Area             = 500
EstPlate1.SpecularFraction = 0.5
EstPlate1.DiffuseFraction  = 0.5
EstPlate1.SolveFors        = {};

EstPlate2.Type             = FixedInBody
EstPlate2.PlateNormal      = [-1.0, 0.0, 0.0] 					
EstPlate2.LitFraction      = 1.0                          				
EstPlate2.AreaCoefficient  = 1.0			
EstPlate2.Area             = 500
EstPlate2.SpecularFraction = 0.5;
EstPlate2.DiffuseFraction  = 0.5;
EstPlate2.SolveFors        = {};

EstSat.AddPlates = {EstPlate1, EstPlate2}; 

%
%   Spacecraft hardware
%

Create Antenna SpacecraftAntenna;
Create Transponder Transponder1;

Transponder1.PrimaryAntenna  = SpacecraftAntenna;
Transponder1.HardwareDelay   = 0.00005;
Transponder1.TurnAroundRatio = '240/221' 

%
%   GroundStation hardware
%

Create Transmitter Transmitter1;
Create Antenna Antenna1;
Create Receiver Receiver1;

Transmitter1.PrimaryAntenna = Antenna1;
Transmitter1.Frequency      = 2067.5;
Receiver1.PrimaryAntenna    = Antenna1;

%
%   Ground stations
%

Create GroundStation GDS;

GDS.CentralBody           = Earth;
GDS.StateType             = Cartesian;
GDS.HorizonReference      = Ellipsoid;
GDS.Location1             = -2353.621251;
GDS.Location2             = -4641.341542;
GDS.Location3             =  3677.052370;
GDS.Id                    = 'GDS';
GDS.AddHardware           = {Transmitter1, Receiver1, Antenna1};
GDS.MinimumElevationAngle = 10;
GDS.ErrorModels           = {RangeModel, RangeRateModel};

Create GroundStation CAN;

CAN.CentralBody           = Earth;
CAN.StateType             = Cartesian;
CAN.HorizonReference      = Ellipsoid;
CAN.Location1             = -4461.083514;
CAN.Location2             =  2682.281745;
CAN.Location3             = -3674.570392;
CAN.Id                    = 'CAN';
CAN.AddHardware           = {Transmitter1, Receiver1, Antenna1};
CAN.MinimumElevationAngle = 10.;
CAN.ErrorModels           = {RangeModel, RangeRateModel};

Create GroundStation MAD;

MAD.CentralBody           = Earth;
MAD.StateType             = Cartesian;
MAD.HorizonReference      = Ellipsoid;
MAD.Location1             = 4849.519988;
MAD.Location2             = -360.641653;
MAD.Location3             = 4114.504590;
MAD.Id                    = 'MAD';
MAD.AddHardware           = {Transmitter1, Receiver1, Antenna1};
MAD.MinimumElevationAngle = 10.;
MAD.ErrorModels           = {RangeModel, RangeRateModel};

%
%   Error models
%

Create ErrorModel RangeModel;

RangeModel.Type           = 'Range';
RangeModel.NoiseSigma     = 0.010;
RangeModel.Bias           = 0.0;
RangeModel.SolveFors      = {};

Create ErrorModel RangeRateModel;

RangeRateModel.Type       = 'RangeRate';
RangeRateModel.NoiseSigma = 0.00001;
RangeRateModel.Bias       = 0.0;
RangeRateModel.SolveFors  = {};

%
%   Tracking file sets
%

Create TrackingFileSet simData;

simData.AddTrackingConfig       = {{GDS, SimSat, GDS}, 'Range', 'RangeRate'};
simData.AddTrackingConfig       = {{CAN, SimSat, CAN}, 'Range', 'RangeRate'};
simData.AddTrackingConfig       = {{MAD, SimSat, MAD}, 'Range', 'RangeRate'};
simData.FileName                = {'Plate_Est_ApplyNPlateSRP.gmd'};
simData.RampTable               = {};
simData.UseLightTime            = True;
simData.UseRelativityCorrection = False;
simData.UseETminusTAI           = False;
simData.SimRangeModuloConstant  = 67108864;
simData.SimDopplerCountInterval = 10.;  
simData.DataFilters             = {};

Create TrackingFileSet estData;

estData.AddTrackingConfig       = {{GDS, EstSat, GDS}, 'Range', 'RangeRate'};
estData.AddTrackingConfig       = {{CAN, EstSat, CAN}, 'Range', 'RangeRate'};
estData.AddTrackingConfig       = {{MAD, EstSat, MAD}, 'Range', 'RangeRate'};
estData.FileName                = {'Plate_Est_ApplyNPlateSRP.gmd'};
estData.RampTable               = {};
estData.UseLightTime            = True;
estData.UseRelativityCorrection = False;
estData.UseETminusTAI           = False;
estData.SimRangeModuloConstant  = 67108864;
estData.SimDopplerCountInterval = 10.;  
estData.DataFilters             = {};

%
%   Propagators
%

Create ForceModel FM;

FM.CentralBody       = Earth;
FM.PointMasses       = {Earth};
FM.Drag              = None;
FM.SRP               = On;
FM.SRPModel          = NPlate;
% FM.SRPModel          = Spherical;
FM.ErrorControl      = None;

Create Propagator Prop;

Prop.FM              = FM;
Prop.Type            = 'RungeKutta89';
Prop.InitialStepSize = 60;
Prop.Accuracy        = 1e-13;
Prop.MinStep         = 0;
Prop.MaxStep         = 60;
Prop.MaxStepAttempts = 50;

%
%   Simulator
%

Create Simulator sim;

sim.AddData                    = {simData};
sim.EpochFormat                = 'UTCGregorian';
sim.InitialEpoch               = '10 Jun 2012 00:00:00.000';
sim.FinalEpoch                 = '11 Jun 2012 00:00:00.000';
sim.MeasurementTimeStep        = 60;
sim.Propagator                 = Prop;
sim.AddNoise                   = On;

%
%   Estimator
%

Create BatchEstimator bat

bat.ShowProgress               = True;
bat.Measurements               = {estData} 
bat.AbsoluteTol                = 0.0001;
bat.RelativeTol                = 0.0001;
bat.MaximumIterations          = 10;
bat.MaxConsecutiveDivergences  = 3;
bat.Propagator                 = Prop;
bat.ShowAllResiduals           = On;
bat.OLSEInitialRMSSigma        = 1000;
bat.OLSEMultiplicativeConstant = 3;
bat.OLSEAdditiveConstant       = 0;
bat.InversionAlgorithm         = 'Internal';
bat.EstimationEpoch            = 'FromParticipants'; 
bat.ReportStyle                = 'Normal';
bat.ReportFile                 = 'Plate_Est_ApplyNPlateSRP.txt';

BeginMissionSequence

SetSeed(11);

RunSimulator sim;
RunEstimator bat;