fixed two bugs in state observer and disturbance observer implementations, made minimal example for disturbance observer work.

Author: markusgft

ct_optcon/examples/KalmanDisturbanceFiltering.cpp

@@ -12,9 +12,13 @@ Licensed under Apache2 license (see LICENSE file in main directory)
 
 #include <ct/optcon/optcon.h>
 #include <CustomController.h>  // Using the custom controller from ct_core examples.
+#include "exampleDir.h"
 
 int main(int argc, char** argv)
 {
+    // file with kalman weights
+    std::string pathToWeights = ct::optcon::exampleDir + "/kalmanFilterWeights.info";
+
     // 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
@@ -46,9 +50,14 @@ int main(int argc, char** argv)
     ct::core::ControlVectorArray<CONTROL_DIM> controls;
     ct::core::tpl::TimeArray<double> times;
 
+    ct::core::StateMatrix<STATE_DIM> process_var;
+    ct::core::loadMatrix(pathToWeights, "process_noise.process_var", process_var);
+
+    ct::core::GaussianNoise position_process_noise(0.0, process_var(0, 0));
+    ct::core::GaussianNoise velocity_process_noise(0.0, process_var(1, 1));
+
     // simulate 100 steps
     double dt = 0.001;
-    double t0 = 0.0;
     size_t nSteps = 100;
     states.push_back(x);
     for (size_t i = 0; i < nSteps; i++)
@@ -59,6 +68,10 @@ int main(int argc, char** argv)
         controls.push_back(u_temp);
 
         integrator.integrate_n_steps(x, i * dt, 1, dt);
+
+        position_process_noise.noisify(x(0));  // Position noise.
+        velocity_process_noise.noisify(x(1));  // Velocity noise.
+
         states.push_back(x);
         times.push_back(i * dt);
     }
@@ -67,57 +80,105 @@ int main(int argc, char** argv)
     ct::core::OutputStateMatrix<OUTPUT_DIM, STATE_DIM> C;
     C.setIdentity();
 
-    // create Kalman Filter weighting matrices
-    ct::core::StateMatrix<STATE_DIM, double> Q = ct::core::StateMatrix<STATE_DIM, double>::Identity();
-    ct::core::OutputMatrix<OUTPUT_DIM, double> R = 10 * ct::core::OutputMatrix<OUTPUT_DIM, double>::Identity();
+    // load Kalman Filter weighting matrices from file
+    ct::core::StateMatrix<STATE_DIM> Q, dFdv;
+    ct::core::OutputMatrix<OUTPUT_DIM> R;
+    ct::core::loadMatrix(pathToWeights, "kalman_weights.Q", Q);
+    ct::core::loadMatrix(pathToWeights, "kalman_weights.R", R);
+    std::cout << "Loaded Kalman R as " << std::endl << R << std::endl;
+    std::cout << "Loaded Kalman Q as " << std::endl << Q << std::endl;
+
+    dFdv.setIdentity();  // todo tune me!
 
     // 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);
 
     // set up Extended Kalman Filter
-    ct::optcon::ExtendedKalmanFilter<STATE_DIM> ekf(states[0]);
+    ct::optcon::ExtendedKalmanFilter<STATE_DIM> ekf(states[0], R);
 
     // the observer is supplied with a dynamic model identical to the one used above for data generation
     std::shared_ptr<ct::core::SecondOrderSystem> oscillator_observer_model(new ct::core::SecondOrderSystem(w_n));
-    
+
     // initialize the observer
     ct::optcon::StateObserver<OUTPUT_DIM, STATE_DIM, CONTROL_DIM, ct::optcon::ExtendedKalmanFilter<STATE_DIM>>
-        stateObserver(oscillator_observer_model, sensApprox, dt, C, ekf, Q, R);
+        stateObserver(oscillator_observer_model, sensApprox, C, R, ekf, Q, R, dFdv);
+
 
-    ct::core::GaussianNoise position_measurement_noise(0, 0.01);
-    ct::core::GaussianNoise velocity_measurement_noise(0, 0.1);
+    ct::core::StateMatrix<STATE_DIM> meas_var;
+    ct::core::loadMatrix(pathToWeights, "measurement_noise.measurement_var", meas_var);
 
