%
%   Fermi OD using on-board GPS point-solutions
%

SolarSystem.EphemerisSource = 'DE421';
Luna.EquatorialRadius = 1738.2e-30;
Earth.EquatorialRadius = 1738.2e-30;
%setting Earth and Moon radius so that the sun is not occulted.  Want to see full SRP affect.  
%if this doesnt work, we can change s/c to orbit Sun instead of Earth.     
	
	
%
%   Spacecraft
%

Create Spacecraft SimFermi;
SimFermi.DateFormat         = UTCGregorian;
SimFermi.Epoch              = '01 Mar 2013 00:00:00.000';
SimFermi.CoordinateSystem   = EarthMJ2000Eq;
SimFermi.DisplayStateType   = Cartesian;
SimFermi.X                  = 5348.851282     
SimFermi.Y                  = 4375.389181     
SimFermi.Z                  = -189.427745      
SimFermi.VX                 = -4.410259       
SimFermi.VY                 =  5.257511      
SimFermi.VZ                 = -3.272891
SimFermi.DryMass            = 4357.33;
SimFermi.Cd                 = 2.1;
SimFermi.Cr                 = 0.75e70;    %this shouldn't be used, can make it large for test purposes
SimFermi.DragArea           = 14.18;
SimFermi.SRPArea            = 1e70;       %this shouldn't be used, can make it large for test purposes
SimFermi.Id                 = 'Fermi';
SimFermi.AddHardware        = {GpsReceiver, GpsAntenna};
SimFermi.Attitude 			 = NadirPointing;
SimFermi.AttitudeReferenceBody = Sun;
SimFermi.AttitudeConstraintType = 'OrbitNormal';
SimFermi.BodyAlignmentVectorX = 1;
SimFermi.BodyAlignmentVectorY = 0;
SimFermi.BodyAlignmentVectorZ = 0;
SimFermi.BodyConstraintVectorX = 0;
SimFermi.BodyConstraintVectorY = 0;
SimFermi.BodyConstraintVectorZ = 1;
%above attitude should point X body axis towards Sun.  
%the angle of the s/c orbit normal (consider s/c in orbit about Sun) and the Z body axis is minimized.
%if this doesnt work, just change the s/c to orbit the Sun


Create Spacecraft Fermi;
Fermi.DateFormat         = UTCGregorian;
Fermi.Epoch              = '01 Mar 2013 00:00:00.000';
Fermi.CoordinateSystem   = EarthMJ2000Eq;
Fermi.DisplayStateType   = Cartesian;
Fermi.X                  = 5348.851282     
Fermi.Y                  = 4375.389181     
Fermi.Z                  = -189.427745      
Fermi.VX                 = -4.410259       
Fermi.VY                 =  5.257511      
Fermi.VZ                 = -3.272891
Fermi.DryMass            = 4357.33;
Fermi.Cd                 = 2.1;
Fermi.Cr                 = 0.75;
Fermi.DragArea           = 14.18;
Fermi.SRPArea            = 14.18;  
Fermi.Id                 = 'Fermi';
Fermi.AddHardware        = {GpsReceiver, GpsAntenna};
Fermi.SolveFors          = {CartesianState}
Fermi.Attitude 			 = NadirPointing;
Fermi.AttitudeReferenceBody = Sun;
Fermi.AttitudeConstraintType = 'OrbitNormal';
Fermi.BodyAlignmentVectorX = 1;
Fermi.BodyAlignmentVectorY = 0;
Fermi.BodyAlignmentVectorZ = 0;
Fermi.BodyConstraintVectorX = 0;
Fermi.BodyConstraintVectorY = 0;
Fermi.BodyConstraintVectorZ = 1;



Create Plate BP1 BP2 BP3 BP4 BP5 BP6;    %BP = Body plate
SimFermi.AddPlates = {BP1, BP2, BP3, BP4, BP5, BP6}; 
Fermi.AddPlates = {BP1, BP2, BP3, BP4, BP5, BP6}; 
% 
% SimFermi.NPlateSRPAddEqualityConstraint     = {BP1, BP2, BP3, BP4, BP5, BP6}  
% Fermi.NPlateSRPAddEqualityConstraint        = {BP1, BP2, BP3, BP4, BP5, BP6}  

Fermi.NPlateSRPAddEqualityConstraint =...
{BP1.AreaCoefficient, BP2.AreaCoefficient, BP3.AreaCoefficient, BP4.AreaCoefficient, BP5.AreaCoefficient, BP6.AreaCoefficient}

