%
%   FilterSmoother_GpsPosVec
%
%   Check formatting and content of the EKF MATLAB file.
%
%   02/21/2019 SES Initial version
%

% Earth.EopFileName = 'G:\GMAT\Testing\files\eopc04';

%
%   Spacecraft
%

Create Spacecraft SimSat;

SimSat.DateFormat        = UTCGregorian;
SimSat.Epoch             = '10 Jun 2019 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 2019 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 1.0e-6 1.0e-6];

%
%   Spacecraft hardware
%

Create Antenna GpsAntenna;
Create Receiver GpsReceiver;

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

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                = {'KalmanSmoother_MatlabFile_GpsPosVec.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       = {{EstSat.GpsReceiver}, 'GPS_PosVec'};
EstData.FileName                = {'KalmanSmoother_MatlabFile_GpsPosVec.gmd'};
EstData.RampTable               = {};
EstData.UseLightTime            = False;
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;

Create EphemerisFile FilterEphemFile;

FilterEphemFile.Spacecraft          = EstSat;
FilterEphemFile.Filename            = 'GpsPosVec_Filter.e';
FilterEphemFile.FileFormat          = STK-TimePosVel;
FilterEphemFile.CoordinateSystem    = EarthMJ2000Eq;
FilterEphemFile.IncludeCovariance   = PositionAndVelocity;
% FilterEphemFile.WriteEphemeris      = True;
FilterEphemFile.WriteEphemeris      = False;

Create EphemerisFile SmootherEphemFile;

SmootherEphemFile.Spacecraft        = EstSat;
SmootherEphemFile.Filename          = 'GpsPosVec_Smoother.e';
SmootherEphemFile.FileFormat        = STK-TimePosVel;
SmootherEphemFile.CoordinateSystem  = EarthMJ2000Eq;
SmootherEphemFile.IncludeCovariance = PositionAndVelocity;
SmootherEphemFile.WriteEphemeris    = False;

%
%    Simulator
%

Create Simulator Sim;

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

%
%   Reject filter
%

Create RejectFilter RF;

RF.EpochFormat  = 'UTCGregorian'
RF.InitialEpoch = '10 Jun 2019 00:30:00.000';
RF.FinalEpoch   = '10 Jun 2019 00:40:00.000';

%
%   Estimator
%

Create ExtendedKalmanFilter EKF;

EKF.ShowProgress         = True;
EKF.ReportFile           = 'GpsPosVec_Filter.txt';
EKF.MatlabFile           = 'FilterSmoother_GpsPosVec.mat';
EKF.Measurements         = {EstData};
EKF.Propagator           = ODProp;
EKF.ShowAllResiduals     = Off;
EKF.ProcessNoiseTimeStep = 240;
EKF.DataFilters          = {}
EKF.PredictTimeSpan      = 864000;

Create Smoother FPS;

FPS.Filter               = EKF;
FPS.ShowProgress         = True;
FPS.ShowAllResiduals     = Off;
FPS.ReportFile           = 'GpsPosVec_Smoother.txt';
FPS.MatlabFile           = 'FilterSmoother_GpsPosVec.mat';
FPS.ShowAllResiduals     = Off;
FPS.DelayRectifyTimeSpan = 7200.
FPS.PredictTimeSpan      = 864000;
FPS.ReportStyle          = 'Verbose'

%
%   Run mission sequence
%

BeginMissionSequence

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

SetSeed(1);

Toggle FilterEphemFile On

RunSimulator Sim;
RunEstimator EKF;

Toggle FilterEphemFile Off
Toggle SmootherEphemFile On

RunSmoother FPS;

% Need to togle the filter ephem back on to actually write out the file
Toggle FilterEphemFile On