/* Copyright 2002-2017 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.measurements;
import java.util.List;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.stat.descriptive.rank.Median;
import org.hipparchus.util.FastMath;
import org.junit.Assert;
import org.junit.Test;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.Context;
import org.orekit.estimation.EstimationTestUtils;
import org.orekit.estimation.EstimationUtils;
import org.orekit.estimation.ParameterFunction;
import org.orekit.estimation.StateFunction;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
public class AngularTest {
@Test
public void testStateDerivatives() throws OrekitException {
Context context = EstimationTestUtils.geoStationnaryContext();
final NumericalPropagatorBuilder propagatorBuilder =
context.createBuilder(OrbitType.EQUINOCTIAL, PositionAngle.TRUE, false,
1.0e-6, 60.0, 0.001);
// create perfect azimuth-elevation measurements
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
propagatorBuilder);
final List<ObservedMeasurement<?>> measurements =
EstimationTestUtils.createMeasurements(propagator,
new AngularMeasurementCreator(context),
0.25, 3.0, 600.0);
propagator.setSlaveMode();
// Compute measurements.
double[] AzerrorsP = new double[3 * measurements.size()];
double[] AzerrorsV = new double[3 * measurements.size()];
double[] ElerrorsP = new double[3 * measurements.size()];
double[] ElerrorsV = new double[3 * measurements.size()];
int AzindexP = 0;
int AzindexV = 0;
int ElindexP = 0;
int ElindexV = 0;
for (final ObservedMeasurement<?> measurement : measurements) {
// parameter corresponding to station position offset
final GroundStation stationParameter = ((Angular) measurement).getStation();
// We intentionally propagate to a date which is close to the
// real spacecraft state but is *not* the accurate date, by
// compensating only part of the downlink delay. This is done
// in order to validate the partial derivatives with respect
// to velocity. If we had chosen the proper state date, the
// angular would have depended only on the current position but
// not on the current velocity.
final AbsoluteDate datemeas = measurement.getDate();
SpacecraftState state = propagator.propagate(datemeas);
final Vector3D stationP = stationParameter.getOffsetFrame().getPVCoordinates(datemeas, state.getFrame()).getPosition();
final double meanDelay = stationParameter.signalTimeOfFlight(state.getPVCoordinates(), stationP, datemeas);
final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
state = propagator.propagate(date);
final double[][] jacobian = measurement.estimate(0, 0, state).getStateDerivatives();
// compute a reference value using finite differences
final double[][] finiteDifferencesJacobian =
EstimationUtils.differentiate(new StateFunction() {
public double[] value(final SpacecraftState state) throws OrekitException {
return measurement.estimate(0, 0, state).getEstimatedValue();
}
}, measurement.getDimension(), OrbitType.CARTESIAN,
PositionAngle.TRUE, 250.0, 4).value(state);
Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
final double smallest = FastMath.ulp((double) 1.0);
for (int i = 0; i < jacobian.length; ++i) {
for (int j = 0; j < jacobian[i].length; ++j) {
double relativeError = FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
finiteDifferencesJacobian[i][j]);
if ((FastMath.sqrt(finiteDifferencesJacobian[i][j]) < smallest) && (FastMath.sqrt(jacobian[i][j]) < smallest) ){
relativeError = 0.0;
}
if (j < 3) {
if (i == 0) {
AzerrorsP[AzindexP++] = relativeError;
} else {
ElerrorsP[ElindexP++] = relativeError;
}
} else {
if (i == 0) {
AzerrorsV[AzindexV++] = relativeError;
} else {
ElerrorsV[ElindexV++] = relativeError;
}
}
}
}
}
// median errors on Azimuth
Assert.assertEquals(0.0, new Median().evaluate(AzerrorsP), 5.0e-6);
Assert.assertEquals(0.0, new Median().evaluate(AzerrorsV), 6.3e-5);
// median errors on Elevation
Assert.assertEquals(0.0, new Median().evaluate(ElerrorsP), 5.0e-6);
Assert.assertEquals(0.0, new Median().evaluate(ElerrorsV), 2.0e-5);
}
@Test
public void testParameterDerivatives() throws OrekitException {
Context context = EstimationTestUtils.geoStationnaryContext();
final NumericalPropagatorBuilder propagatorBuilder =
context.createBuilder(OrbitType.EQUINOCTIAL, PositionAngle.TRUE, false,
1.0e-6, 60.0, 0.001);
// create perfect azimuth-elevation measurements
for (final GroundStation station : context.stations) {
station.getEastOffsetDriver().setSelected(true);
station.getNorthOffsetDriver().setSelected(true);
station.getZenithOffsetDriver().setSelected(true);
}
final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
propagatorBuilder);
final List<ObservedMeasurement<?>> measurements =
EstimationTestUtils.createMeasurements(propagator,
new AngularMeasurementCreator(context),
0.25, 3.0, 600.0);
propagator.setSlaveMode();
for (final ObservedMeasurement<?> measurement : measurements) {
// parameter corresponding to station position offset
final GroundStation stationParameter = ((Angular) measurement).getStation();
// We intentionally propagate to a date which is close to the
// real spacecraft state but is *not* the accurate date, by
// compensating only part of the downlink delay. This is done
// in order to validate the partial derivatives with respect
// to velocity. If we had chosen the proper state date, the
// angular would have depended only on the current position but
// not on the current velocity.
final AbsoluteDate datemeas = measurement.getDate();
final SpacecraftState stateini = propagator.propagate(datemeas);
final Vector3D stationP = stationParameter.getOffsetFrame().getPVCoordinates(datemeas, stateini.getFrame()).getPosition();
final double meanDelay = stationParameter.signalTimeOfFlight(stateini.getPVCoordinates(), stationP, datemeas);
final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
final SpacecraftState state = propagator.propagate(date);
final ParameterDriver[] drivers = new ParameterDriver[] {
stationParameter.getEastOffsetDriver(),
stationParameter.getNorthOffsetDriver(),
stationParameter.getZenithOffsetDriver()
};
for (int i = 0; i < 3; ++i) {
final double[] gradient = measurement.estimate(0, 0, state).getParameterDerivatives(drivers[i]);
Assert.assertEquals(2, measurement.getDimension());
Assert.assertEquals(2, gradient.length);
for (final int k : new int[] {0, 1}) {
final ParameterFunction dMkdP =
EstimationUtils.differentiate(new ParameterFunction() {
/** {@inheritDoc} */
@Override
public double value(final ParameterDriver parameterDriver) throws OrekitException {
return measurement.estimate(0, 0, state).getEstimatedValue()[k];
}
}, drivers[i], 3, 50.0);
final double ref = dMkdP.value(drivers[i]);
if (ref > 1.e-12) {
Assert.assertEquals(ref, gradient[k], 1e-5 * FastMath.abs(ref));
}
}
}
}
}
}