C++ API and example¶

1. Introduction¶

This page exist in order to extract the examples from the Doxygen documentation, Please have look at the end of this page there are all the examples.

2. C++ API and example¶

class BaseEkfWithImuKin

#include <base_ekf_with_imu_kin.hpp>

Extended Kalman Filter implementation for base state estimation using the imu and the kinematics information.

Details:

Todo:: explain here the math around the EKF

Public Functions

BaseEkfWithImuKin(): Construct a new Base Ekf With Imu and Kinematics object.

~BaseEkfWithImuKin(): Destroy the Base Ekf With Imu Kin object.

void initialize(const BaseEkfWithImuKinSettings &settings)

Get the EKF settings and initialize the filter from them.

Parameters:: settings –

void set_initial_state(Eigen::Ref<const Eigen::Vector3d> base_position, const Eigen::Quaterniond &base_attitude, Eigen::Ref<const Eigen::Vector3d> base_linear_velocity, Eigen::Ref<const Eigen::Vector3d> base_angular_velocity)

Set initial state.

Parameters:

base_position – Base position with respect to the world frame.
base_attitude – Base orientation with respect to the world frame.
base_linear_velocity – Base linear velocity with respect to the base frame.
base_angular_velocity – Base angular velocity with respect to the base frame.

void set_initial_state(Eigen::Ref<const Eigen::Matrix<double, 7, 1>> base_se3_position, Eigen::Ref<const Eigen::Matrix<double, 6, 1>> base_se3_velocity)

Set initial state.

Parameters:

base_se3_position – [XYZ Quaternion] SE3 representation of the base position with respect to the world frame.
base_se3_velocity – [Linear Angular] base velocity in the base frame.

void update_filter(const std::vector<bool> &contact_schedule, Eigen::Ref<const Eigen::Vector3d> imu_accelerometer, Eigen::Ref<const Eigen::Vector3d> imu_gyroscope, Eigen::Ref<const Eigen::VectorXd> joint_position, Eigen::Ref<const Eigen::VectorXd> joint_velocity)

Feed in the sensor raw data from the robot and update the filter output.

Parameters:

contact_schedule – vector of boolean, if one boolean is true it means that the corresponding end-effector is in contact.
imu_accelerometer – imu raw accelerometer data.
imu_gyroscope – imu raw gyroscope data.
joint_position – joint position data (in pinocchio ordering).
joint_velocity – joint velocity data (in pinocchio ordering).

void get_filter_output(Eigen::Ref<Eigen::VectorXd> robot_configuration, Eigen::Ref<Eigen::VectorXd> robot_velocity)

Get the filter output which is the robot state.

Parameters:

robot_configuration –
robot_velocity –

const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &get_measurement()

Get measurement.

Parameters:: root_velocities –

Private Functions

void compute_end_effectors_forward_kinematics(Eigen::Ref<const Eigen::VectorXd> joint_position, Eigen::Ref<const Eigen::VectorXd> joint_velocity): Compute the base/imu velocities.

void integrate_process_model(Eigen::Ref<const Eigen::Vector3d> imu_accelerometer, Eigen::Ref<const Eigen::Vector3d> imu_gyroscope)

Integrate the process model using the imu data.

compute mu_pre.

void compute_discrete_prediction_jacobian(): Fill in the process jacobians (cont_proc_jac_, disc_proc_jac_).

void compute_noise_jacobian(): Fill in the noise jacobian.

void construct_continuous_noise_covariance(): Use and additive white noise as continuous noise.

void construct_discrete_noise_covariance(): Discretise the noise covariance using zero-order hold and truncating higher-order terms.

void construct_discrete_measurement_noise_covariance(): Discretise the measurement noise covariance.

void prediction_step(): Propagate the state covariance (predicted_state_.covariance).

void measurement_model(const std::vector<bool> &contact_schedule, Eigen::Ref<const Eigen::VectorXd> joint_position, Eigen::Ref<const Eigen::VectorXd> joint_velocity)

