API Reference

Contents

API Reference#

This page contains the API documentation of sdu_estimators. The following classes are available:

Parameter Estimator#

Parameter Estimator Base Class#

template<typename T, std::int32_t DIM_N, std::int32_t DIM_P>
class ParameterEstimator#

This class provides a base class for the different parameter estimators for estimating the parameter in the linear regression equation (LRE) defined as:

\( y(t) = \phi^\intercal(t) \theta(t), \)

where \( y : \mathbb{R}_+ \to \mathbb{R}^n \) is the output, \( \phi : \mathbb{R}_+ \to \mathbb{R}^{p \times n} \) is the regressor matrix and \( \theta : \mathbb{R}_+ \to \mathbb{R}^p \) is the parameter vector.

Subclassed by sdu_estimators::parameter_estimators::CascadedDREM< T, DIM_N, DIM_P >, sdu_estimators::parameter_estimators::DREM< T, DIM_N, DIM_P >, sdu_estimators::parameter_estimators::GradientEstimator< T, DIM_N, DIM_P >, sdu_estimators::parameter_estimators::GradientEstimatorSphere< T, DIM_N, DIM_P >

Public Functions

virtual void step(const Eigen::Matrix<T, DIM_N, 1> &y, const Eigen::Matrix<T, DIM_P, DIM_N> &phi) = 0#

Step the execution of the estimator (must be called in a loop externally)

Parameters:

  • y – The output, \( y : \mathbb{R}_+ \to \mathbb{R}^n \).

  • phi – The regressor matrix, \( \phi : \mathbb{R}_+ \to \mathbb{R}^{p \times n} \).

virtual Eigen::Vector<T, DIM_P> get_estimate() = 0#

Get the current estimate of the parameter. Updates when the step function is called.

Returns:

The estimate of the parameter \( \hat{\theta} : \mathbb{R}_+ \to \mathbb{R}^p \).

virtual void reset() = 0#

Reset internal estimator variables.

Gradient Estimator#

template<typename T, std::int32_t DIM_N, std::int32_t DIM_P>
class GradientEstimator : public sdu_estimators::parameter_estimators::ParameterEstimator<T, DIM_N, DIM_P>#

A simple gradient-based parameter estimator as described in e.g., [1] .

The parameter \( \theta \) can be estimated by \( \hat{\theta} \) with the following update rule:

\[\begin{equation} \dot{\hat{\theta}}(t) = \gamma \phi(t) \left( y(t) - \phi^\intercal(t) \hat{\theta}(t) \right), \end{equation}\]

where \( \gamma > 0 \) is a tuning parameter, \( y : \mathbb{R}_+ \to \mathbb{R}^n \) is the output, \( \phi : \mathbb{R}_+ \to \mathbb{R}^{p \times n} \) is, the regressor matrix and \( \theta : \mathbb{R}_+ \to \mathbb{R}^p \) is the parameter vector.

Public Functions

inline GradientEstimator(float dt, const Eigen::Vector<T, DIM_P> gamma, const Eigen::Vector<T, DIM_P> &theta_init, integrator::IntegrationMethod method = integrator::IntegrationMethod::RK4)#

Constructor for the default gradient-based update rule.

Parameters:

  • dt – Sample time.

  • gamma – \( \gamma \in \mathbb{R}^p \) is the vector of gains.

  • theta_init – The initial value of the parameter estimate \( \hat{\theta}(0) \).

inline GradientEstimator(float dt, const Eigen::Vector<T, DIM_P> gamma, const Eigen::Vector<T, DIM_P> &theta_init, float r, integrator::IntegrationMethod method = integrator::IntegrationMethod::RK4)#

Constructor for the gradient-based finite-time update rule.

\[\begin{equation} \dot{\hat{\theta}}(t) = \gamma \phi(t) \lceil y(t) - \phi^\intercal(t) \hat{\theta}(t) \rfloor^r, \end{equation}\]

