%
%   ErrorModel_NoiseSigma_TdrsRangeKm2
%
%   Use the SN_Range error model type with TDRS observations. Use NoiseSigma for
%   simulation equal to 3x noise sigma for estimation, and test that the 
%   solution converges with a WRMS ~= 3.
%
%   01/12/2016 SES Initial version.
%

%
%   Spacecraft
%

Create Spacecraft SimAqua;

SimAqua.DateFormat         = UTCGregorian;
SimAqua.Epoch              = '3 Aug 2015 00:00:00.000';
SimAqua.CoordinateSystem   = EarthMJ2000Eq;
SimAqua.DisplayStateType   = Cartesian;
SimAqua.X                  = -.6429439955867712e+04; 
SimAqua.Y                  =  .2874907490717409e+04;  
SimAqua.Z                  = -.7629764737136650e+03;
SimAqua.VX                 = -.2809923327341527e+00;
SimAqua.VY                 =  .1308479044883331e+01;
SimAqua.VZ                 =  .7382127795330074e+01;
SimAqua.DryMass            = 2937;
SimAqua.Cd                 = 2.2;
SimAqua.Cr                 = 1.5;
SimAqua.DragArea           = 47.95;
SimAqua.SRPArea            = 47.95;
SimAqua.Id                 = 1874;
SimAqua.AddHardware        = {AquaSnTransponder, AquaSnAntenna};

Create Spacecraft SimAquaCopy;
SimAquaCopy = SimAqua;

Create Spacecraft Aqua;

Aqua.DateFormat         = UTCGregorian;
Aqua.Epoch              = '3 Aug 2015 00:00:00.000';
Aqua.CoordinateSystem   = EarthMJ2000Eq;
Aqua.DisplayStateType   = Cartesian;
Aqua.X                  = -.6429439e+04; 
Aqua.Y                  =  .2874907e+04;  
Aqua.Z                  = -.762976e+03;
Aqua.VX                 = -.280992e+00;
Aqua.VY                 =  .1308479e+01;
Aqua.VZ                 =  .7382127e+01;
Aqua.DryMass            = 2937;
Aqua.Cd                 = 2.2;
Aqua.Cr                 = 0.5;
Aqua.DragArea           = 47.95;
Aqua.SRPArea            = 47.95;
Aqua.Id                 = 1874;
Aqua.AddHardware        = {AquaSnTransponder, AquaSnAntenna};
Aqua.SolveFors          = {CartesianState};     

%
%   TDRS-6
%

Create Spacecraft TDRS6;

TDRS6.DateFormat         = UTCGregorian;
TDRS6.Epoch              = '3 Aug 2015 00:00:00.000';
TDRS6.CoordinateSystem   = EarthMJ2000Eq;
TDRS6.DisplayStateType   = Cartesian;
TDRS6.X                  = -.4009814422793567e+04;
TDRS6.Y                  = -.4123905262716184e+05;
TDRS6.Z                  = -.7779138529585810e+04;
TDRS6.VX                 =  .3038104350376643e+01;
TDRS6.VY                 = -.2154583180496774e+00;
TDRS6.VZ                 = -.4239104996030301e+00;
TDRS6.DryMass            = 1745.322;
TDRS6.Cd                 = 2.2;
TDRS6.Cr                 = 1.4;
TDRS6.DragArea           = 40.0;
TDRS6.SRPArea            = 40.0;
TDRS6.Id                 = 1305;
TDRS6.AddHardware        = {TdrsSnTransponder, TdrsToSgltAntenna};
TDRS6.SolveFors          = {};     

Create Spacecraft SimTDRS6;
SimTDRS6 = TDRS6;

%
%   TDRS-7
%

Create Spacecraft TDRS7;

TDRS7.DateFormat         = UTCGregorian;
TDRS7.Epoch              = '3 Aug 2015 00:00:00.000';
TDRS7.CoordinateSystem   = EarthMJ2000Eq;
TDRS7.DisplayStateType   = Cartesian;
TDRS7.X                  = -.4009814422793567e+04;
TDRS7.Y                  = -.4123905262716184e+05;
TDRS7.Z                  = -.7779138529585810e+04;
TDRS7.VX                 =  .3038104350376643e+01;
TDRS7.VY                 = -.2154583180496774e+00;
TDRS7.VZ                 = -.4239104996030301e+00;
TDRS7.DryMass            = 1745.322;
TDRS7.Cd                 = 2.2;
TDRS7.Cr                 = 1.4;
TDRS7.DragArea           = 40.0;
TDRS7.SRPArea            = 40.0;
TDRS7.Id                 = 1306;
TDRS7.AddHardware        = {TdrsSnTransponder, TdrsToSgltAntenna};
TDRS7.SolveFors          = {};     