Use the kinematics to measure the linear velocity of the base.

Parameters:

contact_schedule – This indicates which end-effector is currently in contact.
joint_position – joint positions.
joint_velocity – joint velocities.

void update_step(const std::vector<bool> &contact_schedule, Eigen::Ref<const Eigen::VectorXd> joint_position, Eigen::Ref<const Eigen::VectorXd> joint_velocity)

Update the current state posterior_state_ in function of the measurements.

Parameters:

contact_schedule – This indicates which end-effector is currently in contact.
joint_position – joint positions.
joint_velocity – joint velocities.

Private Members

BaseEkfWithImuKinSettings settings_

Eigen::VectorXd joint_position_: Joint positions reading.

Eigen::VectorXd joint_velocity_: Joint velocities reading.

Eigen::Vector3d imu_accelerometer_: Accelerometer reading.

Eigen::Vector3d imu_gyroscope_: Joint velocities.

SystemState predicted_state_: Predicted system state (next state).

SystemState posterior_state_: Posterior system state (current state).

pinocchio::Data pinocchio_data_: Rigid body dynamics data storage class.

std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> kin_ee_position_: Measured end-effectors positions expressed in the imu/base frame.

std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> kin_ee_velocity_: Measured end-effectors velocities expressed in the imu/base frame.

std::vector<pinocchio::FrameIndex> kin_ee_fid_: Pinocchio frame id corresponding to the end-effector potentially in contact.

Eigen::VectorXd q_kin_: Robot configuration with base at the center of the world.

Eigen::VectorXd dq_kin_: Robot velocity with static base frame.

Eigen::Matrix3d attitude_post_: Rotation matrix from the posterior state.

Eigen::Vector3d root_angular_velocity_: Imu/Base angular velocity.

Eigen::Vector3d root_angular_velocity_prev_: Imu/Base angular velocity previous value.

Eigen::Vector3d root_angular_acceleration_: Imu/Base angular acceleration.

Eigen::Vector3d root_linear_acceleration_: Imu/Base angular velocity.

Eigen::Vector3d gravity_: Gravity vector (default is negative along the z-axis).

Eigen::MatrixXd cont_proc_jac_: Continuous process jacobian.

Eigen::MatrixXd disc_proc_jac_: Discrete process jacobian.

Eigen::MatrixXd proc_noise_jac_: Process noise jacobian.

Eigen::MatrixXd cont_proc_noise_cov_: Continuous process noise covariance.

Eigen::MatrixXd disc_proc_noise_cov_: Discrete process noise covariance.

Eigen::MatrixXd disc_meas_noise_cov_: Discrete measurement noise covariance.

std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> kin_meas_root_velocity_: Linear velocity of the root (base/imu) measured from the kinematics.

Eigen::MatrixXd meas_jac_: Measurement jacobian.

Eigen::VectorXd meas_error_: Predicted base velocity, this is the predicted measurement.

Eigen::MatrixXd meas_covariance_: Measurement covariance.

Eigen::MatrixXd kalman_gain_: Kalman Gain computed during the update_step.

Eigen::LDLT<Eigen::MatrixXd> ldlt: LDLT decomposition to invert some matrix.

Eigen::VectorXd delta_state_: Update vector which is a delta state.

struct BaseEkfWithImuKinSettings

#include <base_ekf_with_imu_kin.hpp>

Subclassed by mim_estimation::RobotStateEstimatorSettings

Public Functions

inline virtual std::string to_string()

Public Members

bool is_imu_frame = true: Is the EKF estimating the imu or the base frame.

std::vector<std::string> end_effector_frame_names: Potential contact end effector frame names.

pinocchio::Model pinocchio_model: Rigid body dynamic model.

pinocchio::SE3 imu_in_base = pinocchio::SE3::Identity(): Imu position and orientation in the base frame.

double dt = 0.001: discretization time.

