Fix for UnscentedKalmannFilter

ethz-adrl · Jun 15, 2021 · 4360735 · 4360735
1 parent e7a29a6
commit 4360735
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
diff --git a/ct_optcon/examples/KalmanFiltering.cpp b/ct_optcon/examples/KalmanFiltering.cpp
@@ -105,7 +105,7 @@ 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;
+    ct::core::OutputMatrix<output_dim> dHdw;
     dHdw.setIdentity();
     std::shared_ptr<ct::optcon::LinearMeasurementModel<output_dim, state_dim>> measModel(
         new ct::optcon::LTIMeasurementModel<output_dim, state_dim>(C, dHdw));

diff --git a/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter-impl.h b/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter-impl.h
@@ -62,6 +62,7 @@ UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::UnscentedKalm
     const ct::core::StateMatrix<STATE_DIM, SCALAR>& P0)
     : Base(f, h, x0), alpha_(alpha), beta_(beta), kappa_(kappa), P_(P0)
 {
+    computeWeights();
 }
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
@@ -75,6 +76,7 @@ UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::UnscentedKalm
       kappa_(ukf_settings.kappa),
       P_(ukf_settings.P0)
 {
+    computeWeights();
 }
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
@@ -263,8 +265,9 @@ void UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computeS
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
 template <size_t DIM>
-auto UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computePredictionFromSigmaPoints(
-    const SigmaPoints<DIM>& sigmaPoints) -> state_vector_t
+Eigen::Matrix<SCALAR, DIM, 1>
+UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computePredictionFromSigmaPoints(
+    const SigmaPoints<DIM>& sigmaPoints)
 {
     // Use efficient matrix x vector computation
     return sigmaPoints * sigmaWeights_m_;

diff --git a/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h b/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h
@@ -147,9 +147,9 @@ class UnscentedKalmanFilter final : public EstimatorBase<STATE_DIM, CONTROL_DIM,
     //! Predict measurements of sigma points.
     void computeSigmaPointMeasurements(SigmaPoints<OUTPUT_DIM>& sigmaMeasurementPoints, const ct::core::Time& t = 0);
 
-    //! Make a prediction based on sigma points.
+    //! Make a prediction based on sigma points. Used for both state and output prediction.
     template <size_t DIM>
-    state_vector_t computePredictionFromSigmaPoints(const SigmaPoints<DIM>& sigmaPoints);
+    Eigen::Matrix<SCALAR, DIM, 1> computePredictionFromSigmaPoints(const SigmaPoints<DIM>& sigmaPoints);
 
 private:
     state_matrix_t P_;                                          //! Covariance matrix.