Filtering of divergent component of motion (DCM) and estimation of a bias betweeen the DCM and the corresponding zero moment point for a linearized inverted pendulum model. More...
#include <state-observation/dynamics-estimators/lipm-dcm-estimator.hpp>
Public Member Functions | |
LipmDcmEstimator (double dt=defaultDt_, double omega_0=defaultOmega_, double biasDriftPerSecondStd=defaultBiasDriftSecond, double dcmMeasureErrorStd=defaultDcmErrorStd, double zmpMeasureErrorStd=defaultZmpErrorStd, const Vector2 &biasLimit=Vector2::Constant(defaultBiasLimit), const Vector2 &initZMP=Vector2::Zero(), const Vector2 &initDcm=Vector2::Zero(), const Vector2 &initBias=Vector2::Zero(), const Vector2 &initDcmUncertainty=Vector2::Constant(defaultDCMUncertainty), const Vector2 &initBiasUncertainty=Vector2::Constant(defaultBiasUncertainty)) | |
Construct a new Lipm Dcm Estimator object. More... | |
void | resetWithMeasurements (const Vector2 &measuredDcm, const Vector2 &measuredZMP, const Matrix2 &yaw=Matrix2::Identity(), bool measurementIsWithBias=true, const Vector2 &initBias=Vector2::Constant(0), const Vector2 &initBiasuncertainty=Vector2::Constant(defaultBiasUncertainty)) |
Resets the estimator with first measurements. More... | |
void | resetWithMeasurements (const Vector2 &measuredDcm, const Vector2 &measuredZMP, double yaw, bool measurementIsWithBias=true, const Vector2 &initBias=Vector2::Constant(0), const Vector2 &initBiasuncertainty=Vector2::Constant(defaultBiasUncertainty)) |
Resets the estimator with first measurements. More... | |
void | resetWithMeasurements (const Vector2 &measuredDcm, const Vector2 &measuredZMP, const Matrix3 &rotation, bool measurementIsWithBias=true, const Vector2 &initBias=Vector2::Constant(0), const Vector2 &initBiasuncertainty=Vector2::Constant(defaultBiasUncertainty)) |
Resets the estimator with first measurements. More... | |
~LipmDcmEstimator () | |
Destroy the Lipm Dcm Bias Estimator object. More... | |
void | setLipmNaturalFrequency (double omega_0) |
Set the Lipm Natural Frequency. More... | |
double | getLipmNaturalFrequency () const |
Get the Lipm Natural Frequency. More... | |
void | setUnbiasedCoMOffset (const Vector2 &gamma) |
Set an offset on the CoM dynamics. More... | |
Vector2 | getUnbiasedCoMOffset () const |
Get the ubiased offset on the CoM dynamics. More... | |
void | setZMPCoef (double kappa) |
Set the ZMP oefficient. More... | |
double | getZMPCoef () const |
get the ZMP Coefficient More... | |
void | setSamplingTime (double dt) |
Set the Sampling Time. More... | |
void | setBias (const Vector2 &bias) |
Set the Bias object from a guess. More... | |
void | setBias (const Vector2 &bias, const Vector2 &uncertainty) |
Set the Bias object from a guess. More... | |
void | setBiasDriftPerSecond (double driftPerSecond) |
Set the Bias Drift Per Second. More... | |
void | setBiasLimit (const Vector2 &biasLimit) |
Set the Bias Limit. More... | |
void | setUnbiasedDCM (const Vector2 &dcm) |
set the unbiased DCM position from a guess More... | |
void | setUnbiasedDCM (const Vector2 &dcm, const Vector2 &uncertainty) |
void | setZmpMeasureErrorStd (double) |
Set the Zmp Measurement Error Stamdard devbiation. More... | |
void | setDcmMeasureErrorStd (double) |
Set the Dcm Measurement Error Standard. More... | |
void | setInputs (const Vector2 &dcm, const Vector2 &zmp, const Matrix3 &orientation, const Vector2 &CoMOffset_gamma=Vector2::Zero(), const double ZMPCoef_kappa=1) |
Set the Inputs of the estimator. More... | |
void | setInputs (const Vector2 &dcm, const Vector2 &zmp, double yaw, const Vector2 &CoMOffset_gamma=Vector2::Zero(), const double ZMPCoef_kappa=1) |
Set the Inputs of the estimator. More... | |
void | setInputs (const Vector2 &dcm, const Vector2 &zmp, const Matrix2 &R=Matrix2::Identity(), const Vector2 &CoMOffset_gamma=Vector2::Zero(), const double ZMPCoef_kappa=1) |
Set the Inputs of the estimator. More... | |
void | update () |
Runs the estimation. Needs to be called every timestep. More... | |
Vector2 | getUnbiasedDCM () const |
Get the Unbiased DCM filtered by the estimator. More... | |
Vector2 | getBias () const |
Get the estimated Bias. More... | |
Vector2 | getLocalBias () const |
Get the estimated Bias expressed in the local frame of the robot. More... | |
LinearKalmanFilter & | getFilter () |
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions. More... | |
const LinearKalmanFilter & | getFilter () const |
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions. More... | |
Static Public Attributes | |
constexpr static double | defaultBiasDriftSecond = 0.002 |
default expected drift of the bias every second More... | |
constexpr static double | defaultZmpErrorStd = 0.005 |
default error in the estimation of the sensors More... | |
constexpr static double | defaultDcmErrorStd = 0.01 |
constexpr static double | defaultDCMUncertainty = 0.01 |
default uncertainty in the initial values of DCM and Bias More... | |
constexpr static double | defaultBiasUncertainty = 0.01 |
constexpr static double | defaultBiasLimit = -1 |
default value for Bias limit (negative means limitless) More... | |
Protected Types | |
typedef Eigen::Matrix< double, 4, 2 > | Matrix42 |
typedef Eigen::Matrix< double, 2, 4 > | Matrix24 |
Protected Member Functions | |
void | updateMatricesABQ_ () |
set Matrices: A, B, Q More... | |
Static Protected Member Functions | |
static Matrix2 | Vec2ToSqDiag_ (const Vector2 &v) |
builds a diagonal out of the square valued of the Vec2 More... | |
static Matrix2 | dblToDiag_ (const double &d) |
builds a constant 2x2 diagonal from a double More... | |
static Matrix2 | dblToSqDiag_ (const double &d) |
builds a constant 2x2 diagonal from a square of a double More... | |
Protected Attributes | |
double | omega0_ |
Vector2 | gamma_ = Vector2::Zero() |
double | kappa_ = 1 |
double | dt_ |
double | biasDriftStd_ |
double | zmpErrorStd_ |
bool | needUpdateMatrices_ = true |
Vector2 | previousZmp_ |
Vector2 | biasLimit_ |
LinearKalmanFilter | filter_ |
Matrix4 | A_ |
Matrix4 | B_ |
Matrix24 | C_ |
this needs to be transposed More... | |
Matrix2 | R_ |
measurement noise More... | |
Matrix4 | Q_ |
process noise More... | |
Matrix2 | previousOrientation_ |
Filtering of divergent component of motion (DCM) and estimation of a bias betweeen the DCM and the corresponding zero moment point for a linearized inverted pendulum model.
A humanoid robot can be modeled as an inverted pendulum. The dynamics can be linearized to obtain a dynamics with a convergent and a divergent component of motion (DCN). The dynamics of the DCM depends on the Zero Moment Point. The DCM can be measured using the CoM and its velocity, but the CoM position can be biased. For instance if the measurement of the CoM is biased, or in presence of an external force the dynamics of the DCM is biased \(\dot{\hat{\xi}} & =\omega_{0}(\hat{\xi}-\kappa z+\detla-b)\) where \(\hat{\xi}$\) is the measured dcm, and \(\gamma$\) and \(\kappa$\) are known CoM offset and ZMP coefficient, and \(b$\) is the unknown bias. Also the measurement of the DCM can be noisy (since it uses CoM velocity estimation).
This estimator uses Kalman Filtering to estimate this bias and give a delay-free filtering of the DCM. The theorerical details are available at LIPM estimator
Inputs and outputs:
Tuning: This estimator has a few parameters
initBiasUncertainty | tunes how fast the bias converges initially. Too High values will make the bias overshoot. |
initDcmUncertainty | tunes how good is the initial guess. Changing it will reduce the initial filtering on the DCM until the steady convergence |
dcmMeasureErrorStd | tunes how filtered is the DCM signal. Lower values give less DCM filtering |
zmpMeasureErrorStd | tunes how much the model can be trusted. If the measurements are noisy or if the LIPM model does not well represent the dynamics then higher values are recommended. Lower values increase DCM filtering |
biasDriftPerSecondStd | tunes how fast the bias changes value during the motion. Higher values will make the bias move more during the mosion |
biaslLimit | clamps the values of Drift between x and -x on X axis and between y and -y on Y axis. |
|
protected |
|
protected |
stateObservation::LipmDcmEstimator::LipmDcmEstimator | ( | double | dt = defaultDt_ , |
double | omega_0 = defaultOmega_ , |
||
double | biasDriftPerSecondStd = defaultBiasDriftSecond , |
||
double | dcmMeasureErrorStd = defaultDcmErrorStd , |
||
double | zmpMeasureErrorStd = defaultZmpErrorStd , |
||
const Vector2 & | biasLimit = Vector2::Constant(defaultBiasLimit) , |
||
const Vector2 & | initZMP = Vector2::Zero() , |
||
const Vector2 & | initDcm = Vector2::Zero() , |
||
const Vector2 & | initBias = Vector2::Zero() , |
||
const Vector2 & | initDcmUncertainty = Vector2::Constant(defaultDCMUncertainty) , |
||
const Vector2 & | initBiasUncertainty = Vector2::Constant(defaultBiasUncertainty) |
||
) |
Construct a new Lipm Dcm Estimator object.
Use this if no DCM measurements are available or when a good guess of its unbiased position is available
dt | the sampling time in seconds |
omega_0 | the natural frequency of the DCM (rad/s) |
biasDriftPerSecondStd | the standard deviation of the bias drift (m/s) |
initZMP | the initial value of the DCM |
initDCM | the initial value of the DCM |
initBias | the initial value of the bias |
dcmMeasureErrorStd | the standard deviation of the dcm estimation error, NOT including the bias (m) |
zmpMeasureErrorStd | the standard deviaiton of the zmp estimation error (m) |
biasLimit | the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit) |
initDcmUncertainty | the uncertainty in the DCM initial value in meters |
initBiasUncertainty | the uncertainty in the bias initial value in meters |
stateObservation::LipmDcmEstimator::~LipmDcmEstimator | ( | ) |
Destroy the Lipm Dcm Bias Estimator object.
|
inlinestaticprotected |
builds a constant 2x2 diagonal from a double
|
inlinestaticprotected |
builds a constant 2x2 diagonal from a square of a double
Vector2 stateObservation::LipmDcmEstimator::getBias | ( | ) | const |
Get the estimated Bias.
|
inline |
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions.
|
inline |
Get the Kalman Filter object This can be used to run specific Advanced Kalman filter related funcions.
|
inline |
Get the Lipm Natural Frequency.
|
inline |
Get the estimated Bias expressed in the local frame of the robot.
|
inline |
Get the ubiased offset on the CoM dynamics.
gets the value set by setUnbiasedCoMOffset
Vector2 stateObservation::LipmDcmEstimator::getUnbiasedDCM | ( | ) | const |
Get the Unbiased DCM filtered by the estimator.
@detailt This is the recommended output to take
|
inline |
get the ZMP Coefficient
void stateObservation::LipmDcmEstimator::resetWithMeasurements | ( | const Vector2 & | measuredDcm, |
const Vector2 & | measuredZMP, | ||
const Matrix2 & | yaw = Matrix2::Identity() , |
||
bool | measurementIsWithBias = true , |
||
const Vector2 & | initBias = Vector2::Constant(0) , |
||
const Vector2 & | initBiasuncertainty = Vector2::Constant(defaultBiasUncertainty) |
||
) |
Resets the estimator with first measurements.
Use this when initializing with an available DCM (biased / or not) measurement
measuredDcm | the the measured position of the DCM in the world frame |
measuredZMP | the the measured position of the ZMP in the world frame |
yaw | the initial yaw angle in the form of a rotation matrix |
measurementIsWithBias | sets if yes or no the first measurement is biased |
biasLimit | the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit) |
initBias | the initial value of the drift |
initBiasuncertainty | the uncertainty in the bias initial value in meters |
|
inline |
Resets the estimator with first measurements.
Use this when initializing with an available DCM (biased / or not) measurement
measuredDcm | the the measured position of the DCM in the world frame |
measuredZMP | the the measured position of the ZMP in the world frame |
rotation | the 3d orientation from which the initial yaw angle will be extracted using the angle agnostic approach. This orientation is from local to global. i.e. bias_global == orientation * bias*local |
measurementIsWithBias | sets if yes or no the first measurement is biased |
biasLimit | the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit) |
initBias | the initial value of the drift |
initBiasuncertainty | the uncertainty in the bias initial value in meters |
|
inline |
Resets the estimator with first measurements.
Use this when initializing with an available DCM (biased / or not) measurement
measuredDcm | the the measured position of the DCM in the world frame |
measuredZMP | the the measured position of the ZMP in the world frame |
yaw | the initial yaw angle |
measurementIsWithBias | sets if yes or no the first measurement is biased |
biasLimit | the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit) |
initBias | the initial value of the drift |
initBiasuncertainty | the uncertainty in the bias initial value in meters |
void stateObservation::LipmDcmEstimator::setBias | ( | const Vector2 & | bias | ) |
Set the Bias object from a guess.
bias | guess |
void stateObservation::LipmDcmEstimator::setBias | ( | const Vector2 & | bias, |
const Vector2 & | uncertainty | ||
) |
Set the Bias object from a guess.
bias | guess |
the | uncertainty you have in this guess in meters |
void stateObservation::LipmDcmEstimator::setBiasDriftPerSecond | ( | double | driftPerSecond | ) |
Set the Bias Drift Per Second.
driftPerSecond | the standard deviation of the drift (m/s) |
void stateObservation::LipmDcmEstimator::setBiasLimit | ( | const Vector2 & | biasLimit | ) |
Set the Bias Limit.
biasLimit | the X and Y (expressed in local frame) largest accepted absolute values of the bias (negative means there is no limit) |
void stateObservation::LipmDcmEstimator::setDcmMeasureErrorStd | ( | double | ) |
Set the Dcm Measurement Error Standard.
void stateObservation::LipmDcmEstimator::setInputs | ( | const Vector2 & | dcm, |
const Vector2 & | zmp, | ||
const Matrix2 & | R = Matrix2::Identity() , |
||
const Vector2 & | CoMOffset_gamma = Vector2::Zero() , |
||
const double | ZMPCoef_kappa = 1 |
||
) |
Set the Inputs of the estimator.
dcm | measurement of the DCM in the world frame |
zmp | mesaurement of the ZMP in the world frame |
R | the 2x2 Matrix'representing the yaw angle i.e. bias_global == R * bias*local |
CoMOffset_gamma | is the unbiased CoM offset in the dynamics of the DCM (see setUnbiasedCoMOffset) |
ZMPCoef_kappa | is the coefficient multiplied by the ZMP in the DCM dynamics (see setZMPCoef) |
|
inline |
Set the Inputs of the estimator.
The yaw will be extracted from the orientation using the axis agnostic approach.
dcm | measurement of the DCM in the world frame |
zmp | mesaurement of the ZMP in the world frame |
orientation | the 3d orientation from which the yaw will be extracted. This orientation is from local to global. i.e. bias_global == orientation * bias*local |
CoMOffset_gamma | is the unbiased CoM offset in the dynamics of the DCM (see setUnbiasedCoMOffset) |
ZMPCoef_kappa | is the coefficient multiplied by the ZMP in the DCM dynamics (see setZMPCoef) |
|
inline |
Set the Inputs of the estimator.
dcm | measurement of the DCM in the world frame |
zmp | mesaurement of the ZMP in the world frame |
yaw | is the yaw angle to be used. This orientation is from local to global. i.e. bias_global == R * bias*local |
CoMOffset_gamma | is the unbiased CoM offset in the dynamics of the DCM (see setUnbiasedCoMOffset) |
ZMPCoef_kappa | is the coefficient multiplied by the ZMP in the DCM dynamics (see setZMPCoef) |
void stateObservation::LipmDcmEstimator::setLipmNaturalFrequency | ( | double | omega_0 | ) |
Set the Lipm Natural Frequency.
omega_0 | is the sampling time in seconds |
void stateObservation::LipmDcmEstimator::setSamplingTime | ( | double | dt | ) |
Set the Sampling Time.
dt | sampling time |
void stateObservation::LipmDcmEstimator::setUnbiasedCoMOffset | ( | const Vector2 & | gamma | ) |
Set an offset on the CoM dynamics.
This is an offset that adds to the CoM and the bias in definin the DCM dynamics, it has the same effect on the dynamics as manually modifying the estimated bias, but it does not need to interfer with the estimation, especially when this offset is obtained from a different independent source (e.g. external force sensor).
offset | in meters |
void stateObservation::LipmDcmEstimator::setUnbiasedDCM | ( | const Vector2 & | dcm | ) |
set the unbiased DCM position from a guess
dcm | guess |
void stateObservation::LipmDcmEstimator::setUnbiasedDCM | ( | const Vector2 & | dcm, |
const Vector2 & | uncertainty | ||
) |
uncertainty | the uncertainty in this guess |
void stateObservation::LipmDcmEstimator::setZMPCoef | ( | double | kappa | ) |
Set the ZMP oefficient.
kappa | is the zmp coefficient (no unit) |
void stateObservation::LipmDcmEstimator::setZmpMeasureErrorStd | ( | double | ) |
Set the Zmp Measurement Error Stamdard devbiation.
void stateObservation::LipmDcmEstimator::update | ( | ) |
Runs the estimation. Needs to be called every timestep.
|
protected |
set Matrices: A, B, Q
|
inlinestaticprotected |
builds a diagonal out of the square valued of the Vec2
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
this needs to be transposed
|
staticconstexpr |
default expected drift of the bias every second
|
staticconstexpr |
default value for Bias limit (negative means limitless)
|
staticconstexpr |
|
staticconstexpr |
|
staticconstexpr |
default uncertainty in the initial values of DCM and Bias
|
staticconstexpr |
default error in the estimation of the sensors
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
process noise
|
protected |
measurement noise
|
protected |