%
%   ProcessNoiseModel_AccelNoiseSigma_GpsPosVec
%
%   Propagate the EKF with a finite process noise
%
%   08/13/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       = {GpsReceiver, GpsAntenna};

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       = {GpsReceiver, GpsAntenna};
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];

%
%   Spacecraft hardware
%

Create Antenna GpsAntenna;
Create Receiver GpsReceiver;

GpsReceiver.PrimaryAntenna = GpsAntenna;
GpsReceiver.Id             = 800;
GpsReceiver.ErrorModels    = {PosVecModel}

%
%   Error models
%

Create ErrorModel PosVecModel;
 
PosVecModel.Type       = 'GPS_PosVec'
PosVecModel.NoiseSigma = 0.500;

%
%   Tracking file sets
%

Create TrackingFileSet SimData;

SimData.AddTrackingConfig       = {{SimSat.GpsReceiver}, 'GPS_PosVec'};
SimData.FileName                = {'ProcessNoiseModel_AccelNoiseSigma_GpsPosVec.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       = {{EstSat.GpsReceiver}, 'GPS_PosVec'};
EstData.FileName                = {'ProcessNoiseModel_AccelNoiseSigma_GpsPosVec.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;

%
%    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 = 900;
Sim.Propagator          = ODProp;
Sim.AddNoise            = Off;

%
%   Reject filter
%

Create RejectFilter RF;

RF.DataTypes = {'GPS_PosVec'};

%
%   Estimator
%

Create ExtendedKalmanFilter EKF;

EKF.ShowProgress         = True;
EKF.Measurements         = {EstData};
EKF.Propagator           = ODProp;
EKF.ShowAllResiduals     = Off;
EKF.ProcessNoiseTimeStep = 100;
EKF.DataFilters          = {RF}
EKF.ReportFile           = 'ProcessNoiseModel_AccelNoiseSigma_GpsPosVec.txt';
EKF.MatlabFile           = 'ProcessNoiseModel_AccelNoiseSigma_GpsPosVec.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;