SimFermi.NPlateSRPAddEqualityConstraint =...
{BP1.AreaCoefficient, BP2.AreaCoefficient, BP3.AreaCoefficient, BP4.AreaCoefficient, BP5.AreaCoefficient, BP6.AreaCoefficient}

 
#Include 'C:\Users\dcooley\Documents\GMAT\Nav\Scripts\Fermi_Test_Nplate\FromSES_2019_08_06\fermi\fermi\input\PlateProperties.txt'

BP1.Area = 500;
BP1.SpecularFraction = 1.0;
BP1.DiffuseFraction = 0.0;
% 
%The S/C attitude points the X axis towards the Sun.
%The orbit obtained should be identical to a canon-ball
%SRP model, Cr=2.0, with a cross sectional area of 500 m^2
%and a mass of 4357.33 kg.



%
%   Spacecraft hardware
%

Create Antenna GpsAntenna;
Create Receiver GpsReceiver;

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

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

%
%   Force model and propagator
%

Create ForceModel FM;
Create Propagator Prop;

FM.CentralBody                       = Earth;
FM.GravityField.Earth.EarthTideModel = 'None';
FM.PointMasses                       = {Earth};
FM.SRP                               = On;
FM.SRP.SRPModel 			 			 	 = NPlate
FM.Drag.AtmosphereModel              = 'None'
FM.ErrorControl                      = 'None';

Prop.FM                              = FM;
Prop.Type                            = RungeKutta89;
Prop.InitialStepSize                 = 5;  %set small to make sure attitude is more accurate
Prop.Accuracy                        = 1e-13;
Prop.MinStep                         = 0;
Prop.MaxStep                         = 60;
Prop.MaxStepAttempts                 = 50;

%
%   Tracking data selection
%

Create TrackingFileSet SimGpsPosVec;

SimGpsPosVec.FileName                = {'fermi_SimEst.gmd'};
SimGpsPosVec.AddTrackingConfig       = {{SimFermi.GpsReceiver}, 'GPS_PosVec'};
SimGpsPosVec.RampTable               = {};
SimGpsPosVec.UseLightTime            = False;
SimGpsPosVec.UseRelativityCorrection = False;
SimGpsPosVec.UseETminusTAI           = False;
SimGpsPosVec.DataFilters             = {};


Create TrackingFileSet GpsPosVec;
GpsPosVec.FileName                = {'fermi_SimEst.gmd'};
GpsPosVec.AddTrackingConfig       = {{Fermi.GpsReceiver}, 'GPS_PosVec'};
GpsPosVec.RampTable               = {};
GpsPosVec.UseLightTime            = False;
GpsPosVec.UseRelativityCorrection = False;
GpsPosVec.UseETminusTAI           = False;
GpsPosVec.DataFilters             = {};

%
%   Configure Simulator and BLS
%

Create Simulator Sim;
Sim.AddData                     = {SimGpsPosVec};
Sim.EpochFormat                 = 'UTCGregorian';
Sim.InitialEpoch                = '01 Mar 2013 00:00:00.000';
Sim.FinalEpoch                  = '01 Mar 2013 18:00:00.000';
Sim.MeasurementTimeStep         = 60;
Sim.Propagator                  = Prop;
Sim.AddNoise                    = On;

Create BatchEstimatorInv Bls;
Bls.ShowProgress                = True;
Bls.Measurements                = {GpsPosVec};
Bls.AbsoluteTol                 = 0.0001;
Bls.RelativeTol                 = 0.0005;
Bls.MaximumIterations           = 10;
Bls.MaxConsecutiveDivergences   = 4;
Bls.Propagator                  = Prop;
Bls.ShowAllResiduals            = On;
Bls.OLSEInitialRMSSigma         = 3000;
Bls.OLSEUseRMSP                 = False;
Bls.OLSEMultiplicativeConstant  = 3;
Bls.OLSEAdditiveConstant        = 0;
Bls.InversionAlgorithm          = 'Internal';
Bls.EstimationEpochFormat       = 'FromParticipants';
Bls.EstimationEpoch             = 'FromParticipants'; 
Bls.ReportStyle                 = 'Normal';
Bls.ReportFile                  = 'C:\Users\dcooley\Documents\GMAT\Nav\Scripts\Fermi_Test_Nplate\FromSES_2019_08_06\fermi\fermi\output\NPlateFermi_SimEst.txt';

%
%   Mission sequence
%

BeginMissionSequence
SetSeed(42)

RunSimulator Sim
RunEstimator Bls;
