%
%   ThrustSegment_SolveFors_ThrustScaleFactor_EpochDuringBurn
%
%   Solve for the Cartesian state and a thrust scale factor. Set the 
%   OD epoch during the burn.
%
%   07/24/2019 SES Initial version
%

%
%   Spacecraft
%

Create Spacecraft SimSat;

SimSat.DateFormat       = UTCGregorian;
SimSat.Epoch            = '01 Jan 2000 00:00:00.000';
SimSat.CoordinateSystem = EarthMJ2000Eq;
SimSat.DisplayStateType = Cartesian;
SimSat.X                = 576.869556
SimSat.Y                = -5701.142761
SimSat.Z                = -4170.593691
SimSat.VX               = -1.76450794
SimSat.VY               = 4.18128798
SimSat.VZ               = -5.96578986
SimSat.DryMass          = 850;
SimSat.Cd               = 2.2;
SimSat.Cr               = 1.4;
SimSat.DragArea         = 15;
SimSat.SRPArea          = 15;
SimSat.Id               = 'LEOSat';
SimSat.AddHardware      = {Transponder1, SpacecraftAntenna};

Create Spacecraft EstSat

EstSat.DateFormat       = UTCGregorian;
EstSat.Epoch            = '01 Jan 2000 00:00:00.000';
EstSat.CoordinateSystem = EarthMJ2000Eq;
EstSat.DisplayStateType = Cartesian;
EstSat.X                = 576.869
EstSat.Y                = -5701.142
EstSat.Z                = -4170.593
EstSat.VX               = -1.76450
EstSat.VY               =  4.18128
EstSat.VZ               = -5.96578
EstSat.DryMass          = 850;
EstSat.Cd               = 2.2;
EstSat.Cr               = 1.4;
EstSat.DragArea         = 15;
EstSat.SRPArea          = 15;
EstSat.Id               = 'LEOSat';
EstSat.AddHardware      = {Transponder1, SpacecraftAntenna};
EstSat.SolveFors        = {CartesianState};

%
%   Spacecraft hardware
%

Create Antenna SpacecraftAntenna;
Create Transponder Transponder1;

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

%
%   GroundStation hardware
%

Create Transmitter Transmitter1;
Create Antenna GroundAntenna;
Create Receiver Receiver1;

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

%
%   Error models
%

Create ErrorModel RangeModel;

RangeModel.Type         = 'DSN_SeqRange';
RangeModel.NoiseSigma   = 10.0;
RangeModel.Bias         = 0.0;
RangeModel.SolveFors    = {};

Create ErrorModel DopplerModel;

DopplerModel.Type       = 'DSN_TCP';
DopplerModel.NoiseSigma = 0.001;
DopplerModel.Bias       = 0.0;
DopplerModel.SolveFors  = {};

%
%   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, GroundAntenna};
GDS.MinimumElevationAngle = 0;
GDS.IonosphereModel       = 'None';
GDS.TroposphereModel      = 'None';
GDS.ErrorModels           = {RangeModel, DopplerModel};

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, GroundAntenna};
CAN.MinimumElevationAngle = 0;
CAN.IonosphereModel       = 'None';
CAN.TroposphereModel      = 'None';
CAN.ErrorModels           = {RangeModel, DopplerModel};


Create GroundStation MAD; 

MAD.CentralBody           = Earth;
MAD.StateType             = Cartesian;
MAD.HorizonReference      = Ellipsoid;
MAD.Location1             =  4849.519988;
MAD.Location2             = -0360.641653;
MAD.Location3             =  4114.504590;
MAD.Id                    = 'MAD';
MAD.AddHardware           = {Transmitter1, Receiver1, GroundAntenna};
MAD.MinimumElevationAngle = 0;
MAD.IonosphereModel       = 'None';
MAD.TroposphereModel      = 'None';
MAD.ErrorModels           = {RangeModel, DopplerModel};

%
%   Tracking file sets
%

Create TrackingFileSet simData;

