%
%   ProcessNoiseModel_AccelNoiseSigma_GN
%
%   Propagate the EKF with a finite process noise
%
%   08/21/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           = 1;
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.8
EstSat.Y                 = -5701.1
EstSat.Z                 = -4170.5
EstSat.VX                = -1.7645
EstSat.VY                = 4.1813
EstSat.VZ                = -5.9658
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};     
EstSat.ProcessNoiseModel = SNC;

Create ProcessNoiseModel SNC;

SNC.Type             = StateNoiseCompensation;
SNC.CoordinateSystem = EarthMJ2000Eq;
SNC.AccelNoiseSigma  = [1.0e-6 2.0e-6 3.0e-6];

%
%   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.00010;
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 = 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};

%
%   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                = {'ProcessNoiseModel_AccelNoiseSigma_GN.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                = {'ProcessNoiseModel_AccelNoiseSigma_GN.gmd'};
EstData.RampTable               = {};
EstData.UseLightTime            = True;
EstData.UseRelativityCorrection = False;
EstData.UseETminusTAI           = False;
EstData.DataFilters             = {};

%
%   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;

%
%   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' 

%
%    Simulator
%

Create Simulator Sim;

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

%
%   Reject filter
%

Create RejectFilter RF;

RF.DataTypes = {'Range', 'RangeRate'};

%
%   Estimator
%

Create ExtendedKalmanFilter EKF;

EKF.ShowProgress         = True;
EKF.Measurements         = {EstData};
EKF.Propagator           = ODProp;
EKF.ShowAllResiduals     = Off;
EKF.ProcessNoiseTimeStep = 3600;
EKF.DataFilters          = {RF}
EKF.ReportFile           = 'ProcessNoiseModel_AccelNoiseSigma_GN.txt';
EKF.MatlabFile           = 'ProcessNoiseModel_AccelNoiseSigma_GN.mat';

%
%   Run mission sequence
%

BeginMissionSequence

EstSat.OrbitErrorCovariance = diag([1.0e-2 1.0e-2 1.0e-2 2.5e-7 2.5e-7 2.5e-7]);

SetSeed(1);

RunSimulator Sim;
RunEstimator EKF;