From 0f70402c10d53316e731227d29bfee984d081a28 Mon Sep 17 00:00:00 2001 From: Rahil Makadia Date: Tue, 17 Sep 2024 17:24:22 -0500 Subject: [PATCH] add delta-v vector estimation --- grss/fit/fit_simulation.py | 15 +++++++++------ src/simulation.cpp | 37 ++++++++++++++++++++++++++++++++----- 2 files changed, 41 insertions(+), 11 deletions(-) diff --git a/grss/fit/fit_simulation.py b/grss/fit/fit_simulation.py index 710b4bf9..6aa92dc8 100644 --- a/grss/fit/fit_simulation.py +++ b/grss/fit/fit_simulation.py @@ -843,7 +843,7 @@ def _get_prop_sim_past(self, name, t_eval_utc, eval_apparent_state, """ # pylint: disable=no-member t_eval_past = self.obs.obsTimeMJD.values[self.past_obs_idx] - tf_past = np.min(t_eval_past) + tf_past = np.min(self.obs.obsTimeMJDTDB.values[self.past_obs_idx]) prop_sim_past = libgrss.PropSimulation(name, self.t_sol, self.de_kernel, self.de_kernel_path) prop_sim_past.tEvalMargin = 1.0 @@ -887,7 +887,7 @@ def _get_prop_sim_future(self, name, t_eval_utc, eval_apparent_state, """ # pylint: disable=no-member t_eval_future = self.obs.obsTimeMJD.values[self.future_obs_idx] - tf_future = np.max(t_eval_future) + tf_future = np.max(self.obs.obsTimeMJDTDB.values[self.future_obs_idx]) prop_sim_future = libgrss.PropSimulation(name, self.t_sol, self.de_kernel, self.de_kernel_path) prop_sim_future.tEvalMargin = 1.0 @@ -1340,7 +1340,7 @@ def _inflate_uncertainties(self, prop_sim_past, prop_sim_future): radius_nonzero = self.fixed_propsim_params['radius'] != 0.0 fac = 0.0 if radius_nonzero: - rel_dists = np.linalg.norm(np.array(state_eval)[:, :3], axis=1) + rel_dists = np.linalg.norm([state[:3] for state in state_eval], axis=1) lmbda = 0.3 # from fuentes-munoz et al. 2024 au2m = 1.495978707e11 fac = (lmbda*self.fixed_propsim_params['radius']/au2m/rel_dists*180/np.pi*3600)**2 @@ -1412,7 +1412,8 @@ def _get_analytic_partials(self, prop_sim_past, prop_sim_future): size = 1 else: raise ValueError("Observer info length not recognized.") - part = np.zeros((size, self.n_fit)) + # part is partial of observable with respect to state at observation time + part = np.zeros((size, 6)) if self.past_obs_exist and i < len_past_idx: sim_idx = i optical_partials = past_optical_partials @@ -1423,14 +1424,16 @@ def _get_analytic_partials(self, prop_sim_past, prop_sim_future): optical_partials = future_optical_partials radar_partials = future_radar_partials state = future_states - stm = libgrss.reconstruct_stm(state[sim_idx][6:]) + # stm is partial of state at observation time with respect to nominal state + stm = libgrss.reconstruct_stm(state[sim_idx][6:])[:6] if is_optical: part[0, :6] = optical_partials[sim_idx, :6]*cos_dec[i] part[1, :6] = optical_partials[sim_idx, 6:12] else: part[0, :6] = radar_partials[sim_idx, :6] + # partial is partial of observable with respect to nominal state partial = part @ stm - partials[partials_idx:partials_idx+size, :] = partial + partials[partials_idx:partials_idx+size, :partial.shape[1]] = partial partials_idx += size return partials diff --git a/src/simulation.cpp b/src/simulation.cpp index 2c75429c..b4ce10c7 100644 --- a/src/simulation.cpp +++ b/src/simulation.cpp @@ -340,17 +340,26 @@ void IntegBody::prepare_stm(){ * @param[inout] xInteg State of the body. * @param[in] propDir Direction of propagation. */ -void Event::apply_impulsive(const real& t, std::vector& xInteg, - const real& propDir) { +void Event::apply_impulsive(PropSimulation *propSim, const real &t, std::vector& xInteg) { if (t != this->t) { throw std::runtime_error( "Event::apply_impulsive: Integration time does " "not match event time. Cannot apply impulse."); } size_t velStartIdx = this->xIntegIndex + 3; + bool forwardProp = propSim->integParams.tf > propSim->integParams.t0; + real propDir = forwardProp ? 1.0L : -1.0L; for (size_t i = 0; i < 3; i++) { xInteg[velStartIdx + i] += propDir * this->multiplier * this->deltaV[i]; } + if (propSim->integBodies[this->bodyIndex].propStm) { + IntegBody *body = &propSim->integBodies[this->bodyIndex]; + const size_t numNongravs = body->ngParams.a1Est + body->ngParams.a2Est + body->ngParams.a3Est; + const size_t DeltaVStmIdx = this->xIntegIndex + 6 + 36 + 6*numNongravs; + xInteg[DeltaVStmIdx+3] = forwardProp ? 1.0L : -1.0L; + xInteg[DeltaVStmIdx+10] = forwardProp ? 1.0L : -1.0L; + xInteg[DeltaVStmIdx+17] = forwardProp ? 1.0L : -1.0L; + } } /** @@ -748,7 +757,7 @@ void PropSimulation::prepare_for_evaluation( // sort tEval into ascending order or descending order based on the // integration direction (not during orbit fits) if (observerInfo.size() == 0) { - std::vector tEvalSortedIdx(tEval.size()); + std::vector tEvalSortedIdx(tEval.size()); sort_vector(tEval, forwardProp, tEvalSortedIdx); } if (forwardProp) { @@ -986,7 +995,7 @@ void PropSimulation::remove_body(std::string name) { std::cout << "Error: Body " << name << " not found." << std::endl; } -size_t event_preprocess(PropSimulation *propSim, const IntegBody &body, +static size_t event_preprocess(PropSimulation *propSim, const IntegBody &body, const real &tEvent) { // check if tEvent is valid const bool forwardProp = propSim->integParams.tf > propSim->integParams.t0; @@ -1016,7 +1025,21 @@ size_t event_preprocess(PropSimulation *propSim, const IntegBody &body, return bodyIndex; } -void event_postprocess(PropSimulation *propSim, Event &event) { +static void event_stm_handling(PropSimulation *propSim, IntegBody *body, const Event &event){ + body->n2Derivs += 3*3; + propSim->integBodies[event.bodyIndex].n2Derivs += 3*3; + propSim->integParams.n2Derivs += 3*3; + + const bool backwardProp = propSim->integParams.tf < propSim->integParams.t0; + std::vector extraVec = std::vector(6*3, 0.0); + // add extra vector at the end of stm vector + for (size_t i = 0; i < extraVec.size(); i++) { + body->stm.push_back(extraVec[i]); + propSim->integBodies[event.bodyIndex].stm.push_back(extraVec[i]); + } +} + +static void event_postprocess(PropSimulation *propSim, Event &event) { // assign event xIntegIndex event.xIntegIndex = 0; for (size_t i = 0; i < event.bodyIndex; i++) { @@ -1059,6 +1082,10 @@ void PropSimulation::add_event(IntegBody body, real tEvent, event.bodyIndex = bodyIndex; event.deltaV = deltaV; event.multiplier = multiplier; + // modify stm if estimating Delta-V + if (body.propStm) { + event_stm_handling(this, &body, event); + } event.tau = 0.0; event.isContinuous = false;