Create Spacecraft SimTDRS7;
SimTDRS7 = TDRS7;
 
%
%   TDRS-10
%

Create Spacecraft TDRS10;

TDRS10.DateFormat        = UTCGregorian;
TDRS10.Epoch             = '3 Aug 2015 00:00:00.000';
TDRS10.CoordinateSystem  = EarthMJ2000Eq;
TDRS10.DisplayStateType  = Cartesian;
TDRS10.X                 = -.3053153777803951e+05;
TDRS10.Y                 =  .2895798519316169e+05;
TDRS10.Z                 =  .2772540318286565e+04;
TDRS10.VX                = -.2111462571384225e+01;
TDRS10.VY                = -.2233689318954797e+01;
TDRS10.VZ                =  .5613667479608277e-01;
TDRS10.DryMass           = 1771.409;
TDRS10.Cd                = 2.2;
TDRS10.Cr                = 1.0;
TDRS10.DragArea          = 65.65;
TDRS10.SRPArea           = 65.65;
TDRS10.Id                = 1309;
TDRS10.AddHardware       = {TdrsSnTransponder, TdrsToSgltAntenna, TdrsToUserAntenna};
TDRS10.SolveFors         = {};     

Create Spacecraft SimTDRS10;
SimTDRS10 = TDRS10;

%
%   TDRS-11
%

Create Spacecraft TDRS11;

TDRS11.DateFormat        = UTCGregorian;
TDRS11.Epoch             = '3 Aug 2015 00:00:00.000';
TDRS11.CoordinateSystem  = EarthMJ2000Eq;
TDRS11.DisplayStateType  = Cartesian;
TDRS11.X                 = -.3234326905566657e+05;
TDRS11.Y                 =  .2702639401810180e+05;
TDRS11.Z                 =  .8185431111661278e+03;
TDRS11.VX                = -.1963514815885257e+01;
TDRS11.VY                = -.2344472068499413e+01;
TDRS11.VZ                = -.3241792651226365e+00;
TDRS11.DryMass           = 2185.181;
TDRS11.Cd                = 2.2;
TDRS11.Cr                = 1.0;
TDRS11.DragArea          = 65.65;
TDRS11.SRPArea           = 65.65;
TDRS11.Id                = 1410;
TDRS11.AddHardware       = {TdrsSnTransponder, TdrsToSgltAntenna};
TDRS11.SolveFors         = {};     

Create Spacecraft SimTDRS11;
SimTDRS11 = TDRS11;

%
%   TDRS-12
%

Create Spacecraft TDRS12;

TDRS12.DateFormat        = UTCGregorian;
TDRS12.Epoch             = '3 Aug 2015 00:00:00.000';
TDRS12.CoordinateSystem  = EarthMJ2000Eq;
TDRS12.DisplayStateType  = Cartesian;
TDRS12.X                 = -.4233934462786078e+01;
TDRS12.Y                 = -.4192968255546268e+05;
TDRS12.Z                 = -.4489431959906155e+04;
TDRS12.VX                =  .3071530933710167e+01;
TDRS12.VY                = -.1558698373469404e-01;
TDRS12.VZ                =  .1315937818133251e+00;
TDRS12.DryMass           = 2192.89;
TDRS12.Cd                = 2.2;
TDRS12.Cr                = 1.0;
TDRS12.DragArea          = 65.65;
TDRS12.SRPArea           = 65.65;
TDRS12.Id                = 1411;
TDRS12.AddHardware       = {TdrsSnTransponder, TdrsToSgltAntenna};
TDRS12.SolveFors         = {};     

Create Spacecraft SimTDRS12;
SimTDRS12 = TDRS12;

%
%   Force model and propagator.
%

Create ForceModel Fm;
Create Propagator Prop;

Fm.CentralBody          = Earth;
Fm.PointMasses          = {Earth};
Fm.SRP                  = Off;
Fm.Drag.AtmosphereModel = None;
Fm.ErrorControl         = 'None';

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

%
%   Tracking data selection.
%

Create TrackingFileSet SimMeas;