-    ct::core::StateVectorArray<STATE_DIM> states_est;
-    states_est.push_back(states[0]);
-    ct::core::StateVector<STATE_DIM> xt;
+    ct::core::GaussianNoise position_measurement_noise(0.0, meas_var(0, 0));
+    ct::core::GaussianNoise velocity_measurement_noise(0.0, meas_var(1, 1));
+
+    ct::core::StateVectorArray<STATE_DIM> states_est(states.size());
+    ct::core::StateVectorArray<STATE_DIM> states_meas(states.size());
+    states_est[0] = states[0];
+    states_meas[0] = states[0];
 
     // run the filter over the simulated data
     for (size_t i = 1; i < states.size(); ++i)
     {
         // compute an observation
-        xt = states[i];
-        position_measurement_noise.noisify(xt[0]);  // Position noise.
-        velocity_measurement_noise.noisify(xt[1]);  // Velocity noise.
-        ct::core::OutputVector<OUTPUT_DIM> y = C * xt;
+        states_meas[i] = states[i];
+
+        // todo 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];
 
         // Kalman filter prediction step
-        stateObserver.predict(controls[i], dt * i);
+        stateObserver.predict(controls[i], dt, dt * i);
 
         // Kalman filter estimation step
-        ct::core::StateVector<STATE_DIM> x_est = stateObserver.update(y);
+        ct::core::StateVector<STATE_DIM> x_est = stateObserver.update(y, dt, dt * i);
 
         // and log for printing
-        states_est.push_back(x_est);
+        states_est[i] = x_est;
+    }
+
+
+// plot if plotting library built.
+#ifdef PLOTTING_ENABLED
+
+    std::vector<double> time_plot, pos_est_plot, vel_est_plot, pos_plot, vel_plot, pos_meas_plot, vel_meas_plot;
+    for (size_t i = 0; i < times.size(); i++)
+    {
+        time_plot.push_back(times[i]);
+        pos_est_plot.push_back(states_est[i](0));
+        vel_est_plot.push_back(states_est[i](1));
+        pos_plot.push_back(states[i](0));
+        vel_plot.push_back(states[i](1));
+        pos_meas_plot.push_back(states_meas[i](0));
+        vel_meas_plot.push_back(states_meas[i](1));
     }
 
-    // print results
+    // plot position
+    ct::core::plot::subplot(2, 1, 1);
+    ct::core::plot::labelPlot("pos est", time_plot, pos_est_plot);
+    ct::core::plot::labelPlot("ground truth", time_plot, pos_plot, "r--");
+    ct::core::plot::labelPlot("pos meas", time_plot, pos_meas_plot, "k--");
+    ct::core::plot::legend();
+    ct::core::plot::ylabel("pos [m]");
+
+    // plot velocity
+    ct::core::plot::subplot(2, 1, 2);
+    ct::core::plot::labelPlot("vel est", time_plot, vel_est_plot);
+    ct::core::plot::labelPlot("ground truth", time_plot, vel_plot, "r--");
+    ct::core::plot::labelPlot("vel meas", time_plot, vel_meas_plot, "k--");
+    ct::core::plot::ylabel("vel [m/sec]");
+    ct::core::plot::xlabel("time [sec]");
+    ct::core::plot::legend();
+
+    ct::core::plot::show();
+#else  // print results to command line
     for (size_t i = 0; i < states_est.size(); ++i)
         std::cout << "State\t\tState_est\n"
                   << std::fixed << std::setprecision(6) << states[i][0] << "\t" << states_est[i][0] << std::endl
                   << states[i][1] << "\t" << states_est[i][1] << std::endl
                   << std::endl;
+#endif
 
     return 0;
 }

ct_optcon/examples/KalmanFiltering.cpp

@@ -11,52 +11,54 @@ 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 ct::core::SensitivityApproximation<STATE_DIM, CONTROL_DIM, STATE_DIM / 2, STATE_DIM / 2, SCALAR>& sensApprox,
-    double dt,
-    unsigned numSubsteps,
+    const SensitivityApprox_t& sensApprox,
     const state_matrix_t& dFdv,
     const ct::core::IntegrationType& intType)
     : system_(system->clone()),
       constantController_(new ct::core::ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>()),
       sensApprox_(sensApprox),
-      dt_(dt),
       dFdv_(dFdv),
