fix the bug for the initialization of the estimators

robotology · Nov 2, 2021 · 10c2474 · 10c2474
1 parent 97201f7
commit 10c2474
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/modules/HapticGlove_module/src/MotorEstimation.cpp b/modules/HapticGlove_module/src/MotorEstimation.cpp
@@ -57,7 +57,7 @@ Estimator::~Estimator()
 bool Estimator::initialize(const Eigen::MatrixXd& z0)
 {
 
-    Eigen::MatrixXd x0 = m_H * z0;
+    Eigen::MatrixXd x0 = (m_H.transpose() * m_H).inverse() * m_H.transpose() * z0;
     Eigen::MatrixXd M0 = Eigen::MatrixXd::Identity(m_n, m_n);
 
     return m_kf->initialize(x0, M0);