def get_propagator(cartesian_orbit, ecef, wgs84ellipsoid):#,builder=False): integratorBuilder = DormandPrince853IntegratorBuilder(PROP_MIN_STEP, PROP_MAX_STEP, PROP_POSITION_ERROR) propagatorBuilder = NumericalPropagatorBuilder(cartesian_orbit, integratorBuilder, PositionAngleType.MEAN, ESTIMATOR_POSITION_SCALE) gravityProvider = GravityFieldFactory.getConstantNormalizedProvider(GRAVITY_DEGREE, GRAVITY_ORDER,cartesian_orbit.getDate()) gravityAttractionModel = HolmesFeatherstoneAttractionModel(ecef, gravityProvider) propagatorBuilder.addForceModel(gravityAttractionModel) if MOON_3D_BODY: moon = CelestialBodyFactory.getMoon() moon_3dbodyattraction = ThirdBodyAttraction(moon) propagatorBuilder.addForceModel(moon_3dbodyattraction) sun = CelestialBodyFactory.getSun() if SUN_3D_BODY: sun_3dbodyattraction = ThirdBodyAttraction(sun) propagatorBuilder.addForceModel(sun_3dbodyattraction) if SOLAR_RAD_PRESSURE: isotropicRadiationSingleCoeff = IsotropicRadiationSingleCoefficient(CS,CR) solarRadiationPressure = SolarRadiationPressure(sun, wgs84ellipsoid.getEquatorialRadius(), isotropicRadiationSingleCoeff) propagatorBuilder.addForceModel(solarRadiationPressure) if ATM_DRAG: msafe = MarshallSolarActivityFutureEstimation( MarshallSolarActivityFutureEstimation.DEFAULT_SUPPORTED_NAMES, MarshallSolarActivityFutureEstimation.StrengthLevel.AVERAGE) atmosphere = NRLMSISE00(msafe, sun, wgs84ellipsoid) isotropicDrag = IsotropicDrag(CS, CD) dragForce = DragForce(atmosphere, isotropicDrag) propagatorBuilder.addForceModel(dragForce) if RELATIVITY: from org.orekit.forces.gravity import Relativity relativity = Relativity(orekit_constants.EIGEN5C_EARTH_MU) propagatorBuilder.addForceModel(relativity) return propagatorBuilder def GetInitialOrbit(e,p,v): epoch = datetime_to_absolutedate(e.replace(tzinfo=utc)) position = Vector3D(float(p[0]), float(p[1]), float(p[2])) velocity = Vector3D(float(v[0]), float(v[1]), float(v[2])) pv_coordinates = PVCoordinates(position, velocity) eci = FramesFactory.getGCRF() initial_orbit = CartesianOrbit(pv_coordinates, eci, epoch, Constants.IERS2010_EARTH_MU) return initial_orbit class MyObserver(PythonKalmanObserver): def __init__(self): super(MyObserver,self).__init__() self.Kalmanestimation = None def evaluationPerformed(self,estimation): self.Kalmanestimation = estimation def getCov(self): InnCov = self.Kalmanestimation.getPhysicalInnovationCovarianceMatrix() K = self.Kalmanestimation.getPhysicalKalmanGain() InnCov = K.multiply(InnCov.multiply(K.transpose())) UpdatedCov = self.Kalmanestimation.getPhysicalEstimatedCovarianceMatrix() return InnCov, UpdatedCov def getSTM(self): return self.Kalmanestimation.getPhysicalStateTransitionMatrix() def PropagateCovari(t0,pv,Cov,tf, IdStation='Dummy', lat=0., lon=0., alt=0., RADEC=[0.,0.], sigmaRADEC=[1e-12,1e-12]): Coveci = Array2DRowRealMatrix(6,6) for i in range(6) : Coveci.setRow(i,[float(i*Fact**2) for i in Cov[i]]) Orbit = GetInitialOrbit(t0,pv[:3],pv[-3:]) NewEpoch = datetime_to_absolutedate(tf.replace(tzinfo=utc)) ecef = FramesFactory.getITRF(ITRFVersion.ITRF_2014, IERSConventions.IERS_2010, False) wgs84ellipsoid = ReferenceEllipsoid.getWgs84(ecef) geodetic_point = GeodeticPoint(float(np.deg2rad(lat)), float(np.deg2rad(lon)), float(alt)) topocentric_frame = TopocentricFrame(wgs84ellipsoid, geodetic_point, IdStation) sta = GroundStation(topocentric_frame) wgs84ellipsoid = ReferenceEllipsoid.getWgs84(ecef) Q = MatrixUtils.createRealDiagonalMatrix([1e-6, 1e-6, 1e-6, 1e-9, 1e-9, 1e-9]) Noise = ConstantProcessNoise(Coveci, Q) propBuilder = get_propagator(Orbit, ecef, wgs84ellipsoid) prop = propBuilder.buildPropagator([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) satellite = Generator().addPropagator(prop) KalmanBuilder = KalmanEstimatorBuilder().addPropagationConfiguration(propBuilder, Noise) estimator = KalmanBuilder.build() Kalmanobserver = MyObserver() estimator.setObserver(Kalmanobserver) RADEC = [float(np.deg2rad(rd)) for rd in RADEC] sigmaRADEC = [float(np.deg2rad(rd)) for rd in sigmaRADEC] RADEC = AngularRaDec(sta,sta.getBaseFrame(),NewEpoch,RADEC,sigmaRADEC,[1.0,1.0],satellite) estimator.estimationStep(RADEC) STM = Kalmanobserver.getSTM() PropCov = STM.multiply(Coveci).multiply(STM.transpose()) PropCov = np.array([[PropCov.getEntry(i,j) for i in range(6)] for j in range(6)]) Inov_cov_eci,Updated_cov_eci = Kalmanobserver.getCov() Predicted_cov_eci = Updated_cov_eci.add(Inov_cov_eci) Inov_cov_eci = np.array([[Inov_cov_eci.getEntry(i,j) for i in range(6)] for j in range(6)]) Predicted_cov_eci = np.array([[Predicted_cov_eci.getEntry(i,j) for i in range(6)] for j in range(6)]) return PropCov, Inov_cov_eci, Predicted_cov_eci