-      integrator_(system_, intType),
-      numSubsteps_(numSubsteps)
+      integrator_(system_, intType)
 {
     if (!system_)
-        throw std::runtime_error("System not initialized!");
+        throw std::runtime_error("CTSystemModel: System not initialized!");
 
     // hand over constant controller for dynamics evaluation with known control inputs to the system.
     system_->setController(constantController_);
-
-    const double dtNormalized = dt_ / (numSubsteps_ + 1);
-    sensApprox_.setTimeDiscretization(dtNormalized);
 }
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
 auto CTSystemModel<STATE_DIM, CONTROL_DIM, SCALAR>::computeDynamics(const state_vector_t& state,
     const control_vector_t& u,
+    const Time_t dt,
     Time_t t) -> state_vector_t
 {
     constantController_->setControl(u);
     state_vector_t x = state;
-    integrator_.integrate_n_steps(x, t, numSubsteps_ + 1, dt_ / (numSubsteps_ + 1));
+    integrator_.integrate_n_steps(x, t, 1, dt);
     return x;
 }
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
 auto CTSystemModel<STATE_DIM, CONTROL_DIM, SCALAR>::computeDerivativeState(const state_vector_t& state,
     const control_vector_t& u,
+    const Time_t dt,
     Time_t t) -> state_matrix_t
 {
-    sensApprox_.getAandB(state, u, state, int(t / dt_ * (numSubsteps_ + 1) + 0.5), numSubsteps_ + 1, A_, B_);
-    return A_;
+    // local vars
+    state_matrix_t A;
+    ct::core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> Btemp;
+
+    sensApprox_.setTimeDiscretization(dt);
+
+    sensApprox_.getAandB(state, u, state, int(t / dt), 1, A, Btemp);
+    return A;
 }
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
 auto CTSystemModel<STATE_DIM, CONTROL_DIM, SCALAR>::computeDerivativeNoise(const state_vector_t& state,
     const control_vector_t& control,
+    const Time_t dt,
     Time_t t) -> state_matrix_t
 {
     return dFdv_;

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

@@ -34,24 +34,31 @@ class CTSystemModel final : public SystemModelBase<STATE_DIM, CONTROL_DIM, SCALA
     using typename Base::state_vector_t;
     using typename Base::Time_t;
 
+    using SensitivityApprox_t =
+        ct::core::SensitivityApproximation<STATE_DIM, CONTROL_DIM, STATE_DIM / 2, STATE_DIM / 2, SCALAR>;
+
     //! 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 ct::core::SensitivityApproximation<STATE_DIM, CONTROL_DIM, STATE_DIM / 2, STATE_DIM / 2, SCALAR>&
-            sensApprox,
-        double dt,
-        unsigned numSubsteps = 0u,
-        const state_matrix_t& dFdv = state_matrix_t::Identity(),
+        const SensitivityApprox_t& sensApprox,
+        const state_matrix_t& dFdv,
         const ct::core::IntegrationType& intType = ct::core::IntegrationType::EULERCT);
 
     //! Propagates the system giving the next state as output. Control input is generated by the system controller.
-    state_vector_t computeDynamics(const state_vector_t& state, const control_vector_t& u, Time_t t) override;
+    state_vector_t computeDynamics(const state_vector_t& state,
+        const control_vector_t& u,
+        const Time_t dt,
+        Time_t t) override;
 
     //! Computes the derivative w.r.t state. Control input is generated by the system controller.
-    state_matrix_t computeDerivativeState(const state_vector_t& state, const control_vector_t& u, Time_t t) override;
+    state_matrix_t computeDerivativeState(const state_vector_t& state,
+        const control_vector_t& u,
+        const Time_t dt,
+        Time_t t) override;
 
     //! Computes the derivative w.r.t noise. Control input is generated by the system controller.
     state_matrix_t computeDerivativeNoise(const state_vector_t& state,
         const control_vector_t& control,
+        const Time_t dt,
         Time_t t) override;
 
 protected:
@@ -62,25 +69,13 @@ 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
-    ct::core::SensitivityApproximation<STATE_DIM, CONTROL_DIM, STATE_DIM / 2, STATE_DIM / 2, SCALAR> sensApprox_;
-
-    //! Time step for the sensitivity approximator.
-    double dt_;
+    SensitivityApprox_t sensApprox_;
 
     //! Derivative w.r.t. noise.
     state_matrix_t dFdv_;
 
     //! Integrator.
     ct::core::Integrator<STATE_DIM, SCALAR> integrator_;