where \( \lceil x \rfloor^r = \lvert x \rvert^r \text{sign}(x) \), \( r \in (0, 1) \).

Parameters:

  • dt – Sample time.

  • gamma – \( \gamma \in \mathbb{R}^p \) is the vector of gains.

  • theta_init – The initial value of the parameter estimate \( \hat{\theta}(0) \).

  • r – The value of the coefficient, \( r \in (0,1) \).

inline virtual void step(const Eigen::Matrix<T, DIM_N, 1> &y, const Eigen::Matrix<T, DIM_P, DIM_N> &phi)#

Step the execution of the estimator (must be called in a loop externally)

inline virtual Eigen::Vector<T, DIM_P> get_estimate()#

Get the current estimate of the parameter. Updates when the step function is called.

inline virtual void reset()#

Reset internal estimator variables.

inline void set_theta_bounds(Eigen::Vector<T, DIM_P> &theta_lower_bound, Eigen::Vector<T, DIM_P> &theta_upper_bound)#

Set bounds on the estimated theta. If this function is called, the step-function uses projection.

If the parameter lies in a set known a-priori \( \theta \in \Theta \), i.e., for every element \( i = 1, \dots, p \) of the parameter vector \( \theta \)

\[\begin{equation} \theta_i \in [\theta_i^-, \theta_i^+] \end{equation}\]

To ensure that \( \hat{\theta}_i \) lies in this set, the update law is updated with:

\[\begin{split}\begin{align} \dot{\hat{\theta}}_i = 0 \quad &\text{if} \quad &\hat{\theta}_i \leq \theta_i^- &\quad \text{and} \quad \dot{\hat{\theta}}_i < 0 \\ \quad &\text{or} \quad &\hat{\theta}_i \geq \theta_i^+ &\quad \text{and} \quad \dot{\hat{\theta}}_i > 0 \end{align}\end{split}\]

I.e., if the parameter estimate is on the boundary it can only be updated to go within the set again or on the boundary &#8212; not outside.

Parameters:

  • theta_lower_bound – Lower bound on the parameter estimate, \( \theta_i^- \) for \( i = 1, \dots, p \).

  • theta_upper_bound – Upper bound on the parameter estimate, \( \theta_i^+ \) for \( i = 1, \dots, p \).

Gradient Estimator on the Sphere#

template<typename T, std::int32_t DIM_N, std::int32_t DIM_P>
class GradientEstimatorSphere : public sdu_estimators::parameter_estimators::ParameterEstimator<T, DIM_N, DIM_P>#

Public Functions

inline GradientEstimatorSphere(double dt, double gamma, const Eigen::Vector<T, DIM_P> &theta_init)#

Constructor for the default gradient-based update rule.

Parameters:

  • dt – Sample time.

  • gamma – \( \gamma \in \mathbb{R}^p \) is the vector of gains.

  • theta_init – The initial value of the parameter estimate \( \hat{\theta}(0) \).

inline void step(const Eigen::Vector<T, DIM_N> &y, const Eigen::Matrix<T, DIM_P, DIM_N> &phi)#

Step the execution of the estimator (must be called in a loop externally)

inline virtual Eigen::Vector<T, DIM_P> get_estimate()#

Get the current estimate of the parameter. Updates when the step function is called.

inline virtual void reset()#

Reset internal estimator variables.

Dynamic Regressor Extension and Mixing (DREM)#

template<typename T, std::int32_t DIM_N, std::int32_t DIM_P>
class DREM : public sdu_estimators::parameter_estimators::ParameterEstimator<T, DIM_N, DIM_P>#

An implementation of dynamic regressor extension and mixing (DREM) as described in e.g., [2] .

Public Functions

inline virtual void step(const Eigen::Matrix<T, DIM_N, 1> &y, const Eigen::Matrix<T, DIM_P, DIM_N> &phi)#

Step the execution of the estimator (must be called in a loop externally)

inline virtual Eigen::Vector<T, DIM_P> get_estimate()#

