File base_ekf_with_imu_kin.hpp¶

Defines a class implementing a base state estimator using the imu and the kinematics information.

License:: BSD 3-clause

Copyright: Copyright (c) 2021, New York University and Max Planck Gesellschaft

namespace mim_estimation

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.

struct SystemState¶

#include <base_ekf_with_imu_kin.hpp>

System state data.

Public Members

Eigen::Vector3d position = Eigen::Vector3d::Zero()¶: Base/Imu position.

Eigen::Vector3d linear_velocity = Eigen::Vector3d::Zero()¶: Base/Imu linear velocity.

Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity()¶: Base/Imu linear attitude.

Eigen::Vector3d bias_accelerometer = Eigen::Vector3d::Zero()¶: Accelerometer bias.

Eigen::Vector3d bias_gyroscope = Eigen::Vector3d::Zero()¶: Gyroscope bias.

Eigen::MatrixXd covariance = Eigen::MatrixXd::Zero(state_dim, state_dim)¶: Process covariance.

Public Static Attributes

static const int state_dim = 15¶: State dimension.

static const int noise_dim = 12¶: Process noise dimension.