Fixing major bug in unscented Kalman Filter, moved system and measurement model pointers to Estimator base, deleted now redundant observer classes.

Author: markusgft

ct_optcon/examples/KalmanDisturbanceFiltering.cpp

@@ -19,26 +19,26 @@ Licensed under Apache2 license (see LICENSE file in main directory)
 #include "exampleDir.h"
 
 // a damped oscillator has two states, position and velocity
-const size_t STATE_DIM = ct::core::SecondOrderSystem::STATE_DIM;      // = 2
-const size_t CONTROL_DIM = ct::core::SecondOrderSystem::CONTROL_DIM;  // = 1
-const size_t OUTPUT_DIM = STATE_DIM;                                  // we measure the full state
-const size_t DIST_DIM = CONTROL_DIM;                                  // we consider an input disturbance
+const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;      // = 2
+const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;  // = 1
+const size_t output_dim = state_dim;                                  // we measure the full state
+const size_t dist_dim = control_dim;                                  // we consider an input disturbance
 
 
 /*!
  * \brief An example controller, in which we artifically add a time-varying disturbance.
  * @note this is only required for this simulated example. Not required for proper simulator/hardware setups.
  */
-class CustomController : public ct::core::Controller<STATE_DIM, CONTROL_DIM>
+class CustomController : public ct::core::Controller<state_dim, control_dim>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    static const size_t STATE_DIM = 2;
-    static const size_t CONTROL_DIM = 1;
+    static const size_t state_dim = 2;
+    static const size_t control_dim = 1;
 
     //! default constructor