Get the current estimate of the parameter. Updates when the step function is called.

inline virtual void reset()#

Reset internal estimator variables.

Cascaded Dynamic Regressor Extension and Mixing (DREM)#

template<typename T, std::int32_t DIM_N, std::int32_t DIM_P>
class CascadedDREM : public sdu_estimators::parameter_estimators::ParameterEstimator<T, DIM_N, DIM_P>#

Cascaded DREM.

An implementation of the cascaded DREM estimator, recently described in [3]

Public Functions

inline virtual void step(const Eigen::Matrix<T, DIM_N, 1> &y, const Eigen::Matrix<T, DIM_P, DIM_N> &phi)#

Step the execution of the estimator (must be called in a loop externally).

inline void set_dy_dphi(const Eigen::Matrix<T, DIM_N, 1> &dy, const Eigen::Matrix<T, DIM_P, DIM_N> &dphi)#

Set the values of the derivative of the LRE components, y and phi.

This function should be called before step.

inline virtual Eigen::Vector<T, DIM_P> get_estimate()#

Get the current estimate of the parameter. Updates when the step function is called.

inline virtual void reset()#

Reset internal estimator variables.

inline void set_eps(double eps)#

Set the lower bound on the Delta value before dividing with it.

Regressor Extension#

template<typename T, int32_t DIM_N, int32_t DIM_P>
class RegressorExtension#

Subclassed by sdu_estimators::regressor_extensions::Kreisselmeier< T, 2 *DIM_N, 2 *DIM_P >, sdu_estimators::regressor_extensions::Delay< T, DIM_N, DIM_P >, sdu_estimators::regressor_extensions::Kreisselmeier< T, DIM_N, DIM_P >, sdu_estimators::regressor_extensions::LTI< T, DIM_N, DIM_P >, sdu_estimators::regressor_extensions::LTV< T, DIM_N, DIM_P >

Delay Operator#

template<typename T, int32_t DIM_N, int32_t DIM_P>
class Delay : public sdu_estimators::regressor_extensions::RegressorExtension<T, DIM_N, DIM_P>#

Public Functions

inline Delay(std::vector<int> d)#
Parameters:

d – : Number of samples to delay.

Linear Time Invariant (LTI) Filter#

template<typename T, int32_t DIM_N, int32_t DIM_P>
class LTI : public sdu_estimators::regressor_extensions::RegressorExtension<T, DIM_N, DIM_P>#

Description

Linear Time Variant (LTV) Filter#

template<typename T, int32_t DIM_N, int32_t DIM_P>
class LTV : public sdu_estimators::regressor_extensions::RegressorExtension<T, DIM_N, DIM_P>#

Description

Kreisselmeier Filter#

template<typename T, int32_t DIM_N, int32_t DIM_P>
class Kreisselmeier : public sdu_estimators::regressor_extensions::RegressorExtension<T, DIM_N, DIM_P>#

Description

Math Utilities#

Riemannian Manifolds#

Manifold#

template<typename T, typename point, typename vector>
class Manifold#

This class provides a base class for a Riemannian manifold. Since we just need it for parameter estimation, we only implement the following functions:

  • Metric

  • Distance

  • Exponential

  • Logarithmic

  • Retraction

  • Euclidean gradient -> Riemannian gradient

  • Projection

Public Functions

virtual T dist(point &point_a, point &point_b) = 0#

The geodesic distance between two points on the manifold

Parameters:

  • point_a –

  • point_b –

Returns:

distance

virtual vector projection(point &point_, vector &vector_) = 0#

Projects vector to tangent space.

Parameters:

  • point_ –

  • vector_ –

Returns:

A vector in the tangent space at the given point.

virtual vector euclidean_to_riemannian_gradient(point &point_, vector &euclidean_gradient) = 0#

Converts from Euclidean gradient to Riemannian gradient.

Parameters:

  • point_ –

  • euclidean_gradient –

Returns:

Riemannian gradient.

