From d2f1d4e28cbe85d19dd5f95adf4b67948b09d42f Mon Sep 17 00:00:00 2001
From: fbailly <fbailly@laas.fr>
Date: Wed, 23 Jan 2019 17:03:54 +0100
Subject: [PATCH] [added weighted mean of euler angles]
---
include/sot/talos_balance/base-estimator.hh | 3 ++
src/base-estimator.cpp | 41 +++++++++++++++++----
2 files changed, 36 insertions(+), 8 deletions(-)
diff --git a/include/sot/talos_balance/base-estimator.hh b/include/sot/talos_balance/base-estimator.hh
index ec1a213..60e381c 100644
--- a/include/sot/talos_balance/base-estimator.hh
+++ b/include/sot/talos_balance/base-estimator.hh
@@ -79,6 +79,9 @@ namespace dynamicgraph {
/** Avoids singularity while taking the mean of euler angles**/
double eulerMean(double a1, double a2);
+
+ /** Avoids singularity while taking the mean of euler angles**/
+ double wEulerMean(double a1, double a2, double w1, double w2);
class SOTBASEESTIMATOR_EXPORT BaseEstimator
:public::dynamicgraph::Entity
diff --git a/src/base-estimator.cpp b/src/base-estimator.cpp
index 88c013d..3b5f14b 100644
--- a/src/base-estimator.cpp
+++ b/src/base-estimator.cpp
@@ -96,17 +96,39 @@ namespace dynamicgraph
inline
double eulerMean(double a1, double a2)
{
- double res = 0;
double mean = (a1+a2)/2.;
if( (a1- a2) > EIGEN_PI || (a1- a2) < -EIGEN_PI)
{
- res = mean > 0 ? -EIGEN_PI + mean : EIGEN_PI + mean ;
+ return mean > 0 ? -EIGEN_PI + mean : EIGEN_PI + mean ;
}
- else
+ return mean;
+ }
+
+ inline
+ double wEulerMean(double a1, double a2, double w1, double w2)
+ {
+ double wMean = (a1*w1+ a2*w2)/(w1+w2);
+ double na,naw,pa,paw;
+ if((a1- a2) > EIGEN_PI || (a1- a2) < -EIGEN_PI)
{
- res = mean;
+ if (a1 < a2)
+ {
+ na = a1; //negative angle
+ naw = w1; //negative angle weight
+ pa = a2; //positive angle
+ paw = w2; //positive angle weight
+ }
+ else
+ {
+ na = a2; //negative angle
+ naw = w2; //negative angle weight
+ pa = a1; //positive angle
+ paw = w1; //positive angle weight
+ }
+ double piFac = (paw-naw)/(naw+paw);
+ return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
}
- return res;
+ return wMean;
}
#define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation"
@@ -681,13 +703,16 @@ namespace dynamicgraph
// average (we do not take into account the IMU yaw)
double wSum = wL + wR + m_w_imu;
- rpy_chest[0] = eulerMean(rpy_chest_lf[0], rpy_chest_rf[0]);
+ rpy_chest[0] = wEulerMean(rpy_chest_lf[0], rpy_chest_rf[0], wL, wR);
+ //rpy_chest[0] = eulerMean(rpy_chest_lf[0], rpy_chest_rf[0]);
//eulerMean(rpy_chest[0], rpy_chest_imu[0], rpy_chest(0))
- rpy_chest[1] = eulerMean(rpy_chest_lf[1], rpy_chest_rf[1]);
+ rpy_chest[1] = wEulerMean(rpy_chest_lf[1], rpy_chest_rf[1], wL, wR);
+ //rpy_chest[1] = eulerMean(rpy_chest_lf[1], rpy_chest_rf[1]);
//eulerMean(rpy_chest[1], rpy_chest_imu[1], rpy_chest(1))
//rpy_chest(0) = (rpy_chest_lf[0]*wL + rpy_chest_rf[0]*wR + rpy_chest_imu[0]*m_w_imu) / wSum;
//rpy_chest(1) = (rpy_chest_lf[1]*wL + rpy_chest_rf[1]*wR + rpy_chest_imu[1]*m_w_imu) / wSum;
- rpy_chest[2] = eulerMean(rpy_chest_lf[2], rpy_chest_rf[2]);
+ rpy_chest[2] = wEulerMean(rpy_chest_lf[2], rpy_chest_rf[2], wL, wR);
+ //rpy_chest[2] = eulerMean(rpy_chest_lf[2], rpy_chest_rf[2]);
//rpy_chest(2) = (rpy_chest_lf[2]*wL + rpy_chest_rf[2]*wR ) / (wL+wR);
rpyToMatrix(rpy_chest, m_oRchest);
--
GitLab