SimMeas.FileName                  = {'ErrorModel_NoiseSigma_TdrsRangeKm2.gmd'};
SimMeas.AddTrackingConfig         = {{SimGW2K, SimTDRS7,  SimAqua, SimTDRS7,  SimGW2K}, 'SN_Range'};
SimMeas.AddTrackingConfig         = {{SimST2K, SimTDRS12, SimAqua, SimTDRS12, SimST2K}, 'SN_Range'};
SimMeas.AddTrackingConfig         = {{SimWH2K, SimTDRS11, SimAqua, SimTDRS11, SimWH2K}, 'SN_Range'};
SimMeas.SimTDRSServiceAccessList  = {'SA1'};
SimMeas.RampTable                 = {};
SimMeas.UseLightTime              = True;
SimMeas.UseRelativityCorrection   = False;
SimMeas.UseETminusTAI             = False;
SimMeas.DataFilters               = {};

Create TrackingFileSet EstMeas;

EstMeas.FileName                  = {'ErrorModel_NoiseSigma_TdrsRangeKm2.gmd'};
EstMeas.AddTrackingConfig         = {{EstGW2K, TDRS7,  Aqua, TDRS7,  EstGW2K}, 'SN_Range'};
EstMeas.AddTrackingConfig         = {{EstST2K, TDRS12, Aqua, TDRS12, EstST2K}, 'SN_Range'};
EstMeas.AddTrackingConfig         = {{EstWH2K, TDRS11, Aqua, TDRS11, EstWH2K}, 'SN_Range'};
EstMeas.RampTable                 = {};
EstMeas.UseLightTime              = True;
EstMeas.UseRelativityCorrection   = False;
EstMeas.UseETminusTAI             = False;
EstMeas.DataFilters               = {};

%
%   Error models.
%

Create ErrorModel SimTDRSRangeRate;

SimTDRSRangeRate.Type       = 'SN_Doppler';
SimTDRSRangeRate.NoiseSigma = 0.3;
SimTDRSRangeRate.Bias       = 0.0;
SimTDRSRangeRate.SolveFors  = {};

Create ErrorModel EstTDRSRangeRate;

EstTDRSRangeRate.Type       = 'SN_Doppler';
EstTDRSRangeRate.NoiseSigma = 0.1;
EstTDRSRangeRate.Bias       = 0.0;
EstTDRSRangeRate.SolveFors  = {};

Create ErrorModel SimTDRSRange;

SimTDRSRange.Type           = 'SN_Range';
SimTDRSRange.NoiseSigma     = 0.030;
SimTDRSRange.Bias           = 0.0;
SimTDRSRange.SolveFors      = {};

Create ErrorModel EstTDRSRange;

EstTDRSRange.Type           = 'SN_Range';
EstTDRSRange.NoiseSigma     = 0.010;
EstTDRSRange.Bias           = 0.0;
EstTDRSRange.SolveFors      = {};

%
%   Configure BLS.
%

Create BatchEstimatorInv Bls;

Bls.ShowProgress                = true;
Bls.Measurements                = {EstMeas};
Bls.AbsoluteTol                 = 0.0001;
Bls.RelativeTol                 = 0.0001;
Bls.MaximumIterations           = 4;
Bls.MaxConsecutiveDivergences   = 4;
Bls.Propagator                  = Prop;
Bls.ShowAllResiduals            = On;
Bls.OLSEInitialRMSSigma         = 3000;
Bls.OLSEMultiplicativeConstant  = 3;
Bls.OLSEAdditiveConstant        = 0;
Bls.InversionAlgorithm          = 'Internal';
Bls.EstimationEpochFormat       = 'FromParticipants';
Bls.EstimationEpoch             = 'FromParticipants'; 
Bls.ReportStyle                 = 'Normal';
Bls.ReportFile                  = 'ErrorModel_NoiseSigma_TdrsRangeKm2_batreport.txt';
Bls.MatlabFile                  = 'ErrorModel_NoiseSigma_TdrsRangeKm2.mat';
% Bls.MatlabFile                  = 'C:\Users\sslojkow\Desktop\ErrorModel_NoiseSigma_TdrsRangeKm2.mat';
% Bls.MatlabFile                  = '\\mesa-file\595\GMAT\SES\ErrorModel_NoiseSigma_TdrsRangeKm2.mat';

%
%	Aqua spacecraft electronics. 
%	Transponder delay is 209.67 meters. HardwareDelay is in seconds.
%

Create Antenna AquaSnAntenna;
Create Transponder AquaSnTransponder;

AquaSnTransponder.PrimaryAntenna  = AquaSnAntenna;
AquaSnTransponder.HardwareDelay   = 1.398767677e-6;
AquaSnTransponder.TurnAroundRatio = '1/1' 

%
%	TDRS spacecraft electronics. 
%	No transponder delay for TDRS - it is removed at White Sands.
%