virtual point retraction(point &point_, vector &tangent_vector) = 0#

Retraction from tangent space to the manifold.

Parameters:

  • point_ –

  • trangent_vector –

Returns:

virtual point exp(point &point_, vector &tangent_vector) = 0#

Exponential map on the manifold.

Parameters:

  • point_ –

  • tangent_vector –

Returns:

virtual vector log(point &point_a, point &point_b) = 0#

Logarithmic map on the manifold.

Parameters:

  • point_a –

  • point_b –

Returns:

Manifold of Spheres#

template<typename T, std::int32_t DIM_N>
class Sphere : private sdu_estimators::math::manifold::Manifold<T, Eigen::Vector<T, DIM_N>, Eigen::Vector<T, DIM_N>>#

Point: A n vector with unit length.

Vector: A n tangent vector.

Public Functions

inline virtual T dist(point &point_a, point &point_b)#

The geodesic distance between two points on the manifold

Parameters:

  • point_a –

  • point_b –

Returns:

distance

inline virtual vector projection(point &point_, vector &vector_)#

Projects vector to tangent space.

Parameters:

  • point_ –

  • vector_ –

Returns:

A vector in the tangent space at the given point.

inline virtual vector euclidean_to_riemannian_gradient(point &point_, vector &euclidean_gradient)#

Converts from Euclidean gradient to Riemannian gradient.

Parameters:

  • point_ –

  • euclidean_gradient –

Returns:

Riemannian gradient.

inline virtual point retraction(point &point_, vector &tangent_vector)#

Retraction from tangent space to the manifold.

Parameters:

  • point_ –

  • tangent_vector –

Returns:

inline virtual point exp(point &point_, vector &tangent_vector)#

Exponential map on the manifold.

Parameters:

  • point_ –

  • tangent_vector –

Returns:

inline virtual vector log(point &point_a, point &point_b)#

Logarithmic map on the manifold.

Parameters:

  • point_a –

  • point_b –

Returns:

Manifold of Symmetric and Positive Definite Matrices#

template<typename T, std::int32_t n>
class SymmetricPositiveDefinite : private sdu_estimators::math::manifold::Manifold<T, Eigen::Matrix<T, n, n>, Eigen::Matrix<T, n, n>>#

Point: A PSD n x n matrix.

Vector: A n x n matrix.

Public Functions

inline virtual T dist(point &point_a, point &point_b)#

The geodesic distance between two points on the manifold

Parameters:

  • point_a –

  • point_b –

Returns:

distance

inline virtual vector projection(point &point_, vector &vector_)#

Projects vector to tangent space.

Parameters:

  • point_ –

  • vector_ –

Returns:

A vector in the tangent space at the given point.

inline virtual vector euclidean_to_riemannian_gradient(point &point_, vector &euclidean_gradient)#

Converts from Euclidean gradient to Riemannian gradient.

Parameters:

  • point_ –

  • euclidean_gradient –

Returns:

Riemannian gradient.

inline virtual point retraction(point &point_, vector &tangent_vector)#

Retraction from tangent space to the manifold.

Parameters:

  • point_ –

  • tangent_vector –

Returns:

inline virtual point exp(point &point_, vector &tangent_vector)#

Exponential map on the manifold.

Parameters:

  • point_ –

  • tangent_vector –

Returns:

inline virtual vector log(point &point_a, point &point_b)#

Logarithmic map on the manifold.

Parameters:

  • point_a –

  • point_b –

Returns:

Persistencty of Excitation#

template<typename T, std::int32_t DIM_N, std::int32_t DIM_P>
class PersistencyOfExcitation#

A class for computing the persistency of excitation integral online.

The definition can be seen in e.g., [1] .

Defintion (Persistency of Excitation):

A function \( \phi : \mathbb{R}_+ \to \mathbb{R}^{n} \) is persistently exciting, noted as \( (\gamma, \beta) \)-PE, if there exists \( \gamma, \beta > 0 \) such that

