handing over a shared_ptr to ct system model rather than a reference

Author: markusgft

ct_optcon/examples/KalmanDisturbanceFiltering.cpp

@@ -183,12 +183,13 @@ int main(int argc, char** argv)
     ct::core::loadMatrix(configFile, "kalman_weights.Qaug", Qaug);
     ct::core::loadMatrix(configFile, "kalman_weights.R", R);
 
-    dFdv.setIdentity();  // todo tune me
+    dFdv.setIdentity();
 
     // create a sensitivity approximator to obtain discrete-time dynamics matrices
     std::shared_ptr<ct::core::SystemLinearizer<state_dim + dist_dim, control_dim>> linearizer(
         new ct::core::SystemLinearizer<state_dim + dist_dim, control_dim>(inputDisturbedSystem));
-    ct::core::SensitivityApproximation<state_dim + dist_dim, control_dim> sensApprox(dt, linearizer);
+    std::shared_ptr<ct::core::SensitivityApproximation<state_dim + dist_dim, control_dim>> sensApprox(
+        new ct::core::SensitivityApproximation<state_dim + dist_dim, control_dim>(dt, linearizer));
 
     // set up the system model
     std::shared_ptr<ct::optcon::CTSystemModel<state_dim + dist_dim, control_dim>> sysModel(

ct_optcon/examples/KalmanFiltering.cpp

@@ -83,7 +83,7 @@ int main(int argc, char** argv)
     // load Kalman Filter weighting matrices from file
     ct::core::StateMatrix<state_dim> Q, dFdv;
     ct::core::OutputMatrix<output_dim> R;
-    dFdv.setIdentity();  // todo tune me!
+    dFdv.setIdentity();
     ct::core::loadMatrix(settingsFile, "kalman_weights.Q", Q);
     ct::core::loadMatrix(settingsFile, "kalman_weights.R", R);
     std::cout << "Loaded Kalman R as " << std::endl << R << std::endl;
@@ -92,7 +92,9 @@ int main(int argc, char** argv)
     // create a sensitivity approximator to compute A and B matrices
     std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> linearizer(
         new ct::core::SystemLinearizer<state_dim, control_dim>(oscillator));
-    ct::core::SensitivityApproximation<state_dim, control_dim> sensApprox(dt, linearizer);
+
+    std::shared_ptr<ct::core::SensitivityApproximation<state_dim, control_dim>> sensApprox(
+        new ct::core::SensitivityApproximation<state_dim, control_dim>(dt, linearizer));
 
 
     // the observer is supplied with a dynamic model identical to the one used above for data generation
@@ -103,8 +105,10 @@ int main(int argc, char** argv)
         new ct::optcon::CTSystemModel<state_dim, control_dim>(oscillator_observer_model, sensApprox, dFdv));
 
     // set up the measurement model
+    ct::core::OutputStateMatrix<output_dim, state_dim> dHdw;
+    dHdw.setIdentity();
     std::shared_ptr<ct::optcon::LinearMeasurementModel<output_dim, state_dim>> measModel(
-        new ct::optcon::LTIMeasurementModel<output_dim, state_dim>(C));
+        new ct::optcon::LTIMeasurementModel<output_dim, state_dim>(C, dHdw));
 
     // set up Filter, e.g. extended Kalman or Unscented Kalman filter
     ct::optcon::ExtendedKalmanFilter<state_dim, control_dim, output_dim> filter(
@@ -129,7 +133,7 @@ int main(int argc, char** argv)
         // compute an observation
         states_meas[i] = states[i];
 
-        // todo this is technically not correct, the noise enters not on the state but on the output!!!
+        // note that this is technically not correct, the noise enters not on the state but on the output!!!
         position_measurement_noise.noisify(states_meas[i](0));  // Position noise.
         velocity_measurement_noise.noisify(states_meas[i](1));  // Velocity noise.
         ct::core::OutputVector<output_dim> y = C * states_meas[i];

ct_optcon/include/ct/optcon/filter/CTSystemModel-impl.h

@@ -11,10 +11,10 @@ namespace optcon {
 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
 CTSystemModel<STATE_DIM, CONTROL_DIM, SCALAR>::CTSystemModel(
     std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> system,
-    const SensitivityApprox_t& sensApprox,
+    std::shared_ptr<SensitivityApprox_t> sensApprox,
     const state_matrix_t& dFdv,
     const ct::core::IntegrationType& intType)
-    : system_(system->clone()),
+    : system_(system),
       constantController_(new ct::core::ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>()),
       sensApprox_(sensApprox),
       dFdv_(dFdv),
@@ -49,9 +49,9 @@ auto CTSystemModel<STATE_DIM, CONTROL_DIM, SCALAR>::computeDerivativeState(const
     state_matrix_t A;
     ct::core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> Btemp;
 
-    sensApprox_.setTimeDiscretization(dt);
+    sensApprox_->setTimeDiscretization(dt);
 
-    sensApprox_.getAandB(state, u, state, int(t / dt), 1, A, Btemp);
+    sensApprox_->getAandB(state, u, state, int(t / dt), 1, A, Btemp);
     return A;
 }
 

ct_optcon/include/ct/optcon/filter/CTSystemModel.h

@@ -39,7 +39,7 @@ class CTSystemModel final : public SystemModelBase<STATE_DIM, CONTROL_DIM, SCALA
 
     //! Constructor. Takes in the system with defined controller, and sens approximator for computing the derivatives
     CTSystemModel(std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> system,
-        const SensitivityApprox_t& sensApprox,
+        std::shared_ptr<SensitivityApprox_t> sensApprox,
         const state_matrix_t& dFdv,
         const ct::core::IntegrationType& intType = ct::core::IntegrationType::EULERCT);
 
@@ -69,7 +69,7 @@ class CTSystemModel final : public SystemModelBase<STATE_DIM, CONTROL_DIM, SCALA
     std::shared_ptr<ct::core::ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>> constantController_;
 
     //! The sensitivity approximator
-    SensitivityApprox_t sensApprox_;
+    std::shared_ptr<SensitivityApprox_t> sensApprox_;
 
     //! Derivative w.r.t. noise.
     state_matrix_t dFdv_;