Create Antenna TdrsToSgltAntenna;
Create Antenna TdrsToUserAntenna;
Create Transponder TdrsSnTransponder;

TdrsSnTransponder.PrimaryAntenna  = TdrsToSgltAntenna;
TdrsSnTransponder.HardwareDelay   = 0;
TdrsSnTransponder.TurnAroundRatio = '1/1' 

%
%   Ground electronics.
%

Create Transmitter UplinkTransmitter;
Create Antenna UplinkAntenna;
Create Receiver GroundReceiver;

UplinkTransmitter.Frequency      = 2106.406250; %% MHz
UplinkTransmitter.PrimaryAntenna = UplinkAntenna;
GroundReceiver.PrimaryAntenna    = UplinkAntenna;

%
%   White Sands SGLTs.
%

Create GroundStation SimGW2K;

SimGW2K.CentralBody           = Earth;
SimGW2K.StateType             = Cartesian;
SimGW2K.HorizonReference      = Ellipsoid;
SimGW2K.Location1             = -5069.63392;
SimGW2K.Location2             =  3570.81969;
SimGW2K.Location3             =  1488.71610;
SimGW2K.Id                    = 95;
SimGW2K.AddHardware           = {UplinkTransmitter, UplinkAntenna, GroundReceiver};
SimGW2K.MinimumElevationAngle = 0.0;
SimGW2K.IonosphereModel       = 'IRI2007';
SimGW2K.TroposphereModel      = 'HopfieldSaastamoinen';
% SimGW2K.Temperature           = 295.1;   % Kelvin
% SimGW2K.Pressure              = 1013.5;  % hPa
% SimGW2K.Humidity              = 55.0;    % Percent
SimGW2K.ErrorModels           = {SimTDRSRange, SimTDRSRangeRate};

Create GroundStation EstGW2K;

EstGW2K.CentralBody           = Earth;
EstGW2K.StateType             = Cartesian;
EstGW2K.HorizonReference      = Ellipsoid;
EstGW2K.Location1             = -5069.63392;
EstGW2K.Location2             =  3570.81969;
EstGW2K.Location3             =  1488.71610;
EstGW2K.Id                    = 95;
EstGW2K.AddHardware           = {UplinkTransmitter, UplinkAntenna, GroundReceiver};
EstGW2K.MinimumElevationAngle = 0.0;
EstGW2K.IonosphereModel       = 'IRI2007';
EstGW2K.TroposphereModel      = 'HopfieldSaastamoinen';
% EstGW2K.Temperature           = 295.1;   % Kelvin
% EstGW2K.Pressure              = 1013.5;  % hPa
% EstGW2K.Humidity              = 55.0;    % Percent
EstGW2K.ErrorModels           = {EstTDRSRange, EstTDRSRangeRate};

Create GroundStation SimST2K;

SimST2K.CentralBody           = Earth;
SimST2K.StateType             = Cartesian;
SimST2K.HorizonReference      = Ellipsoid;
SimST2K.Location1             = -1538.98711;
SimST2K.Location2             = -5158.45398;
SimST2K.Location3             =  3412.12307;
SimST2K.Id                    = 48;
SimST2K.AddHardware           = {UplinkTransmitter, UplinkAntenna, GroundReceiver};
SimST2K.MinimumElevationAngle = 0.0;
SimST2K.IonosphereModel       = 'IRI2007';
SimST2K.TroposphereModel      = 'HopfieldSaastamoinen';
% SimST2K.Temperature           = 295.1;   % Kelvin
% SimST2K.Pressure              = 1013.5;  % hPa
% SimST2K.Humidity              = 55.0;    % Percent
SimST2K.ErrorModels           = {SimTDRSRange, SimTDRSRangeRate};

Create GroundStation EstST2K;

EstST2K.CentralBody           = Earth;
EstST2K.StateType             = Cartesian;
EstST2K.HorizonReference      = Ellipsoid;
EstST2K.Location1             = -1538.98711;
EstST2K.Location2             = -5158.45398;
EstST2K.Location3             =  3412.12307;
EstST2K.Id                    = 48;
EstST2K.AddHardware           = {UplinkTransmitter, UplinkAntenna, GroundReceiver};
EstST2K.MinimumElevationAngle = 0.0;
EstST2K.IonosphereModel       = 'IRI2007';
EstST2K.TroposphereModel      = 'HopfieldSaastamoinen';
% EstST2K.Temperature           = 295.1;   % Kelvin
% EstST2K.Pressure              = 1013.5;  % hPa
% EstST2K.Humidity              = 55.0;    % Percent
EstST2K.ErrorModels           = {EstTDRSRange, EstTDRSRangeRate};