\[\begin{equation} \int_{t}^{t + \gamma} \phi(\tau) \phi^\intercal(\tau) \, \mathrm{d}\tau \geq \beta I_n , \qquad t \geq 0, \end{equation}\]

where \( I \in \mathbb{R}^{n \times n} \) is the identity matrix.

As of now, the closed integral above is computed using the extended trapezoidal rule [4] .

Template Parameters:

  • T –

  • DIM_N –

  • DIM_P –

Public Functions

inline Eigen::Vector<T, DIM_P> get_eigen_values()#

Return the vector of eigenvalues of the current value of the PE-integral.

Returns:

Eigen::Vector<T, DIM_P> A vector of eigenvalues sorted in descending order.

State Space Estimators#

State Space Model#

template<typename T, int32_t DIM_Nx, int32_t DIM_Nu, int32_t DIM_Ny>
class StateSpaceModel#

Public Functions

inline StateSpaceModel(Eigen::Matrix<T, DIM_Nx, DIM_Nx> &A, Eigen::Matrix<T, DIM_Nx, DIM_Nu> &B, Eigen::Matrix<T, DIM_Ny, DIM_Nx> &C, Eigen::Matrix<T, DIM_Ny, DIM_Nu> &D, float Ts, utils::IntegrationMethod method)#

Constructor for continouos time model.

Parameters:

  • A –

  • B –

  • C –

  • D –

  • Ts –

  • method –

inline StateSpaceModel(Eigen::Matrix<T, DIM_Nx, DIM_Nx> &Ad, Eigen::Matrix<T, DIM_Nx, DIM_Nu> &Bd, Eigen::Matrix<T, DIM_Ny, DIM_Nx> &C, Eigen::Matrix<T, DIM_Ny, DIM_Nu> &D)#

Constructor for discrete model.

Parameters:

  • Ad –

  • Bd –

  • C –

  • D –

Luenberger Observer#

template<typename T, int32_t DIM_Nx, int32_t DIM_Nu, int32_t DIM_Ny>
class LuenbergerObserver#

Public Functions

inline LuenbergerObserver(Eigen::Matrix<T, DIM_Nx, DIM_Nx> &A, Eigen::Matrix<T, DIM_Nx, DIM_Nu> &B, Eigen::Matrix<T, DIM_Ny, DIM_Nx> &C, Eigen::Matrix<T, DIM_Nx, DIM_Ny> &L, float Ts, utils::IntegrationMethod method)#

Continous.

Parameters:

  • A –

  • B –

  • C –

  • L –

  • Ts –

  • method –

Utility Functions#

namespace utils#

Enums

enum IntegrationMethod#

Values:

enumerator Euler#
enumerator EulerBackwards#
enumerator Bilinear#
enumerator Exact#

Functions

template<typename T, int32_t DIM_Nx, int32_t DIM_Nu>
void c2d(Eigen::Matrix<T, DIM_Nx, DIM_Nx> &A, Eigen::Matrix<T, DIM_Nx, DIM_Nu> &B, float Ts, Eigen::Matrix<T, DIM_Nx, DIM_Nx> &Ad, Eigen::Matrix<T, DIM_Nx, DIM_Nu> &Bd, IntegrationMethod method)#
inline Eigen::MatrixXd obsv(Eigen::MatrixXd &A, Eigen::MatrixXd &C)#

Compute the observability matrix for state space systems.

Parameters:

  • A –

  • C –

Returns:

inline Eigen::MatrixXd obsvf(Eigen::MatrixXd &A, Eigen::MatrixXd &B, Eigen::MatrixXd &C, float tol = NAN)#
inline void ctrbf(Eigen::MatrixXd &A, Eigen::MatrixXd &B, Eigen::MatrixXd &C, float tol = NAN)#

Disturbance Observers#

Unknown Input Observer#

template<typename T, int32_t DIM_Nx, int32_t DIM_Nu, int32_t DIM_Ny>
class UnknownInputObserver#