simData.AddTrackingConfig       = {{GDS, SimSat, GDS}, 'DSN_SeqRange', 'DSN_TCP'};
simData.AddTrackingConfig       = {{CAN, SimSat, CAN}, 'DSN_SeqRange', 'DSN_TCP'};
simData.AddTrackingConfig       = {{MAD, SimSat, MAD}, 'DSN_SeqRange', 'DSN_TCP'};
simData.FileName                = {'ThrustSegment_SolveFors_ThrustScaleFactor_EpochDuringBurn.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}, 'DSN_SeqRange', 'DSN_TCP'};
estData.AddTrackingConfig       = {{CAN, EstSat, CAN}, 'DSN_SeqRange', 'DSN_TCP'};
estData.AddTrackingConfig       = {{MAD, EstSat, MAD}, 'DSN_SeqRange', 'DSN_TCP'};
estData.FileName                = {'ThrustSegment_SolveFors_ThrustScaleFactor_EpochDuringBurn.gmd'};
estData.RampTable               = {};
estData.UseLightTime            = True;
estData.UseRelativityCorrection = False;
estData.UseETminusTAI           = False;
estData.DataFilters             = {};

%
%   Propagators
%

Create ForceModel FM;
Create Propagator Prop;

FM.CentralBody       = Earth;
FM.PrimaryBodies     = {Earth};
FM.SRP               = Off;
FM.Drag              = None
FM.ErrorControl      = None;

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

%
%   Simulator
%

Create Simulator sim;

sim.AddData             = {simData};
sim.EpochFormat         = 'UTCGregorian';
sim.InitialEpoch        = '01 Jan 2000 00:00:00.000';
sim.FinalEpoch          = '02 Jan 2000 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           = 5;
bat.MaxConsecutiveDivergences   = 10;
bat.Propagator                  = Prop;
bat.ShowAllResiduals            = On;
bat.OLSEInitialRMSSigma         = 3.0e6;
bat.OLSEMultiplicativeConstant  = 3;
bat.OLSEAdditiveConstant        = 0;
bat.InversionAlgorithm          = 'Internal';
bat.EstimationEpoch             = 'FromParticipants'; 
bat.ReportStyle                 = 'Normal';
bat.ReportFile                  = 'ThrustSegment_SolveFors_ThrustScaleFactor_EpochDuringBurn.txt';

%
%   Thrust history file and thrusters
%

Create ThrustHistoryFile simThrustHistoryFile

simThrustHistoryFile.AddThrustSegment = {Thruster1_SetPoint1}   
simThrustHistoryFile.FileName         = '../data/navdata/thrusthistory/Thrust_CSA.thrust'

Create ThrustHistoryFile estThrustHistoryFile

estThrustHistoryFile.AddThrustSegment = {Thruster1_SetPoint1_Est}  
estThrustHistoryFile.FileName         = '../data/navdata/thrusthistory/Thrust_CSA_Est.thrust'

Create ThrustSegment Thruster1_SetPoint1;  

Thruster1_SetPoint1.ThrustScaleFactor              = 1.0
Thruster1_SetPoint1.ApplyThrustScaleToMassFlow     = False
Thruster1_SetPoint1.MassFlowScaleFactor            = 1.0
Thruster1_SetPoint1.SolveFors                      = {};
Thruster1_SetPoint1.MassSource                     = {}

Create ThrustSegment Thruster1_SetPoint1_Est;

Thruster1_SetPoint1_Est.ThrustScaleFactor          = 0.8
Thruster1_SetPoint1_Est.ApplyThrustScaleToMassFlow = False
Thruster1_SetPoint1_Est.MassFlowScaleFactor        = 1.0
Thruster1_SetPoint1_Est.SolveFors                  = {ThrustScaleFactor}  
Thruster1_SetPoint1_Est.MassSource                 = {}

%
%   Run mission sequence
%

BeginMissionSequence

SetSeed(1);

BeginFileThrust simThrustHistoryFile (SimSat);
RunSimulator sim;
EndFileThrust simThrustHistoryFile (SimSat);

BeginFileThrust estThrustHistoryFile (EstSat); 
Propagate Prop(EstSat) {EstSat.TAIModJulian = 21545.00106481481};
RunEstimator bat
EndFileThrust estThrustHistoryFile (EstSat);