Eigen::Vector3d noise_accelerometer = 0.0001962 * 0.0001962 * (Eigen::Vector3d() << dt, dt, dt).finished(): Accelerometer noise covariance.

Eigen::Vector3d noise_gyroscope = 0.0000873 * 0.0000873 * (Eigen::Vector3d() << dt, dt, dt).finished(): Gyroscope noise covariance.

Eigen::Vector3d noise_bias_accelerometer = 0.0001 * 0.0001 * (Eigen::Vector3d() << 1.0, 1.0, 1.0).finished(): Accelerometer bias noise covariance.

Eigen::Vector3d noise_bias_gyroscope = 0.000309 * 0.000309 * (Eigen::Vector3d() << 1.0, 1.0, 1.0).finished(): Gyroscope bias noise covariance.

Eigen::Vector3d meas_noise_cov = (Eigen::Vector3d() << 1e-5, 1e-5, 1e-5).finished(): Continuous measurement noise covariance.

template<class classType, int _Order> class ButterworthFilter: #include <butterworth_filter.hpp>

General filter template

template<typename _Scalar, int _Rows, int _Order> class ButterworthFilter<Eigen::Matrix<_Scalar, _Rows, 1, Eigen::ColMajor, _Rows, 1>, _Order>

#include <butterworth_filter.hpp>

Specialization to an Eigen::Vector. This Filter class implements a butterworth low pass filter able to filter a vector of measurements. Only Eigen::Vectors or Eigen::Matrices with column dimension 1 are valid. Usage example: typedef Filter<Eigen::Matrix<double,6,1> > FilterType; INSTANTIATE FilterType myfilter; INITIALIZE myfilter.initialize(initial_state, filter_order, cuttoff_frequency); UPDATE myfilter.update(measurement); RETRIEVE EST myfilter.getData(estimate);

Public Types

typedef Eigen::Matrix<_Scalar, _Rows, 1, Eigen::ColMajor, _Rows, 1> VecToFilter: Input and Output type definition (VecToFilter)

Public Functions

ButterworthFilter()

~ButterworthFilter()

void initialize(const VecToFilter &current_state, double cutoff_frequency)

Function to initialize several parameters of the filter

Parameters:

current_state – [in] initial state of the system of type VecToFilter
filter_order – [in] order of the butterworth filter (integer >= 1)
cutoff_frequency – [in] cuttoff frequency for the filter as percentage of the nyquist frequency. For example, if sampling frequency is 200Hz; then the Nyquist frequency is half of the sampling frequency, namely 100Hz. cut_off frequency is a double element of [0-1], such that 0 corresponds to 0*Nyquist frequency and 1 corresponds to a cutoff_frequency equal to the Nyquist frequency.

void update(const VecToFilter &new_measurement)

Function to update the filtered state based on the new raw measurement

Parameters:: new_measurement – [in] new raw measurement of type VecToFilter

void getData(VecToFilter &current_state)

Function to ask for the filtered estimate

Parameters:: current_state – [out] filtered estimate of state of type VecToFilter

void getData(Eigen::Ref<ButterworthFilter::VecToFilter> current_state)

Private Functions

bool compute_filter_coefficients(): Function to compute the coefficients of a Butterworth filter without reading them from a table. It only needs to be done once at initialization. return true if successfull computing filter coefficients

void CT_transform(const std::vector<std::complex<double>> &Czeros, const std::vector<std::complex<double>> &Cpoles, double Cgain, double omega, bool flag, std::vector<std::complex<double>> &CTzeros, std::vector<std::complex<double>> &CTpoles, double &CTgain): Auxiliar function to compute the coefficients of a Butterworth filter.

void DT_transform(const std::vector<std::complex<double>> &CTzeros, const std::vector<std::complex<double>> &CTpoles, double CTgain, double T, std::vector<std::complex<double>> &DTzeros, std::vector<std::complex<double>> &DTpoles, double &DTgain): Auxiliar function to compute the coefficients of a Butterworth filter.