Public Functions

inline UnknownInputObserver(Eigen::Matrix<T, DIM_Nx, DIM_Nx> &F, Eigen::Matrix<T, DIM_Nx, DIM_Nx> &Tmat, Eigen::Matrix<T, DIM_Nx, DIM_Nu> &B, Eigen::Matrix<T, DIM_Nx, DIM_Ny> &K, Eigen::Matrix<T, DIM_Nx, DIM_Ny> &H, float Ts, sdu_estimators::state_estimators::utils::IntegrationMethod method)#

The unknown input observer. The matrices F, T, K and H should be found following a design procedure in e.g., MATLAB or Python.

Parameters:

  • F –

  • Tmat –

  • B –

  • K –

  • H –

  • Ts –

  • method –

Momentum Observer#

class MomentumObserver#

Public Functions

template<typename ModelType>
inline MomentumObserver(const std::shared_ptr<ModelType> &model, double dt, const Eigen::VectorXd &K)#

Create the momentum observer from a model. It is assumed that the model provides the following methods:

  • get_inertia_matrix(const Eigen::VectorXd& q) -> Eigen::MatrixXd

  • get_coriolis(const Eigen::VectorXd& q, const Eigen::VectorXd& qd) -> Eigen::MatrixXd

  • get_gravity(const Eigen::VectorXd& q) -> Eigen::VectorXd

  • get_friction(const Eigen::VectorXd& qd) -> Eigen::VectorXd

Which should return the torque contributions for the given state.

Template Parameters:

ModelType – Type of the dynamic model

Parameters:

  • model – Shared pointer to the dynamic model

  • dt – Time step for the observer

  • K – Gain matrix for the observer

Throws:

std::invalid_argument – if the size of K does not match the model DOF.

inline MomentumObserver(const std::function<Eigen::MatrixXd(const Eigen::VectorXd&)> &get_inertia_matrix, const std::function<Eigen::MatrixXd(const Eigen::VectorXd&, const Eigen::VectorXd&)> &get_coriolis, const std::function<Eigen::VectorXd(const Eigen::VectorXd&)> &get_gravity, const std::function<Eigen::VectorXd(const Eigen::VectorXd&)> &get_friction, double dt, const Eigen::VectorXd &K)#

Create the momentum observer from function handles. The functions should return the torque contributions for the given state.

Parameters:

  • get_inertia_matrix – [in] Function to get the inertia matrix B(q)

  • get_coriolis – [in] Function to get the Coriolis forces C(q, qd)

  • get_gravity – [in] Function to get the gravity forces g(q)

  • get_friction – [in] Function to get the friction forces f(qd)

  • dt – [in] Time step for the observer

  • K – [in] Gain matrix for the observer

inline void reset()#

Reset the observer internal state.

inline void update(const Eigen::VectorXd &q, const Eigen::VectorXd &qd, const Eigen::VectorXd &tau)#

Update the observer with new measurements.

Parameters:

  • q – [in] Joint positions

  • qd – [in] Joint velocities

  • tau – [in] Measured joint torques

inline void update(const std::vector<double> &q, const std::vector<double> &qd, const std::vector<double> &tau_m)#

Update the observer with new measurements.

Parameters:

  • q – [in] Joint positions

  • qd – [in] Joint velocities

  • tau_m – [in] Measured joint torques

inline Eigen::VectorXd estimatedTorques() const#

Get the estimated external joint torques.

Returns:

Estimated external joint torques

inline Eigen::VectorXd getAccEstimate(const Eigen::VectorXd &q, const Eigen::VectorXd &qd, const Eigen::VectorXd &tau) const#

Get the estimated acceleration.

Parameters:

  • q – [in] Joint positions

  • qd – [in] Joint velocities

  • tau – [in] Measured joint torques

Returns:

Eigen::VectorXd

inline void zeroExternalFT()#

Zero the external force/torque estimate. This will adjust the internal state to make the current external torque estimate zero.

Contents