%
%   BatchEstimator_EstimateCartesianState
%
%   Solve for the CartesianState only.
%
%   5/22/2015 SES Updated to new nav resources.
%   9/25/2014 SES Initial version.
%

%
%   Spacecraft
%

Create Spacecraft SimSat;

SimSat.DateFormat       = UTCGregorian;
SimSat.Epoch            = '01 Mar 2005 00:00:00.000';
SimSat.CoordinateSystem = EarthMJ2000Eq;
SimSat.DisplayStateType = Cartesian;
SimSat.X                = 6648.937;
SimSat.Y                = -335.169;
SimSat.Z                = 2789.137;
SimSat.VX               = -0.34697109;
SimSat.VY               = 7.092177415;
SimSat.VZ               = 2.14549075;
SimSat.DryMass          = 850;
SimSat.Cd               = 2.2;
SimSat.Cr               = 1.8;
SimSat.DragArea         = 15;
SimSat.SRPArea          = 1;
SimSat.Id               = 'LEOSat';
SimSat.AddHardware      = {Transponder1, Antenna2};

Create Spacecraft SimSatCopy;
SimSatCopy = SimSat

Create Spacecraft EstSat;

EstSat.DateFormat       = UTCGregorian;
EstSat.Epoch            = '01 Mar 2005 00:00:00.000';
EstSat.CoordinateSystem = EarthMJ2000Eq;
EstSat.DisplayStateType = Cartesian;
EstSat.X                = 6648.937;
EstSat.Y                = -335.169;
EstSat.Z                = 2789.137;
EstSat.VX               = -0.34697109;
EstSat.VY               = 7.092177415;
EstSat.VZ               = 2.14549075;
EstSat.DryMass          = 850;
EstSat.Cd               = 2.2;
EstSat.Cr               = 1.8;
EstSat.DragArea         = 15;
EstSat.SRPArea          = 1;
EstSat.Id               = 'LEOSat';
EstSat.AddHardware      = {Transponder1, Antenna2};
EstSat.SolveFors        = {CartesianState};     

%
%   Ground stations
%

Create GroundStation GDS;

GDS.CentralBody             = Earth;
GDS.StateType               = Cartesian;
GDS.HorizonReference        = Ellipsoid;
GDS.Location1               = -2353621.251e-3;
GDS.Location2               = -4641341.542e-3;
GDS.Location3               = 3677052.370e-3;
GDS.Id                      = 'GDS';
GDS.AddHardware             = {Transmitter1, Receiver1, Antenna1};
GDS.MinimumElevationAngle   = 10;
GDS.ErrorModels             = {DSNRange_ErrorModel, TcpModel};
GDS.IonosphereModel         = 'None';

Create GroundStation CAN;

CAN.CentralBody           = Earth;
CAN.StateType             = Cartesian;
CAN.HorizonReference      = Ellipsoid;
CAN.Location1             = -4461083.514e-3;
CAN.Location2             = 2682281.745e-3;
CAN.Location3             = -3674570.392e-3;
CAN.Id                    = 'CAN';
CAN.AddHardware           = {Transmitter1, Receiver1, Antenna1};
CAN.MinimumElevationAngle = 10;
CAN.ErrorModels           = {DSNRange_ErrorModel, TcpModel};
CAN.TroposphereModel      = 'None';
CAN.IonosphereModel       = 'None';

%
%   Error models
%   Range units are RU, not meters.
%

Create ErrorModel DSNRange_ErrorModel;

DSNRange_ErrorModel.Type       = Range_RU;
DSNRange_ErrorModel.NoiseSigma = 10.;
DSNRange_ErrorModel.Bias       = 0.0;
DSNRange_ErrorModel.SolveFors  = {};

Create ErrorModel TcpModel;

TcpModel.Type       = Doppler_HZ;
TcpModel.NoiseSigma = 0.01;
TcpModel.Bias       = 0.0;
TcpModel.SolveFors  = {};

%
%   Tracking file sets
%

Create TrackingFileSet simData;

simData.AddTrackingConfig       = {{GDS, SimSat, GDS}, 'DSNRange'}
simData.AddTrackingConfig       = {{CAN, SimSat, CAN}, 'DSNRange'}
simData.FileName                = {BatchEstimator_EstimateCartesianState.gmd};
% simData.RampTable               = {ramp.rmp};
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}, 'DSNRange'}
estData.AddTrackingConfig       = {{CAN, EstSat, CAN}, 'DSNRange'}
estData.FileName                = {BatchEstimator_EstimateCartesianState.gmd};
% estData.RampTable               = {ramp.rmp};
estData.UseLightTime            = True;
estData.UseRelativityCorrection = False;
estData.UseETminusTAI           = False;
estData.DataFilters             = {};

Create StatisticsAcceptFilter filter;
% Create StatisticsRejectFilter filter;
filter.FileNames = {'From_AddTrackingConfig'};

%
%   Propagators
%

Create ForceModel ODProp_ForceModel;

ODProp_ForceModel.CentralBody  = Earth;
ODProp_ForceModel.PointMasses  = {Earth};
ODProp_ForceModel.Drag         = None;
ODProp_ForceModel.SRP          = Off;
ODProp_ForceModel.ErrorControl = RSSStep;

Create Propagator ODProp;

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

%
%   Communications Hardware
%

Create Antenna Antenna1 Antenna2;

Create Transmitter Transmitter1;

Transmitter1.PrimaryAntenna = Antenna1;
Transmitter1.Frequency      = 7186.5; %% MHz
% Transmitter1.Frequency      = 0.; %% MHz

Create Receiver Receiver1;

Receiver1.PrimaryAntenna = Antenna1;

Create Transponder Transponder1;

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

%
%    Solvers
%

Create Simulator sim;

sim.AddData                     = {simData};
sim.EpochFormat                 = UTCGregorian;
sim.InitialEpoch                = '01 Mar 2005 00:00:00.000';
sim.FinalEpoch                  = '04 Mar 2005 00:00:00.000';
sim.MeasurementTimeStep         = 60;
sim.Propagator                  = ODProp;
sim.AddNoise                    = True;

Create BatchEstimatorInv bat

bat.ShowProgress                = true;
bat.Measurements                = {estData} 
bat.AbsoluteTol                 = 0.0001;
bat.RelativeTol                 = 0.01;
bat.MaximumIterations           = 10;
bat.MaxConsecutiveDivergences   = 3;
bat.Propagator                  = ODProp;
bat.ShowAllResiduals            = On;
bat.OLSEInitialRMSSigma         = 1e70;
bat.OLSEMultiplicativeConstant  = 3;
bat.OLSEAdditiveConstant        = 0;
bat.InversionAlgorithm          = 'Internal';
bat.EstimationEpoch             = 'FromParticipants'; 
bat.ReportStyle                 = 'Normal';
bat.ReportFile                  = 'BatchEstimator_EstimateCartesianState.txt';
bat.MatlabFile                  = 'BatchEstimator_EstimateCartesianState.mat';

%
%   Mission Sequence
%

BeginMissionSequence

SetSeed(11);

RunSimulator sim;
RunEstimator bat;

% Write EstSat {LogFile = True};