void poly_coeffs(const std::vector<std::complex<double>> &roots, Eigen::Matrix<double, _Order + 1, 1> &coeffs): Auxiliar function to compute the coefficients of a Butterworth filter.

Private Members

Eigen::Matrix<double, _Order + 1, 1> zeros_coeffs_vec_: storage of butterworth filter coefficients

Eigen::Matrix<double, _Order + 1, 1> poles_coeffs_vec_

bool initialized_filter_: logic variable

double cutoff_

order for the butterworth filter (Should be >=1)

cutoff frequency for the butterworth filter ([0-1] percentage of the nyquist frequency)

Eigen::Matrix<double, _Rows, _Order + 1> measurements_mat_: matrices to store past estimates and measurements for filtering

Eigen::Matrix<double, _Rows, _Order + 1> states_mat_

class DataCollector

#include <data_collector.hpp>

Subclassed by mim_estimation::io_tools::NonRtDataCollector

Public Functions

inline DataCollector()

inline virtual ~DataCollector()

virtual void addVariable(const double *data, const std::string &name, const std::string &units) = 0: virtual functions to add variables to data collection for different types

virtual void addVariable(const float *data, const std::string &name, const std::string &units) = 0

virtual void addVariable(const int *data, const std::string &name, const std::string &units) = 0

virtual void updateDataCollection() = 0: virtual function to update the collection with the recently added variables

virtual void stopDataCollection() = 0

virtual void startDataCollection() = 0

virtual bool isDataCollectionDone() = 0: virtual function to check whether data collection has completed:

void addVector(const Eigen::Ref<const Eigen::VectorXd> &data, const std::vector<std::string> &name, const std::vector<std::string> &units)

helper functions to unroll eigen vectors into simpler types that can be handled by the addVariable functions

arbitrary Eigen Vectors

void addVector(const Eigen::Ref<const Eigen::VectorXd> &data, const std::string &name, const std::vector<std::string> &extension, const std::vector<std::string> &units): extension contains the list of strings to be appended to name for each elem of data

void addVector(const Eigen::Ref<const Eigen::VectorXd> &data, const std::string &name, const std::string &unit): will just add 0,1,2..etc as extension to name

void addVector3d(const Eigen::Ref<const Eigen::Vector3d> &data, const std::string &name, const std::string &units): the actual recorded names will be name_x, name_y, name_z

void addVector3d(const Eigen::Ref<const Eigen::Vector3d> &data, const std::string &name, const std::string &units, const std::string &extension): the actual recorded names will be name_x, name_y, name_z plus extension

void addQuaternion(const Eigen::Ref<const Eigen::Vector4d> &data, const std::string &name): the actual recorded names will be name_q0, name_q1, name_q2, name_q3 with units “-”

void addQuaternion(const Eigen::Quaterniond &data, const std::string &name): the actual recorded names will be name_q0, name_q1, name_q2, name_q3 with units “-”

void addVector6d(const Eigen::Ref<const Eigen::Matrix<double, 6, 1>> &data, const std::string &name, const std::string &units): the actual recorded names will have extension x,y,z,a,b,g