Create GroundStation SimWH2K;

SimWH2K.CentralBody           = Earth;
SimWH2K.StateType             = Cartesian;
SimWH2K.HorizonReference      = Ellipsoid;
SimWH2K.Location1             = -1539.38574;
SimWH2K.Location2             = -5160.95312;
SimWH2K.Location3             =  3408.20216;
SimWH2K.Id                    = 9;
SimWH2K.AddHardware           = {UplinkTransmitter, UplinkAntenna, GroundReceiver};
SimWH2K.MinimumElevationAngle = 0.0;
SimWH2K.IonosphereModel       = 'IRI2007';
SimWH2K.TroposphereModel      = 'HopfieldSaastamoinen';
% SimWH2K.Temperature           = 295.1;   % Kelvin
% SimWH2K.Pressure              = 1013.5;  % hPa
% SimWH2K.Humidity              = 55.0;    % Percent
SimWH2K.ErrorModels           = {SimTDRSRange, SimTDRSRangeRate};

Create GroundStation EstWH2K;

EstWH2K.CentralBody           = Earth;
EstWH2K.StateType             = Cartesian;
EstWH2K.HorizonReference      = Ellipsoid;
EstWH2K.Location1             = -1539.38574;
EstWH2K.Location2             = -5160.95312;
EstWH2K.Location3             =  3408.20216;
EstWH2K.Id                    = 9;
EstWH2K.AddHardware           = {UplinkTransmitter, UplinkAntenna, GroundReceiver};
EstWH2K.MinimumElevationAngle = 0.0;
EstWH2K.IonosphereModel       = 'IRI2007';
EstWH2K.TroposphereModel      = 'HopfieldSaastamoinen';
% EstWH2K.Temperature           = 295.1;   % Kelvin
% EstWH2K.Pressure              = 1013.5;  % hPa
% EstWH2K.Humidity              = 55.0;    % Percent
EstWH2K.ErrorModels           = {EstTDRSRange, EstTDRSRangeRate};

%
%   Simulator.
%

Create Simulator Sim;

Sim.AddData             = {SimMeas};
Sim.EpochFormat         = 'UTCGregorian';
Sim.InitialEpoch        = '03 Aug 2015 00:00:00.000';
Sim.FinalEpoch          = '04 Aug 2015 00:00:00.000';
Sim.MeasurementTimeStep = 120;
Sim.Propagator          = Prop;
Sim.AddNoise            = On;

%
%   Mission sequence.
%

Create Variable PosError VelError

Create ReportFile rf

rf.Filename         = 'ErrorModel_NoiseSigma_TdrsRangeKm2.report';
rf.WriteHeaders     = false;

Create ReportFile rftemp

rftemp.Filename     = 'ErrorModel_NoiseSigma_TdrsRangeKm2_temp.report';
rftemp.WriteHeaders = false;

%----- Run the Simulator followed by the Estimator

BeginMissionSequence

SetSeed(1);

RunSimulator Sim;
RunEstimator Bls;

PosError = sqrt ((Aqua.X - SimAquaCopy.X)^2 + ...
                 (Aqua.Y - SimAquaCopy.Y)^2 + ...
                 (Aqua.Z - SimAquaCopy.Z)^2 )

VelError = sqrt ((Aqua.VX - SimAquaCopy.VX)^2  + ...
                 (Aqua.VY - SimAquaCopy.VY)^2  + ...
                 (Aqua.VZ - SimAquaCopy.VZ)^2 )

Report rftemp SimAquaCopy.TAIModJulian ...
    SimAquaCopy.EarthMJ2000Eq.X ...
    SimAquaCopy.EarthMJ2000Eq.Y ...
    SimAquaCopy.EarthMJ2000Eq.Z ...
    SimAquaCopy.EarthMJ2000Eq.VX ...
    SimAquaCopy.EarthMJ2000Eq.VY ...
    SimAquaCopy.EarthMJ2000Eq.VZ;
    
Report rftemp Aqua.TAIModJulian ...
    Aqua.EarthMJ2000Eq.X ...
    Aqua.EarthMJ2000Eq.Y ...
    Aqua.EarthMJ2000Eq.Z ...
    Aqua.EarthMJ2000Eq.VX ...
    Aqua.EarthMJ2000Eq.VY ...
    Aqua.EarthMJ2000Eq.VZ;

Report rf PosError VelError