-
-    //! Integration substeps.
-    unsigned numSubsteps_;
-
-    //! dFdx or the derivative w.r.t. state.
-    state_matrix_t A_;
-
-    //! Matrix for storing control linearization.
-    ct::core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B_;
 };
 
 }  // namespace optcon

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

@@ -12,12 +12,19 @@ template <size_t OUTPUT_DIM, size_t STATE_DIM, size_t DIST_DIM, size_t CONTROL_D
 DisturbanceObserver<OUTPUT_DIM, STATE_DIM, DIST_DIM, CONTROL_DIM, ESTIMATOR, SCALAR>::DisturbanceObserver(
     std::shared_ptr<DisturbedSystem_t> system,
     const SensitivityApproximation_t& sensApprox,
-    double dt,
     const output_estimate_matrix_t& Caug,
     const ESTIMATOR& estimator,
     const estimate_matrix_t& Qaug,
-    const output_matrix_t& R)
-    : Base(system, sensApprox, dt, Caug, estimator, Qaug, R)
+    const output_matrix_t& R,
+    const estimate_matrix_t& dFdv)
+    : Base(system,
+          sensApprox,
+          Caug,
+          output_matrix_t::Zero(), // todo this should actually be D
+          estimator,
+          Qaug,
+          R,
+          dFdv)  
 {
 }
 
@@ -27,24 +34,26 @@ DisturbanceObserver<OUTPUT_DIM, STATE_DIM, DIST_DIM, CONTROL_DIM, ESTIMATOR, SCA
     const SensitivityApproximation_t& sensApprox,
     const ESTIMATOR& estimator,
     const DisturbanceObserverSettings<OUTPUT_DIM, ESTIMATE_DIM, SCALAR>& do_settings)
-    : Base(system, sensApprox, do_settings.dt, do_settings.C, estimator, do_settings.Qaug, do_settings.R)
+    : Base(system, sensApprox, do_settings.dFdv, do_settings.C, estimator, do_settings.Qaug, do_settings.R)
 {
 }
 
 template <size_t OUTPUT_DIM, size_t STATE_DIM, size_t DIST_DIM, size_t CONTROL_DIM, class ESTIMATOR, typename SCALAR>
 auto DisturbanceObserver<OUTPUT_DIM, STATE_DIM, DIST_DIM, CONTROL_DIM, ESTIMATOR, SCALAR>::predict(
     const control_vector_t& u,
+    const ct::core::Time& dt,
     const Time_t& t) -> estimate_vector_t
 {
-    return this->estimator_.template predict<CONTROL_DIM>(this->f_, u, this->Q_, t);
+    return this->estimator_.template predict<CONTROL_DIM>(this->f_, u, this->Q_, dt, t);
 }
 
 template <size_t OUTPUT_DIM, size_t STATE_DIM, size_t DIST_DIM, size_t CONTROL_DIM, class ESTIMATOR, typename SCALAR>
 auto DisturbanceObserver<OUTPUT_DIM, STATE_DIM, DIST_DIM, CONTROL_DIM, ESTIMATOR, SCALAR>::update(
     const output_vector_t& y,
-    const Time_t&) -> estimate_vector_t
+    const ct::core::Time& dt,
+    const Time_t& t) -> estimate_vector_t
 {
-    return this->estimator_.template update<OUTPUT_DIM>(y, this->h_, this->R_);
+    return this->estimator_.template update<OUTPUT_DIM>(y, this->h_, this->R_, dt, t);
 }
 
 template <size_t OUTPUT_DIM, size_t STATE_DIM, size_t DIST_DIM, size_t CONTROL_DIM, class ESTIMATOR, typename SCALAR>
@@ -61,5 +70,13 @@ auto DisturbanceObserver<OUTPUT_DIM, STATE_DIM, DIST_DIM, CONTROL_DIM, ESTIMATOR
     return this->estimator_.getEstimate().template tail<DIST_DIM>();
 }
 
+template <size_t OUTPUT_DIM, size_t STATE_DIM, size_t DIST_DIM, size_t CONTROL_DIM, class ESTIMATOR, typename SCALAR>
+auto DisturbanceObserver<OUTPUT_DIM, STATE_DIM, DIST_DIM, CONTROL_DIM, ESTIMATOR, SCALAR>::getCovarianceMatrix()
+    -> const estimate_matrix_t&
+{
+    return this->estimator_.getCovarianceMatrix();
+}
+
+
 }  // namespace optcon
 }  // namespace ct