add delta-v vector estimation

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
 

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<real>& xInteg,
-                         const real& propDir) {
+void Event::apply_impulsive(PropSimulation *propSim, const real &t, std::vector<real>& 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<size_t> tEvalSortedIdx(tEval.size());
+        std::vector<size_t> 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<real> extraVec = std::vector<real>(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;