void addMatrix(const Eigen::Ref<const Eigen::MatrixXd> &data, const std::string &name, const std::string &unit): arbitrary Eigen matrices (will just add matrix indes as extension to name, eg 11, 12, 21, 22

class DataReader

#include <data_reader.hpp>

Public Functions

inline DataReader()

inline ~DataReader()

void read(const std::string &fname)

read, reads a clmcplot file and fills the internal data structure.

Parameters:: fname – name of the d-file to read.

int getIndex(const std::string &stream_name)

findIndex, find the column index corresponding to the field name.

Parameters:: name – name of the field.
Returns:: the column index corresponding to the field name.

void getIndexes(const std::vector<std::string> &stream_names, std::vector<int> &indexes)

getIndexes:, find stream column indexes corresponding to the names.

Parameters:

names – list of the stream name.
indexes – the output list of indexes.

void fillPose(int row, const std::vector<int> &indexes, Eigen::Vector3d &pose, Eigen::Quaterniond &orientation)

fillPose, fill an existing pose object with the data.

Parameters:

row – row index homogenous to time/sampling_period.
indexes – the indexes of the data streams to copy in order: [x, y, z, qw, qx, qy, qz].
pose – the object to fill.

void fillVector(int row, const std::vector<int> &indexes, Eigen::Ref<Eigen::VectorXd> vec)

fillVector, fill an existing vector from the data

Parameters:

row – row index homogenous to time/sampling_period.
indexes – the indexes of the data streams to copy.
vec – the vector with the correct dimension that is filled

inline int getNbRows()

getNbRows, get the number of rows equivalent to the number of time step saved in the d-file

Returns:: the number of time step saved

inline double getValue(int row, int index)

getValue, get the value of a specific stream at a specific time

Parameters:

row – row index homogenous to time/sampling_period.
index – the index of the data streams to copy from.

Returns:

the value

inline double getFrequency()

getFrequence, get the getFrequence of the read data

Returns:: the frequence

Private Members

Eigen::MatrixXd data_

std::vector<std::string> var_names_

std::vector<std::string> units_

int nb_rows_

int nb_cols_

int buffer_size_

double frequency_

template<class S, class M> class EKF

#include <ekf.hpp>

This class implements an extended Kalman Filter (EKF).

This class implements a templated Extended Kalman Filter that receives a State class “S” and a measurement class “M”. Both templated class must have this following operator: -, +, =, <<. This operator must be eigen compatible. To achieve this in a compact way I strongly suggest inheriting directly from the class Eigen::Vector<N> (see Eigen documentation https://eigen.tuxfamily.org/dox/TopicCustomizing_InheritingMatrix.html).

The main sources of reading comes from

[1] “S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, 2000” (Chapter 3)
[2] “http://wolfweb.unr.edu/~fadali/EE782/NumEvalQphi.pdf”
[3] “https://en.wikipedia.org/wiki/Extended_Kalman_filter”

The notation used in this documentation is the one from “http://wolfweb.unr.edu/~fadali/EE782/NumEvalQphi.pdf”.

\[\begin{split}\begin{align} x_t = g(u_t, x_{t-1}) + \epsilon_t \\ z_t = h(x_t) + \delta_t \end{align}\end{split}\]

With \( x_t \) the system state, \( u_t \) the system control, \( g \) the process evolution, \( \epsilon_t \approx N(0, R_t) \) the process Gaussian noise, \( h \) the measurement evolution and \( \delta_t \approx N(0, Q_t) \) the measurement Gaussian noise. The first equation depicts the “process” in the code. The second equation represents the “measurement” in the code.

Using a taylor expansion of the previous equation we get:

\[\begin{split}\begin{align} x_t = g(u_t, \mu_{t-1}) + G_t \; * \; (x_{t-1} - \mu_{t-1}) + \epsilon_t \\ z_t = h(\mu_t) + H_t \; * \; (x_{t} - \mu_{t}) + \delta_t \end{align}\end{split}\]

With \( \mu_{t} \) the average of the estimated state of covariance \( \Sigma_t \). The equation of the EKF routine are defined with the “update_filter” function.

The mapping between the variable names and this piece of documentation is done next to each variable.

Public Functions

EKF(bool numerical_jac, bool is_discrete, double dt, unsigned lin_order = 2)

EKF, constructor of the class.

Parameters:

numerical_jac – do we compute the Jacobians analytically or not?
is_discrete – is the process discrete?
dt – the sampling period of the EKF.

inline ~EKF(): Default destructor.

void updateFilter(bool update_on)

updateFilter: compute a pass of the extended Kalman filter

From here on we only have access to \( \mu_{t-1} \) and \( \Sigma_{t-1} \) the previous estimated state along time. First of all we need to check if the system is a discrete one (see constructor). If the system is continuous we need to build a discrete approximation of it. In order to do so we use the Van Loan’s method (see [2] slide 15).

1/ Compute the dynamic matrix of the continuous process \( M \)
\[\begin{align} M = \left[ \left. \frac{-G_t}{0} \right| \frac{E_t R_t E_t^T}{G_t^T} \right] \end{align}\]
With \( E_t \) being the jacobian of the noise. In [2], \( E_t \) is also considered as the control jacobian in a way.
2/ Obtain the matrix M exponential
\[\begin{align} e^{M \Delta t} = \left[ \left. \frac{ e^{-G_t \Delta t} }{0} \right| \frac{ e^{-G_t \Delta t} F_{t, disc} }{ e^{G_t^T \Delta t} } \right] \end{align}\]
3/ Transpose the lower right corner to compute the discretized process noise covariance.
\[\begin{align} R_{t, disc} = e^{G_t \Delta t} = (e^{G_t^T \Delta t})^T \end{align}\]
4/ Compute the discretized process jacobian as follow:
\[\begin{align} F_{t, disc} = R_{t, disc} \;\;\; e^{M \Delta t}[\text{upper right corner}] \end{align}\]
In the following we will discard the \( {disc} \) subscript in the expression of the process Jacobian and noise covariance for sake of clarity. We assume that the system is either discrete or already discretized.

When the system has been discretized (or is discrete) we compute the EKF with the following procedure. First we need to predict what is supposed to be the current estimated state \( \tilde{\mu}_{t} \) and its Covariance \( \tilde{\Sigma}_{t} \) according to the model and the current control:

\[\begin{split}\begin{align} \tilde{\mu}_{t} &= g(u_t, \mu_{t-1}) \\ \tilde{\Sigma}_{t} &= G_t \Sigma_{t-1} G_t^T + R_t \end{align}\end{split}\]

The second step consist in introducing the actual measurement when one occur.

\[\begin{split}\begin{align} K_{t} &= \tilde{\Sigma}_{t} H_t^T (H_t \tilde{\Sigma}_t H_t^T + Q_t)^{-1} \\ \mu_{t} &= \tilde{\mu}_{t} + K_t (z_t - h(\tilde{\mu}_t)) \\ \Sigma_{t} &= (I - K_t H_t) \tilde{\Sigma}_{t} \end{align}\end{split}\]

Parameters:: update_on – is the measurement new?

virtual Eigen::Matrix<double, N, 1> getFilterState(void) = 0

getFilterState

Returns:: the current state estimate \( \mu_{t} \)

void printDebug(void): printDebug, print all the protected method of the class for debugging purposes

Public Static Attributes

static const int N = S::state_dim: State dimension

static const int P = S::noise_dim: Noise dimension

static const int K = M::meas_dim: Measurement dimension

Protected Functions

virtual Eigen::Matrix<double, N, 1> processModel(S &s) = 0

processModel, a model of the continuous/discrete state dynamics \( g(u_t, x_{t-1}) \)

Parameters:: s – \( [x_{t-1}^T \;\; u_t^T]^T \)
Returns:: the predicted state (Max: should not return anything and update state_pre_?)

virtual void formProcessJacobian(void) = 0: formProcessJacobian, compute the Jacobian of the process \( G_t \)

virtual void formProcessNoise(void) = 0: formProcessNoise, compute the covariance of the process noise \( R_t \)

virtual void formNoiseJacobian(void) = 0: formNoiseJacobian, compute the Jacobian of the process noise \( E_t \)

virtual Eigen::Matrix<double, K, 1> measModel(S &s) = 0

measModel, a model of the measurement dynamics \( h(x_t) \)

Parameters:: s – a state \( x_t \)
Returns:: the predicted measurement (Max: should not return anything and update meas_pred_?)

virtual void formMeasJacobian(void) = 0: formMeasJacobian, compute the Jacobian of the measurement \( H_t \)

virtual void formMeasNoise(void) = 0: formMeasJacobian, compute the measure covariance \( Q_t \)

virtual void formActualMeas(void) = 0: formMeasJacobian, compute the measurement from sensors \( z_t \)

void computeJacobians(Eigen::MatrixXd &G_t, Eigen::MatrixXd &H_t, double ds)

computeJacobians, compute the finite differentiation

Parameters:

G_t – [inout] the process jacobian to compute
H_t – [inout] the measurement jacobian
ds – [inout] the finite differentiation delta \( \frac{f(x+ds) - f(x-ds)}{2ds} \)

bool check_jacobians(bool evaluate)

check_the_jacobians, this function computes the jacobian from the discrete model and from the continuous jacobian discretized and compare them

Parameters:: evaluate – [in] true, the function is computing the check/false it does nothing
Returns:: a boolean that says if everything is alright. for Van Loan discretization, \( M \)

Protected Attributes

S state_pre_: Predicted state ( \( \tilde{\mu} \), \( \tilde{\Sigma} \))

S state_post_: State of the filter. One variable is used for the previous ( \( \mu_{t-1} \), \( \Sigma_{t-1} \)) and current state ( \( \mu_{t} \), \( \Sigma_{t} \))

M meas_actual_: Current measurement ( \( z_{t} \))

M meas_pred_: Predicted measurement ( \( h(\tilde{\mu}_t)\) )

Eigen::MatrixXd VL_mat_: for Van Loan discretization, \( e^{M dt} \)

Eigen::MatrixXd exp_VL_mat_: discretized/discrete process Jacobian, \( G_{t,disc}/G_t \)

Eigen::MatrixXd proc_jac_disc_: user input process Jacobian, \( G_t \)

Eigen::MatrixXd proc_jac_: Jacobian of the process noise, \( E_t \)

Eigen::MatrixXd noise_jac_: Jacobian of the measurement, \( H_t \)

Eigen::MatrixXd meas_jac_: user input covariance of the process noise \( R_t \)

Eigen::MatrixXd proc_noise_: discretized/discrete covariance of the process noise \( R_{t,disc}/R_t \)

Eigen::MatrixXd proc_noise_disc_: user input covariance of the measure noise \( Q_t \)

Eigen::MatrixXd meas_noise_: covariance of the process noise \( Q_t \) assuming a discrete measurement

Eigen::MatrixXd meas_noise_disc_: Kalman filter gain matrix \( K_t \)

Eigen::MatrixXd gain_mat_: LDLT decomposition to invert some matrix

Eigen::LDLT<Eigen::MatrixXd> ldlt: Do we compute the Jacobian numerically or analytically?

bool numerical_jac_: Is the process discrete?

bool is_discrete_: sampling period

double dt_: order of the process dynamic linearization

unsigned lin_order_

class EndEffectorForceEstimator

#include <end_effector_force_estimator.hpp>

Public Types

typedef std::map<std::string, pinocchio::FrameIndex> EndEffectorIdMap

typedef std::map<pinocchio::FrameIndex, MatrixX3, std::less<pinocchio::FrameIndex>, Eigen::aligned_allocator<std::pair<const pinocchio::FrameIndex, MatrixX3>>> MatrixX3Map

typedef std::map<pinocchio::FrameIndex, pinocchio::Data::Matrix6x, std::less<pinocchio::FrameIndex>, Eigen::aligned_allocator<std::pair<const pinocchio::FrameIndex, pinocchio::Data::Matrix6x>>> Matrix6xMap

typedef std::map<pinocchio::FrameIndex, Eigen::Matrix<double, 6, 1>, std::less<pinocchio::FrameIndex>, Eigen::aligned_allocator<std::pair<const pinocchio::FrameIndex, Eigen::Matrix<double, 6, 1>>>> EndEffectorForceMap

Public Functions

EndEffectorForceEstimator()

Construct a new EndEffectorForceEstimator object.

No memory allocation,