I’m including my code for completeness below, but for the first iteration/evaluation of the BatchLSEstimator if I’m starting with my apriori state as the truth state (same orbit that I used to generate measurements), shouldn’t the residuals be the same order of magnitude as my sigma in the measurements (range rate)? I’m getting residuals that are the same order of magnitude as my measurements. I’m using the same force/drag/atmosphere models when generating my ‘data’ and when running the estimator.
Residual[0] = +25.1879
Residual[1] = +54.8990
Residual[2] = +131.704
Residual[3] = +367.654
Residual[4] = +1026.94
Residual[5] = +1159.12
Residual[6] = +455.001
Residual[7] = +163.948
Residual[8] = +73.1320
Residual[9] = +36.6805
Residual[10] = +359.574
Residual[11] = +611.428
Residual[12] = +1081.79
Residual[13] = +1905.81
Residual[14] = +2929.49
public static void main(String[] args) {
// ===== Setup Orekit
initOrekit.initializeOrekitData();
// ===== Generate initial state from TLE
TLE tle = new TLE(TLE_LINE1, TLE_LINE2);
// Eventually, shift the trueInitialState slightly forward or backward in time from the TLE
SpacecraftState trueInitialState = eme2000InitialStateFromTLE(tle);
AbsoluteDate startDate = trueInitialState.getDate();
AbsoluteDate endDate = startDate.shiftedBy(PROP_TIME_S);
// ===== Define the ground stations
GroundStation gs1 = createGroundStation(GS1_LAT, GS1_LON, GS1_ALT_M, GS1_NAME);
// ===== Create the "TRUE" numerical propagator
// This includes gravity (16,16) and a simplified model for drag (flat plate?)
NumericalPropagator trueProp = buildNumericalPropagator(trueInitialState);
// ===== Generate true range rate measurements
// This ObservableSatellite MUST be reused for the estimator
ObservableSatellite sat = new ObservableSatellite(0);
SortedSet<ObservedMeasurement<?>> measurements = generateRangeRateAboveElevation(
trueProp, gs1, sat, SIGMA_RR, MEASUREMENT_DT, startDate, endDate, GS_ELEV_DEG);
// optional: limit how many you want for OD
List<ObservedMeasurement<?>> selectedMeasurements =
pickFirstN(measurements, 15);
// ===== Create the estimation propagator builder
NumericalPropagatorBuilder builder = buildPropagatorBuilder(trueInitialState);
// ===== Create least squares estimator, add measurements
BatchLSEstimator estimator = initializeEstimator(builder, selectedMeasurements);
BatchLSObserver observer = createObserver(); // output the residual with the measurement and covariance matrix diagonal at each iteration
estimator.setObserver(observer);
// ===== Run estimation
Propagator estimated = estimator.estimate()[0];
SpacecraftState estimatedInitialState = estimated.getInitialState();
// output result
Vector3D deltaPos = trueInitialState.getPVCoordinates().getPosition()
.subtract(estimatedInitialState.getPVCoordinates().getPosition());
Vector3D deltaVel = trueInitialState.getPVCoordinates().getVelocity()
.subtract(estimatedInitialState.getPVCoordinates().getVelocity());
System.out.println("Position difference [m]: " + deltaPos.getNorm());
System.out.println("Velocity difference [m/s]: " + deltaVel.getNorm());
}
/**
* Generate the initial spacecraft state based on feeding the TLE through the TLE propagator.
*
* @param tle
* @return
*/
private static SpacecraftState eme2000InitialStateFromTLE(TLE tle) {
// ---- Create the TLE propagator using the TLE lines
Frames frames = DataContext.getDefault().getFrames();
TLEPropagator SGP4propagator = TLEPropagator.selectExtrapolator(
tle,
InertialProvider.of(frames.getTEME()),
SPACECRAFT_MASS_KG,
frames.getTEME());
// ---- Get the initial state SGP4 osculating state in TEME
AbsoluteDate epoch = tle.getDate();
SpacecraftState initialState_TEME = SGP4propagator.getInitialState();
// ---- Convert the initial state to the EME (Inertial) reference frame
Frame emeFrame = FramesFactory.getEME2000();
Frame teme = initialState_TEME.getFrame();
Transform toEME = teme.getTransformTo(emeFrame, epoch);
PVCoordinates pvEME = toEME.transformPVCoordinates(initialState_TEME.getPVCoordinates());
// ---- Build an orbit with this data
double mu = Constants.EGM96_EARTH_MU;
Orbit initialOrbit = new CartesianOrbit(pvEME, emeFrame, epoch, mu);
// ---- use that orbit to determine the initial state
SpacecraftState initialState_EME = new SpacecraftState(initialOrbit, SPACECRAFT_MASS_KG);
return initialState_EME;
}
/**
* Create the ground station based on the given LLA (degree, degree, m). This object is used to determine
* the range rate.
*
* @param lat_deg
* @param lon_deg
* @param alt_m
* @param name
* @return
*/
private static GroundStation createGroundStation(double lat_deg, double lon_deg, double alt_m, String name) {
// create the ground station geodetic point
GeodeticPoint geo = new GeodeticPoint(FastMath.toRadians(lat_deg), FastMath.toRadians(lon_deg), alt_m);
// Earth ellipsoid in ITRF frame
Frame earthFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true); // Earth-centered Earth-fixed (geodesy)
BodyShape earth = new OneAxisEllipsoid(
Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
Constants.WGS84_EARTH_FLATTENING,
earthFrame);
// create the topocentric frame for the ground station
TopocentricFrame topo = new TopocentricFrame(earth, geo, name);
// create the ground station object
return new GroundStation(topo);
}
/**
* Generate the range rate measurements.
* This uses the numerical propagator to propagate the spacecraft state forward in time. It then uses the
* RangeRateBuilder to determine the range rate at the propagated state. It only generates measurements
* when the spacecraft is above the specified elevation from the ground station.
*
* @param propagator propagator for propagating the orbit/state in time
* @param station ground station
* @param sat satellite that is being propagated
* @param sigmaRR range rate estimated 1 sigma bound noise
* @param step time step
* @param start starting time
* @param end ending time
* @param elevation_deg elevation threshold
* @return measurements
*/
private static SortedSet<ObservedMeasurement<?>> generateRangeRateAboveElevation(
Propagator propagator,
GroundStation station,
ObservableSatellite sat,
double sigmaRR,
double step,
AbsoluteDate start,
AbsoluteDate end,
double elevation_deg) {
// --- Noise generator
double[][] cov = { { sigmaRR * sigmaRR } };
CorrelatedRandomVectorGenerator noise =
new CorrelatedRandomVectorGenerator(
MatrixUtils.createRealMatrix(cov),
1e-10,
new GaussianRandomGenerator(new Well19937a(1234)));
// --- RangeRate builder
// for now, assume perfect measurements to verify estimator
RangeRateBuilder builder = new RangeRateBuilder(noise, station, false, sigmaRR, 1.0, sat);
// --- Generator
Generator gen = new Generator();
gen.addPropagator(propagator);
// Create the elevation detector
TopocentricFrame gsTopo = station.getBaseFrame();
ElevationDetector elevDetector = new ElevationDetector(gsTopo)
.withConstantElevation(FastMath.toRadians(elevation_deg))
.withMaxCheck(1.0)
.withThreshold(1e-6)
.withHandler(new ContinueOnEvent<>());
// ---- Scheduler automatically enforces elevation mask and step
EventBasedScheduler<RangeRate> scheduler = new EventBasedScheduler<>(
builder,
new FixedStepSelector(step, TimeScalesFactory.getUTC()),
propagator,
elevDetector,
SignSemantic.FEASIBLE_MEASUREMENT_WHEN_POSITIVE);
gen.addScheduler(scheduler);
// generate measurements between start and end
SortedSet<ObservedMeasurement<?>> measurements = gen.generate(start, end);
return measurements;
}
/**
* Utility function to select the first N range-rate measurements. For use alongside
* an estimator to determine impact of number of measurements.
*
* @param all SortedSet<ObservedMeasurement<?>> list of full measurements
* @param N number of measurements to pull
* @return first N measurements
*/
private static List<ObservedMeasurement<?>> pickFirstN(
SortedSet<ObservedMeasurement<?>> all, int N) {
List<ObservedMeasurement<?>> out = new ArrayList<>(N);
int i = 0;
for (ObservedMeasurement<?> m : all) {
if (i >= N) break;
out.add(m);
i++;
}
return out;
}
private static NumericalPropagator buildNumericalPropagator(SpacecraftState initState) {
DormandPrince54Integrator integrator =
new DormandPrince54Integrator(MIN_STEP, MAX_STEP, POSITION_TOLERANCE, VELOCITY_TOLERANCE);
NumericalPropagator propagator =
new NumericalPropagator(integrator);
// ---- Reference frames
Frame emeFrame = FramesFactory.getEME2000(); // Inertial Earth-centered, Earth-fixed aligned with ICRF (satellite tracking)
Frame earthFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true); // Earth-centered Earth-fixed (geodesy)
// ---- Earth Shape and Sun PVC
OneAxisEllipsoid earth = new OneAxisEllipsoid(
Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
Constants.WGS84_EARTH_FLATTENING,
earthFrame);
PVCoordinatesProvider sun = CelestialBodyFactory.getSun();
// ---- Use Earth gravity truncated to J2, J3, J4 only to align with SGP4
NormalizedSphericalHarmonicsProvider gravity =
GravityFieldFactory.getNormalizedProvider(16, 16);
ForceModel gravityModel = new HolmesFeatherstoneAttractionModel(earthFrame, gravity);
// ---- Account for drag with Cd, A, and real density
NRLMSISE00InputParameters weatherData = new CssiSpaceWeatherData("SpaceWeather-All.*\\.txt");
Atmosphere nrlAtmosphere = new NRLMSISE00(weatherData, sun, earth);
DragSensitive spacecraft = new IsotropicDrag(SPACECRAFT_CROSSSECTION_M2, SPACECRAFT_CD);
DragForce dragModel = new DragForce(nrlAtmosphere, spacecraft);
// ---- Optional, lock nadir pointing
NadirPointing nadirLaw = new NadirPointing(emeFrame, earth);
propagator.setAttitudeProvider(nadirLaw);
propagator.addForceModel(gravityModel);
propagator.addForceModel(dragModel);
propagator.setInitialState(initState);
return propagator;
}
public static NumericalPropagatorBuilder buildPropagatorBuilder(SpacecraftState initState) {
Orbit initOrbit = initState.getOrbit();
ODEIntegratorBuilder integratorBuilder =
new DormandPrince54IntegratorBuilder(MIN_STEP, MAX_STEP, POSITION_TOLERANCE);
NumericalPropagatorBuilder propagatorBuilder = new NumericalPropagatorBuilder(
initOrbit,
integratorBuilder,
PositionAngle.MEAN,
1.0);
// ---- Reference frames
Frame emeFrame = FramesFactory.getEME2000(); // Inertial Earth-centered, Earth-fixed aligned with ICRF (satellite tracking)
Frame earthFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true); // Earth-centered Earth-fixed (geodesy)
// ---- Earth Shape and Sun PVC
OneAxisEllipsoid earth = new OneAxisEllipsoid(
Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
Constants.WGS84_EARTH_FLATTENING,
earthFrame);
PVCoordinatesProvider sun = CelestialBodyFactory.getSun();
// ---- Use Earth gravity truncated to J2, J3, J4 only to align with SGP4
NormalizedSphericalHarmonicsProvider gravity =
GravityFieldFactory.getNormalizedProvider(16, 16);
ForceModel gravityModel = new HolmesFeatherstoneAttractionModel(earthFrame, gravity);
// ---- Account for drag with Cd, A, and real density
NRLMSISE00InputParameters weatherData = new CssiSpaceWeatherData("SpaceWeather-All.*\\.txt");
Atmosphere nrlAtmosphere = new NRLMSISE00(weatherData, sun, earth);
DragSensitive spacecraft = new IsotropicDrag(SPACECRAFT_CROSSSECTION_M2, SPACECRAFT_CD);
DragForce dragModel = new DragForce(nrlAtmosphere, spacecraft);
// ---- Optional, lock nadir pointing
NadirPointing nadirLaw = new NadirPointing(emeFrame, earth);
propagatorBuilder.setAttitudeProvider(nadirLaw);
propagatorBuilder.addForceModel(gravityModel);
propagatorBuilder.addForceModel(dragModel);
return propagatorBuilder;
}
/**
* Initialize the estimator used for the orbit determination.
* @param inputData input data
* @param propagator orbit propagator
* @param initialGuess intial guess of the orbit determination
* @param measurements measurements
* @return a configured estimator
*/
private static BatchLSEstimator initializeEstimator(final NumericalPropagatorBuilder propagator,
final List<ObservedMeasurement<?>> measurements) {
// Optimizer
GaussNewtonOptimizer optimizer = new GaussNewtonOptimizer(new QRDecomposer(1e-11), false);
// Estimator
BatchLSEstimator estimator = new BatchLSEstimator(optimizer, propagator);
double convergenceThreshold = 1.0e-3;
int maxIterations = 10;
int maxEvaluations = 20;
estimator.setParametersConvergenceThreshold(convergenceThreshold);
estimator.setMaxIterations(maxIterations);
estimator.setMaxEvaluations(maxEvaluations);
// Add the measurements to the estimator
for (ObservedMeasurement<?> measurement : measurements) {
estimator.addMeasurement(measurement);
}
// Return the configured estimator
return estimator;
}
private static BatchLSObserver createObserver() {
BatchLSObserver observer = new BatchLSObserver() {
@Override
public void evaluationPerformed(int iteration,
int evalCount,
Orbit[] estimatedOrbits,
ParameterDriversList orbitalParams,
ParameterDriversList propagationParams,
ParameterDriversList measurementParams,
EstimationsProvider estimations,
Evaluation evalInfo) {
System.out.println("\n=== Iteration " + iteration +
" Evaluation " + evalCount + " ===");
// Print RMS
System.out.println("RMS: " + evalInfo.getRMS());
// Access residuals via the evaluation object
double[] residuals = evalInfo.getResiduals().toArray();
for (int i = 0; i < residuals.length; i++) {
System.out.printf("Residual[%d] = %+g%n", i, residuals[i]);
}
// Covariance (diagonal)
RealMatrix cov = evalInfo.getCovariances(1e-10);
if (cov != null) {
System.out.println("Covariance diagonal:");
for (int i = 0; i < cov.getRowDimension(); i++) {
System.out.printf(" σ[%d]^2 = %g%n", i, cov.getEntry(i, i));
}
}
}
};
return observer;
}