From 4360735ced1ba968b5a43c90a1439cbb40ccb134 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?C=C3=A9dric=20Perottet?= <cedric.perottet@outlook.com>
Date: Tue, 15 Jun 2021 05:21:54 +0200
Subject: [PATCH] Fix for UnscentedKalmannFilter

---
 ct_optcon/examples/KalmanFiltering.cpp                     | 2 +-
 .../include/ct/optcon/filter/UnscentedKalmanFilter-impl.h  | 7 +++++--
 ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h | 4 ++--
 3 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/ct_optcon/examples/KalmanFiltering.cpp b/ct_optcon/examples/KalmanFiltering.cpp
index d3f8a7dba..fef7a0989 100644
--- 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
index 2e5dfb5c1..57d30e4b4 100644
--- 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
index bffac4c60..36f160d96 100644
--- 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.