-    CustomController(const ct::core::ControlVector<CONTROL_DIM>& uff_max,  // feedforward control amplitude
+    CustomController(const ct::core::ControlVector<control_dim>& uff_max,  // feedforward control amplitude
         const double& uff_frequency,         // frequency of the feedforward control osciallation
         const double& kp,                    // P gain
         const double& kd,                    // D gain
@@ -58,25 +58,25 @@ class CustomController : public ct::core::Controller<STATE_DIM, CONTROL_DIM>
     CustomController* clone() const override { return new CustomController(*this); }
 
     //! override the compute control method with a custom control law which includes a disturbance
-    void computeControl(const ct::core::StateVector<STATE_DIM>& state,
+    void computeControl(const ct::core::StateVector<state_dim>& state,
         const double& t,
-        ct::core::ControlVector<CONTROL_DIM>& controlAction) override
+        ct::core::ControlVector<control_dim>& controlAction) override
     {
         controlAction = uff_max_ * std::sin(t * uff_frequency_);  // apply feedforward control
         controlAction += getSimulatedDisturbance(t);              // apply simulated disturbance
         controlAction(0) += -kp_ * state(0) - kd_ * state(1);     // add feedback control
     }
 
     //! simulate the disturbance allow to reconstruct it from outside
-    ct::core::ControlVector<CONTROL_DIM> getSimulatedDisturbance(const double& t)
+    ct::core::ControlVector<control_dim> getSimulatedDisturbance(const double& t)
     {
-        ct::core::ControlVector<CONTROL_DIM> dist_in;
+        ct::core::ControlVector<control_dim> dist_in;
         dist_in(0) = d_ * std::cos(t * t * d_freq_);
         return dist_in;
     }
 
 private:
-    ct::core::ControlVector<CONTROL_DIM> uff_max_;  //! feedforward control action
+    ct::core::ControlVector<control_dim> uff_max_;  //! feedforward control action
     double uff_frequency_;                          // frequency of feedforward oscillation
     double kp_;                                     //! P gain
     double kd_;                                     //! D gain
@@ -90,21 +90,21 @@ class CustomController : public ct::core::Controller<STATE_DIM, CONTROL_DIM>
  * for dynamics prediction and computing derivatives.
  * @note this system is not used for simulation, but for filtering.
  */
-template <size_t STATE_DIM, size_t DIST_DIM, size_t CONTROL_DIM, typename SCALAR = double>
-class CustomDisturbedSystem : public ct::optcon::DisturbedSystem<STATE_DIM, DIST_DIM, CONTROL_DIM, SCALAR>
+template <size_t state_dim, size_t dist_dim, size_t control_dim, typename SCALAR = double>
+class CustomDisturbedSystem : public ct::optcon::DisturbedSystem<state_dim, dist_dim, control_dim, SCALAR>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     //! constructor
-    CustomDisturbedSystem(std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> sys)
-        : ct::optcon::DisturbedSystem<STATE_DIM, DIST_DIM, CONTROL_DIM, SCALAR>(sys->getController()), system_(sys)
+    CustomDisturbedSystem(std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, SCALAR>> sys)
+        : ct::optcon::DisturbedSystem<state_dim, dist_dim, control_dim, SCALAR>(sys->getController()), system_(sys)
     {
     }
 
     //! copy constructor
     CustomDisturbedSystem(const CustomDisturbedSystem& other)
-        : ct::optcon::DisturbedSystem<STATE_DIM, DIST_DIM, CONTROL_DIM, SCALAR>(*this), system_(other.system_->clone())
+        : ct::optcon::DisturbedSystem<state_dim, dist_dim, control_dim, SCALAR>(*this), system_(other.system_->clone())
     {
     }
 
@@ -114,27 +114,27 @@ class CustomDisturbedSystem : public ct::optcon::DisturbedSystem<STATE_DIM, DIST
     /*!
      * Override the computeControlledDynamics() with a custom update rule.
      */
-    void computeControlledDynamics(const ct::core::StateVector<STATE_DIM + DIST_DIM, SCALAR>& state,
+    void computeControlledDynamics(const ct::core::StateVector<state_dim + dist_dim, SCALAR>& state,
         const SCALAR& t,
-        const ct::core::ControlVector<CONTROL_DIM, SCALAR>& control,
-        ct::core::StateVector<STATE_DIM + DIST_DIM, SCALAR>& derivative) override
+        const ct::core::ControlVector<control_dim, SCALAR>& control,
+        ct::core::StateVector<state_dim + dist_dim, SCALAR>& derivative) override
     {
         derivative.setZero();
-        ct::core::StateVector<STATE_DIM, SCALAR> tempDerivative;
+        ct::core::StateVector<state_dim, SCALAR> tempDerivative;
 
         // the control consists of actual commanded control plus the estimated input disturbance,
         // which is the augmented part of the state vector
-        ct::core::ControlVector<CONTROL_DIM, SCALAR> disturbed_control = control + state.template tail<DIST_DIM>();
+        ct::core::ControlVector<control_dim, SCALAR> disturbed_control = control + state.template tail<dist_dim>();
 
         // the dynamics of the augmented system
-        system_->computeControlledDynamics(state.head(STATE_DIM), t, disturbed_control, tempDerivative);
-        derivative.template head<STATE_DIM>() = tempDerivative;
+        system_->computeControlledDynamics(state.head(state_dim), t, disturbed_control, tempDerivative);
+        derivative.template head<state_dim>() = tempDerivative;
     }
 
 
 private:
     // the nominal system (the one we are trying to control)
-    std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> system_;
+    std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, SCALAR>> system_;
 };
 
 
@@ -149,7 +149,7 @@ int main(int argc, char** argv)
     ct::core::loadScalar(configFile, "experiment_settings.nSteps", nSteps);
 
     // state vector (initial state)
-    ct::core::StateVector<STATE_DIM> x;
+    ct::core::StateVector<state_dim> x;
     ct::core::loadMatrix(configFile, "x0", x);
 
     // create an oscillator with resonance-frequency w_n
@@ -160,7 +160,7 @@ int main(int argc, char** argv)
     // create our controller: a PD controller which also "simulates" a time-varying disturbance
     double kp = 10.0;
     double kd = 1.0;
-    ct::core::ControlVector<CONTROL_DIM> uff_magnitude;
+    ct::core::ControlVector<control_dim> uff_magnitude;
     double uff_frequency, disturbance_max, disturbance_freq;
     ct::core::loadMatrix(configFile, "experiment_settings.uff_magnitude", uff_magnitude);
     ct::core::loadScalar(configFile, "experiment_settings.uff_frequency", uff_frequency);
@@ -174,24 +174,23 @@ int main(int argc, char** argv)
     oscillator->setController(disturbed_controller);
 
     // create an integrator for "simulating" the measured data
-    ct::core::Integrator<STATE_DIM> integrator(oscillator, ct::core::IntegrationType::EULERCT);
+    ct::core::Integrator<state_dim> integrator(oscillator, ct::core::IntegrationType::EULERCT);
 
-    ct::core::StateVectorArray<STATE_DIM> states;        // recorded sim states
-    ct::core::ControlVectorArray<DIST_DIM> disturbance;  // recorded disturbance
+    ct::core::StateVectorArray<state_dim> states;        // recorded sim states
+    ct::core::ControlVectorArray<dist_dim> disturbance;  // recorded disturbance
     ct::core::tpl::TimeArray<double> times;              // recorded times
 
     // read process noise parameters from file
-    ct::core::StateMatrix<STATE_DIM> process_var;
+    ct::core::StateMatrix<state_dim> process_var;
     ct::core::loadMatrix(configFile, "process_noise.process_var", process_var);
 
     // process noise (scaled with dt)
     ct::core::GaussianNoise position_process_noise(0.0, dt * process_var(0, 0));
     ct::core::GaussianNoise velocity_process_noise(0.0, dt * process_var(1, 1));
 
     // simulate the disturbed system to generate data
-    ct::core::Time t0 = 0.0;
     states.push_back(x);
-    for (size_t i = 0; i < nSteps; i++)
+    for (int i = 0; i < nSteps; i++)
     {
         // integrate system
         integrator.integrate_n_steps(x, i * dt, 1, dt);
@@ -216,85 +215,87 @@ int main(int argc, char** argv)
     std::shared_ptr<CustomController> controller_nominal(
         new CustomController(uff_magnitude, uff_frequency, kp, kd, 0.0, 0.0));
 
-    std::shared_ptr<CustomDisturbedSystem<STATE_DIM, DIST_DIM, CONTROL_DIM>> customdisturbedSystem(
-        new CustomDisturbedSystem<STATE_DIM, DIST_DIM, CONTROL_DIM>(oscillator_obs));
+    std::shared_ptr<CustomDisturbedSystem<state_dim, dist_dim, control_dim>> customdisturbedSystem(
+        new CustomDisturbedSystem<state_dim, dist_dim, control_dim>(oscillator_obs));
 
     // Observation matrix for the state
-    ct::core::OutputStateMatrix<OUTPUT_DIM, STATE_DIM> C;
+    ct::core::OutputStateMatrix<output_dim, state_dim> C;
     C.setIdentity();
 
     // Observation matrix for the disturbance (assuming the disturbance does not enter the output equation)
-    ct::core::OutputStateMatrix<OUTPUT_DIM, DIST_DIM> Cd;
+    ct::core::OutputStateMatrix<output_dim, dist_dim> Cd;
     Cd.setZero();
 
     // Total observation matrix, state and disturbance combined
-    ct::core::OutputStateMatrix<OUTPUT_DIM, STATE_DIM + DIST_DIM, double> Caug;
+    ct::core::OutputStateMatrix<output_dim, state_dim + dist_dim, double> Caug;
     Caug << C, Cd;
 
     // Kalman filter weighting matrices Q, dFdv and R
-    ct::core::StateMatrix<STATE_DIM + DIST_DIM> Qaug, dFdv;
-    ct::core::OutputMatrix<OUTPUT_DIM> R;
+    ct::core::StateMatrix<state_dim + dist_dim> Qaug, dFdv;
+    ct::core::OutputMatrix<output_dim> R;
     ct::core::loadMatrix(configFile, "kalman_weights.Qaug", Qaug);
     ct::core::loadMatrix(configFile, "kalman_weights.R", R);
 
     dFdv.setIdentity();  // todo tune me
 
     // 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>(customdisturbedSystem));
-    ct::core::SensitivityApproximation<STATE_DIM + DIST_DIM, CONTROL_DIM> sensApprox(dt, linearizer);
+    std::shared_ptr<ct::core::SystemLinearizer<state_dim + dist_dim, control_dim>> linearizer(
+        new ct::core::SystemLinearizer<state_dim + dist_dim, control_dim>(customdisturbedSystem));
+    ct::core::SensitivityApproximation<state_dim + dist_dim, control_dim> sensApprox(dt, linearizer);
+
+    // set up the system model
+    std::shared_ptr<ct::optcon::CTSystemModel<state_dim + dist_dim, control_dim>> sysModel(
+        new ct::optcon::CTSystemModel<state_dim + dist_dim, control_dim>(customdisturbedSystem, sensApprox, dFdv));
+
+    // set up the measurement model
+    std::shared_ptr<ct::optcon::LinearMeasurementModel<output_dim, state_dim + dist_dim>> measModel(
+        new ct::optcon::LTIMeasurementModel<output_dim, state_dim + dist_dim>(Caug));
 
     // load measurement noise data from file
-    ct::core::StateMatrix<STATE_DIM> meas_var;
+    ct::core::StateMatrix<state_dim> meas_var;
     ct::core::loadMatrix(configFile, "measurement_noise.measurement_var", meas_var);
     ct::core::GaussianNoise position_measurement_noise(0.0, meas_var(0, 0));
     ct::core::GaussianNoise velocity_measurement_noise(0.0, meas_var(1, 1));
 
     // generate initial state for the estimator (with noise, disturbance assumed to be zero at beginning)
-    ct::core::StateVector<STATE_DIM + DIST_DIM> x0aug;
+    ct::core::StateVector<state_dim + dist_dim> x0aug;
     x0aug << states[0], 0.0;
     position_measurement_noise.noisify(x0aug(0));
     velocity_measurement_noise.noisify(x0aug(1));
 
-    // create instance of the extended Kalman Filter
-    // initialize it estimated starting covariance equal to Qaug
-    ct::optcon::ExtendedKalmanFilter<STATE_DIM + DIST_DIM> ekf(x0aug, Qaug);
-
-    // create instance of the disturbance observer
-    ct::optcon::DisturbanceObserver<OUTPUT_DIM, STATE_DIM, DIST_DIM, CONTROL_DIM,
-        ct::optcon::ExtendedKalmanFilter<STATE_DIM + DIST_DIM>>
-        disturbanceObserver(customdisturbedSystem, sensApprox, Caug, ekf, Qaug, R, dFdv);
-
-
     // data containers for logging data while estimating
-    ct::core::StateVectorArray<STATE_DIM + DIST_DIM> states_est(states.size());
-    ct::core::OutputVectorArray<OUTPUT_DIM> output_meas(states.size());
-    ct::core::StateMatrixArray<STATE_DIM + DIST_DIM> cov_est(states.size());
+    ct::core::StateVectorArray<state_dim + dist_dim> states_est(states.size());
+    ct::core::OutputVectorArray<output_dim> output_meas(states.size());
+    ct::core::StateMatrixArray<state_dim + dist_dim> cov_est(states.size());
     states_est[0] = x0aug;
     output_meas[0] = states[0];
-    cov_est[0] = disturbanceObserver.getCovarianceMatrix();
+    cov_est[0] = Qaug;
+
+
+    // set up Extended Kalman Filter
+    ct::optcon::ExtendedKalmanFilter<state_dim + dist_dim, control_dim, output_dim> ekf(sysModel, measModel, Qaug, R, x0aug, Qaug);
 
 
     // run the filter over the simulated data
     for (size_t i = 1; i < states.size(); ++i)
     {
         // compute an observation
-        ct::core::OutputVector<OUTPUT_DIM> y = C * states[i] + Cd * disturbance[i - 1];
+        ct::core::OutputVector<output_dim> y = C * states[i] + Cd * disturbance[i - 1];
         position_measurement_noise.noisify(y(0));  // Position noise.
         velocity_measurement_noise.noisify(y(1));  // Velocity noise.
         output_meas[i] = y;
 
         // this is the control input that we would have "measured"
-        ct::core::ControlVector<CONTROL_DIM> nominal_control;
-        controller_nominal->computeControl(states_est[i - 1].template head<STATE_DIM>(), dt * (i - 1), nominal_control);
+        ct::core::ControlVector<control_dim> nominal_control;
+        controller_nominal->computeControl(states_est[i - 1].template head<state_dim>(), dt * (i - 1), nominal_control);
 
         // Kalman filter prediction step
-        disturbanceObserver.predict(nominal_control, dt, dt * i);
+        ekf.predict(nominal_control, dt, dt * i);
 
         // Kalman filter estimation step (state + disturbance)
-        states_est[i] = disturbanceObserver.update(y, dt, dt * i);
+        states_est[i] = ekf.update(y, dt, dt * i);
 
-        cov_est[i] = disturbanceObserver.getCovarianceMatrix();
+        cov_est[i] = ekf.getCovarianceMatrix();
     }