%
%   EKF_ErrorModel_NoiseSigma_DSN
%
%   Test that the measurement noise sigma value is applied properly during the 
%   filter measurement update.
%
%   08/06/2019 SES Initial version
%

%
%   Spacecraft
%

Create Spacecraft SimSat;

SimSat.DateFormat        = UTCGregorian;
SimSat.Epoch             = '10 Jun 2010 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.8;
SimSat.DragArea          = 15;
SimSat.SRPArea           = 15;
SimSat.Id                = 'LEOSat';
SimSat.AddHardware       = {Transponder1, Antenna2};

Create Spacecraft EstSat;

EstSat.DateFormat        = UTCGregorian;
EstSat.Epoch             = '10 Jun 2010 00:00:00.000';
EstSat.CoordinateSystem  = EarthMJ2000Eq;
EstSat.DisplayStateType  = Cartesian;
EstSat.X                 =   576.869556
EstSat.Y                 = -5701.142761
EstSat.Z                 = -4170.593691
EstSat.VX                = -1.76450794
EstSat.VY                =  4.18128798
EstSat.VZ                = -5.96578986
EstSat.DryMass           = 850;
EstSat.Cd                = 2.2;
EstSat.Cr                = 1.8;
EstSat.DragArea          = 15;
EstSat.SRPArea           = 15;
EstSat.Id                = 'LEOSat';
EstSat.AddHardware       = {Transponder1, Antenna2};
EstSat.SolveFors         = {CartesianState};     
EstSat.ProcessNoiseModel = SNC;

Create ProcessNoiseModel SNC;

SNC.Type             = StateNoiseCompensation;
SNC.CoordinateSystem = EarthMJ2000Eq;
SNC.AccelSigmaVector = [0 0 0];

%
%   Communications Hardware
%

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

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

Create Transponder Transponder1;

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

%
%   Error models
%

Create ErrorModel RangeModel;

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

Create ErrorModel RangeRateModel;

RangeRateModel.Type       = 'DSN_TCP';
RangeRateModel.NoiseSigma = 0.001;
RangeRateModel.Bias       = 0.0;
RangeRateModel.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, 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 = 0;
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 = 0;
MAD.ErrorModels           = {RangeModel, RangeRateModel};

%
%   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                = {'EKF_ErrorModel_NoiseSigma_DSN.gmd'};
SimData.RampTable               = {};
SimData.UseLightTime            = False;
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                = {'EKF_ErrorModel_NoiseSigma_DSN.gmd'};
EstData.RampTable               = {};
EstData.UseLightTime            = False;
EstData.UseRelativityCorrection = False;
EstData.UseETminusTAI           = False;
EstData.SimRangeModuloConstant  = 67108864;
EstData.SimDopplerCountInterval = 10.;  
EstData.DataFilters             = {AF};

Create AcceptFilter AF;

AF.DataTypes = {'DSN_SeqRange', 'DSN_TCP'};

%
%   Propagators
%

Create ForceModel FM;

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

Create Propagator ODProp;

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

%
%    Solvers
%

Create Simulator Sim;

Sim.AddData             = {SimData};
Sim.EpochFormat         = 'UTCGregorian';
Sim.InitialEpoch        = '10 Jun 2010 00:00:00.000';
Sim.FinalEpoch          = '10 Jun 2010 04:00:00.000';
Sim.MeasurementTimeStep = 60;
Sim.Propagator          = ODProp;
Sim.AddNoise            = Off;

%
%   Estimator
%

Create ExtendedKalmanFilter EKF;

EKF.ShowProgress            = True;
EKF.ReportFile              = 'EKF_ErrorModel_NoiseSigma_DSN.txt';
EKF.MatlabFile              = 'EKF_ErrorModel_NoiseSigma_DSN.mat';
EKF.Measurements            = {EstData};
EKF.Propagator              = ODProp;
EKF.ShowAllResiduals        = Off;
EKF.ScaledResidualThreshold = 4;
EKF.ProcessNoiseTimeStep    = 86400.;
EKF.DataFilters             = {RF}

Create RejectFilter RF;
RF.DataTypes = {'DSN_SeqRange', 'DSN_TCP'}

%
%   Mission Sequence
%

BeginMissionSequence

EstSat.OrbitErrorCovariance = diag([1e-20 1e-20 1e-20 1e-20 1e-20 1e-20]);

RunSimulator Sim;
RunEstimator EKF;