diff --git a/.gitignore b/.gitignore
index 0fcd422..8e8e975 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,5 +5,6 @@
/docs
/build
/test/build/
+/test/results/
/ros/devel/
/ros/build/
diff --git a/config/go1.json b/config/go1.json
index ade0fb6..2b33f33 100644
--- a/config/go1.json
+++ b/config/go1.json
@@ -45,6 +45,9 @@
"imu_angular_velocity_bias_covariance": [1e-5, 1e-5, 1e-5],
"imu_linear_acceleration_covariance": [1e-2, 1e-2, 1e-2],
"imu_linear_acceleration_bias_covariance": [1e-4, 1e-4, 1e-4],
+
+ "use_contacts_in_base_estimation": false,
+ "base_linear_velocity_covariance": [1e-1, 1e-1, 1e-1],
"contact_position_covariance": [1e-4, 1e-4, 1e-4],
"contact_orientation_covariance": null,
"contact_position_slip_covariance": [1e-6, 1e-6, 1e-6],
diff --git a/config/go2.json b/config/go2.json
index 42aba3e..c9ed8ad 100644
--- a/config/go2.json
+++ b/config/go2.json
@@ -45,6 +45,9 @@
"imu_angular_velocity_bias_covariance": [1e-5, 1e-5, 1e-5],
"imu_linear_acceleration_covariance": [1e-2, 1e-2, 1e-2],
"imu_linear_acceleration_bias_covariance": [1e-4, 1e-4, 1e-4],
+
+ "use_contacts_in_base_estimation": false,
+ "base_linear_velocity_covariance": [1e-1, 1e-1, 1e-1],
"contact_position_covariance": [1e-4, 1e-4, 1e-4],
"contact_orientation_covariance": null,
"contact_position_slip_covariance": [1e-6, 1e-6, 1e-6],
diff --git a/config/nao.json b/config/nao.json
index ebbedf7..90d3d0d 100644
--- a/config/nao.json
+++ b/config/nao.json
@@ -44,6 +44,9 @@
"imu_angular_velocity_bias_covariance": [1e-6, 1e-6, 1e-6],
"imu_linear_acceleration_covariance": [1e-3, 1e-3, 1e-3],
"imu_linear_acceleration_bias_covariance": [1e-5, 1e-5, 1e-5],
+
+ "use_contacts_in_base_estimation": false,
+ "base_linear_velocity_covariance": [1e-1, 1e-1, 1e-1],
"contact_position_covariance": [1e-6, 1e-6, 1e-6],
"contact_orientation_covariance": [1e-3, 1e-3, 1e-3],
"contact_position_slip_covariance": [1e-4, 1e-4, 1e-4],
diff --git a/config/valkyrie.json b/config/valkyrie.json
index a06908f..f91330a 100644
--- a/config/valkyrie.json
+++ b/config/valkyrie.json
@@ -40,10 +40,14 @@
"median_window": 13,
"outlier_detection": false,
"convergence_cycles": 0,
+
"imu_angular_velocity_covariance": [1e-4, 1e-4, 1e-4],
"imu_angular_velocity_bias_covariance": [1e-6, 1e-6, 1e-6],
"imu_linear_acceleration_covariance": [1e-3, 1e-3, 1e-3],
"imu_linear_acceleration_bias_covariance": [1e-5, 1e-5, 1e-5],
+
+ "use_contacts_in_base_estimation": false,
+ "base_linear_velocity_covariance": [1e-1, 1e-1, 1e-1],
"contact_position_covariance": [1e-6, 1e-6, 1e-6],
"contact_orientation_covariance": [1e-3, 1e-3, 1e-3],
"contact_position_slip_covariance": [1e-4, 1e-4, 1e-4],
@@ -56,6 +60,7 @@
"initial_base_position_covariance": [1.0, 1.0, 1.0],
"initial_base_orientation_covariance": [1.0, 1.0, 1.0],
"initial_base_linear_velocity_covariance": [1.0, 1.0, 1.0],
+
"initial_contact_position_covariance": [1.0, 1.0, 1.0],
"initial_contact_orientation_covariance": [1.0, 1.0, 1.0],
"initial_imu_linear_acceleration_bias_covariance": [1.0, 1.0, 1.0],
diff --git a/core/src/BaseEKF.cpp b/core/src/BaseEKF.cpp
new file mode 100644
index 0000000..88e7ee2
--- /dev/null
+++ b/core/src/BaseEKF.cpp
@@ -0,0 +1,262 @@
+/**
+ * Copyright (C) 2024 Stylianos Piperakis, Ownage Dynamics L.P.
+ * Serow is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * General Public License as published by the Free Software Foundation, version 3.
+ *
+ * Serow is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with Serow. If not,
+ * see .
+ **/
+#include "BaseEKF.hpp"
+
+#include "lie.hpp"
+
+namespace serow {
+
+void BaseEKF::init(const BaseState& state,
+ double g, double imu_rate, bool outlier_detection) {
+ g_ = Eigen::Vector3d(0.0, 0.0, -g);
+ outlier_detection_ = outlier_detection;
+ num_states_ = 15;
+ num_inputs_ = 12;
+ nominal_dt_ = 1.0 / imu_rate;
+ I_.setIdentity(num_states_, num_states_);
+
+ // Initialize state indices
+ v_idx_ = Eigen::Array3i::LinSpaced(0, 3);
+ r_idx_ = v_idx_ + 3;
+ p_idx_ = r_idx_ + 3;
+ bg_idx_ = p_idx_ + 3;
+ ba_idx_ = bg_idx_ + 3;
+
+ ng_idx_ = Eigen::Array3i::LinSpaced(0, 3);
+ na_idx_ = ng_idx_ + 3;
+ nbg_idx_ = na_idx_ + 3;
+ nba_idx_ = nbg_idx_ + 3;
+
+ // Initialize the error state covariance
+ P_ = I_;
+ P_(v_idx_, v_idx_) = state.base_linear_velocity_cov;
+ P_(r_idx_, r_idx_) = state.base_orientation_cov;
+ P_(p_idx_, p_idx_) = state.base_position_cov;
+ P_(bg_idx_, bg_idx_) = state.imu_angular_velocity_bias_cov;
+ P_(ba_idx_, ba_idx_) = state.imu_linear_acceleration_bias_cov;
+
+ // Compute some parts of the Input-Noise Jacobian once since they are constants
+ // gyro (0), acc (3), gyro_bias (6), acc_bias (9)
+ Lc_.setZero(num_states_, num_inputs_);
+ Lc_(v_idx_, na_idx_) = -Eigen::Matrix3d::Identity();
+ Lc_(r_idx_, ng_idx_) = -Eigen::Matrix3d::Identity();
+ Lc_(bg_idx_, nbg_idx_) = Eigen::Matrix3d::Identity();
+ Lc_(ba_idx_, nba_idx_) = Eigen::Matrix3d::Identity();
+
+ std::cout << "Base EKF Initialized Successfully" << std::endl;
+}
+
+std::tuple BaseEKF::computePredictionJacobians(
+ const BaseState& state, Eigen::Vector3d angular_velocity) {
+ angular_velocity -= state.imu_angular_velocity_bias;
+ const Eigen::Vector3d& v = state.base_linear_velocity;
+ const Eigen::Matrix3d& R = state.base_orientation.toRotationMatrix();
+
+ Eigen::MatrixXd Ac, Lc;
+ Lc = Lc_;
+ Lc(v_idx_, ng_idx_).noalias() = -lie::so3::wedge(v);
+
+ Ac.setZero(num_states_, num_states_);
+ Ac(v_idx_, v_idx_).noalias() = -lie::so3::wedge(angular_velocity);
+ Ac(v_idx_, r_idx_).noalias() = lie::so3::wedge(R.transpose() * g_);
+ Ac(v_idx_, bg_idx_).noalias() = -lie::so3::wedge(v);
+ Ac(v_idx_, ba_idx_).noalias() = -Eigen::Matrix3d::Identity();
+ Ac(r_idx_, r_idx_).noalias() = -lie::so3::wedge(angular_velocity);
+ Ac(r_idx_, bg_idx_).noalias() = -Eigen::Matrix3d::Identity();
+ Ac(p_idx_, v_idx_).noalias() = R;
+ Ac(p_idx_, r_idx_).noalias() = -R * lie::so3::wedge(v);
+
+ return std::make_tuple(Ac, Lc);
+}
+
+BaseState BaseEKF::predict(const BaseState& state, const ImuMeasurement& imu) {
+ double dt = nominal_dt_;
+ if (last_imu_timestamp_.has_value()) {
+ dt = imu.timestamp - last_imu_timestamp_.value();
+ }
+ if (dt < nominal_dt_ / 2) {
+ dt = nominal_dt_;
+ }
+ // Compute the state and input-state Jacobians
+ const auto& [Ac, Lc] = computePredictionJacobians(state, imu.angular_velocity);
+ // Euler Discretization - First order Truncation
+ Eigen::MatrixXd Ad = I_;
+ Ad += Ac * dt;
+
+ Eigen::MatrixXd Qc = Eigen::MatrixXd::Zero(num_inputs_, num_inputs_);
+ // Covariance Q with full state + biases
+ Qc(ng_idx_, ng_idx_) = imu.angular_velocity_cov;
+ Qc(na_idx_, na_idx_) = imu.linear_acceleration_cov;
+ Qc(nbg_idx_, nbg_idx_) = imu.angular_velocity_bias_cov;
+ Qc(nba_idx_, nba_idx_) = imu.linear_acceleration_bias_cov;
+
+ // Predict the state error covariance
+ Eigen::MatrixXd Qd = Ad * Lc * Qc * Lc.transpose() * Ad.transpose() * dt;
+ P_ = Ad * P_ * Ad.transpose() + Qd;
+
+ // Predict the state
+ const BaseState& predicted_state = computeDiscreteDynamics(
+ state, dt, imu.angular_velocity, imu.linear_acceleration);
+ last_imu_timestamp_ = imu.timestamp;
+ return predicted_state;
+}
+
+BaseState BaseEKF::computeDiscreteDynamics(
+ const BaseState& state, double dt, Eigen::Vector3d angular_velocity,
+ Eigen::Vector3d linear_acceleration) {
+ BaseState predicted_state = state;
+ angular_velocity -= state.imu_angular_velocity_bias;
+ linear_acceleration -= state.imu_linear_acceleration_bias;
+
+ // Nonlinear Process Model
+ // Linear velocity
+ const Eigen::Vector3d& v = state.base_linear_velocity;
+ const Eigen::Matrix3d& R = state.base_orientation.toRotationMatrix();
+ predicted_state.base_linear_velocity = v.cross(angular_velocity);
+ predicted_state.base_linear_velocity += R.transpose() * g_;
+ predicted_state.base_linear_velocity += linear_acceleration;
+ predicted_state.base_linear_velocity *= dt;
+ predicted_state.base_linear_velocity += v;
+ // Position
+ const Eigen::Vector3d& r = state.base_position;
+ predicted_state.base_position = R * v;
+ predicted_state.base_position *= dt;
+ predicted_state.base_position += r;
+
+ // Biases
+ predicted_state.imu_angular_velocity_bias = state.imu_angular_velocity_bias;
+ predicted_state.imu_linear_acceleration_bias = state.imu_linear_acceleration_bias;
+
+ // Orientation
+ predicted_state.base_orientation =
+ Eigen::Quaterniond(lie::so3::plus(R, angular_velocity * dt)).normalized();
+
+ return predicted_state;
+}
+
+BaseState BaseEKF::updateWithOdometry(const BaseState& state,
+ const Eigen::Vector3d& base_position,
+ const Eigen::Quaterniond& base_orientation,
+ const Eigen::Matrix3d& base_position_cov,
+ const Eigen::Matrix3d& base_orientation_cov) {
+ BaseState updated_state = state;
+
+ Eigen::MatrixXd H;
+ H.setZero(6, num_states_);
+ Eigen::MatrixXd R = Eigen::Matrix::Zero();
+
+ // Construct the innovation vector z
+ const Eigen::Vector3d& zp = base_position - state.base_position;
+ const Eigen::Vector3d& zq = lie::so3::minus(base_orientation, state.base_orientation);
+ Eigen::VectorXd z;
+ z.setZero(6);
+ z.head(3) = zp;
+ z.tail(3) = zq;
+
+ // Construct the linearized measurement matrix H
+ H.block(0, p_idx_[0], 3, 3) = Eigen::Matrix3d::Identity();
+ H.block(3, r_idx_[0], 3, 3) = Eigen::Matrix3d::Identity();
+
+ // Construct the measurement noise matrix R
+ R.topLeftCorner<3, 3>() = base_position_cov;
+ R.bottomRightCorner<3, 3>() = base_orientation_cov;
+
+ const Eigen::Matrix s = R + H * P_ * H.transpose();
+ const Eigen::MatrixXd& K = P_ * H.transpose() * s.inverse();
+ const Eigen::VectorXd& dx = K * z;
+
+ P_ = (I_ - K * H) * P_;
+ updateState(updated_state, dx, P_);
+
+ return updated_state;
+}
+
+
+BaseState BaseEKF::updateWithTwist(const BaseState& state,
+ const Eigen::Vector3d& base_linear_velocity,
+ const Eigen::Matrix3d& base_linear_velocity_cov) {
+ BaseState updated_state = state;
+
+ const Eigen::Matrix3d R_world_to_base = state.base_orientation.toRotationMatrix();
+ // Construct the linearized measurement matrix H
+ Eigen::MatrixXd H;
+ H.setZero(3, num_states_);
+ H.block(0, v_idx_[0], 3, 3) = R_world_to_base;
+ H.block(0, r_idx_[0], 3, 3) = -R_world_to_base * lie::so3::wedge(state.base_linear_velocity);
+
+ // Construct the innovation vector z
+ const Eigen::Vector3d z = base_linear_velocity - R_world_to_base * state.base_linear_velocity;
+
+ // Construct the measurement noise matrix R
+ const Eigen::Matrix3d R = base_linear_velocity_cov;
+
+ const Eigen::Matrix3d s = R + H * P_ * H.transpose();
+ const Eigen::MatrixXd K = P_ * H.transpose() * s.inverse();
+ const Eigen::VectorXd dx = K * z;
+
+ P_ = (I_ - K * H) * P_;
+ updateState(updated_state, dx, P_);
+
+ return updated_state;
+}
+
+
+BaseState BaseEKF::updateStateCopy(const BaseState& state, const Eigen::VectorXd& dx,
+ const Eigen::MatrixXd& P) const {
+ BaseState updated_state = state;
+ updated_state.base_position += dx(p_idx_);
+ updated_state.base_position_cov = P(p_idx_, p_idx_);
+ updated_state.base_linear_velocity += dx(v_idx_);
+ updated_state.base_linear_velocity_cov = P(v_idx_, v_idx_);
+ updated_state.base_orientation =
+ Eigen::Quaterniond(lie::so3::plus(state.base_orientation.toRotationMatrix(), dx(r_idx_)))
+ .normalized();
+ updated_state.base_orientation_cov = P(r_idx_, r_idx_);
+ updated_state.imu_angular_velocity_bias += dx(bg_idx_);
+ updated_state.imu_angular_velocity_bias_cov = P(bg_idx_, bg_idx_);
+ updated_state.imu_linear_acceleration_bias += dx(ba_idx_);
+ updated_state.imu_linear_acceleration_bias_cov = P(ba_idx_, ba_idx_);
+ return updated_state;
+}
+
+void BaseEKF::updateState(BaseState& state, const Eigen::VectorXd& dx,
+ const Eigen::MatrixXd& P) const {
+ state.base_position += dx(p_idx_);
+ state.base_position_cov = P(p_idx_, p_idx_);
+ state.base_linear_velocity += dx(v_idx_);
+ state.base_linear_velocity_cov = P(v_idx_, v_idx_);
+ state.base_orientation =
+ Eigen::Quaterniond(lie::so3::plus(state.base_orientation.toRotationMatrix(), dx(r_idx_)))
+ .normalized();
+ state.base_orientation_cov = P(r_idx_, r_idx_);
+ state.imu_angular_velocity_bias += dx(bg_idx_);
+ state.imu_angular_velocity_bias_cov = P(bg_idx_, bg_idx_);
+ state.imu_linear_acceleration_bias += dx(ba_idx_);
+ state.imu_linear_acceleration_bias_cov = P(ba_idx_, ba_idx_);
+}
+
+BaseState BaseEKF::update(const BaseState& state, const KinematicMeasurement& kin,
+ std::optional odom) {
+ BaseState updated_state =
+ updateWithTwist(state, kin.base_linear_velocity, kin.base_linear_velocity_cov);
+
+ if (odom.has_value()) {
+ updated_state =
+ updateWithOdometry(updated_state, odom->base_position, odom->base_orientation,
+ odom->base_position_cov, odom->base_orientation_cov);
+ }
+
+ return updated_state;
+}
+
+} // namespace serow
diff --git a/core/src/BaseEKF.hpp b/core/src/BaseEKF.hpp
new file mode 100644
index 0000000..b8bb3b2
--- /dev/null
+++ b/core/src/BaseEKF.hpp
@@ -0,0 +1,162 @@
+/**
+ * Copyright (C) 2024 Stylianos Piperakis, Ownage Dynamics L.P.
+ * Serow is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * General Public License as published by the Free Software Foundation, version 3.
+ *
+ * Serow is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with Serow. If not,
+ * see .
+ **/
+
+/**
+ * @file BaseEKF.hpp
+ * @brief Implementation of Extended Kalman Filter (EKF) for state estimation in humanoid robots.
+ * This EKF fuses data from an Inertial Measurement Unit (IMU), base linear velocity
+ * measurement, and optionally external odometry (e.g., Visual Odometry or Lidar
+ * Odometry). It models the state of the robot including base position, velocity,
+ * orientation in world frame coordinates as well as the gyro and accelerometer biases.
+ * @author Stylianos Piperakis
+ * @details The state estimation involves predicting and updating the robot's state based on sensor
+ * measurements and the robot's dynamics. The EKF integrates outlier detection mechanisms
+ * to improve state estimation robustness, incorporating beta distribution parameters and
+ * digamma function approximations. More information on the nonlinear state estimation for
+ * humanoid robot walking can be found in:
+ * https://www.researchgate.net/publication/326194869_Nonlinear_State_Estimation_for_Humanoid_Robot_Walking
+ */
+
+#pragma once
+
+#include
+
+#include "OutlierDetector.hpp"
+#include "Measurement.hpp" // Includes various sensor measurements
+#include "State.hpp" // Includes definitions of robot state variables
+
+namespace serow {
+
+/**
+ * @class BaseEKF
+ * @brief Implements an Extended Kalman Filter (EKF) for state estimation in humanoid robots,
+ * specifically for fusing IMU data, base linear velocity, and external odometry.
+ */
+class BaseEKF {
+ public:
+ /**
+ * @brief Initializes the EKF with the initial robot state and other parameters.
+ * @param state Initial state of the robot.
+ * @param g Acceleration due to gravity.
+ * @param imu_rate IMU update rate.
+ * @param outlier_detection Flag indicating if outlier detection mechanisms should be enabled.
+ */
+ void init(const BaseState& state, double g, double imu_rate, bool outlier_detection = false);
+
+ /**
+ * @brief Predicts the robot's state forward based on IMU.
+ * @param state Current state of the robot.
+ * @param imu IMU measurements.
+ * @return Predicted state after applying prediction step of the EKF.
+ */
+ BaseState predict(const BaseState& state, const ImuMeasurement& imu);
+
+ /**
+ * @brief Updates the robot's state based on kinematic measurements and optionally odometry
+ * measurements.
+ * @param state Current state of the robot.
+ * @param kin Kinematic measurements.
+ * @param odom Optional odometry measurements.
+ * @param terrain Optional terrain measurements.
+ * @return Updated state after applying update step of the EKF.
+ */
+ BaseState update(const BaseState& state, const KinematicMeasurement& kin,
+ std::optional odom = std::nullopt);
+
+ private:
+ int num_states_{}; ///< Number of state variables.
+ int num_inputs_{}; ///< Number of input variables.
+ bool outlier_detection_{}; ///< Flag indicating if outlier detection is enabled.
+ double nominal_dt_{}; ///< Nominal sampling time for prediction step.
+ Eigen::Vector3d g_; ///< Gravity vector.
+ // State indices
+ Eigen::Array3i v_idx_; ///< Indices for velocity state variables.
+ Eigen::Array3i r_idx_; ///< Indices for orientation state variables.
+ Eigen::Array3i p_idx_; ///< Indices for position state variables.
+ Eigen::Array3i bg_idx_; ///< Indices for gyro bias state variables.
+ Eigen::Array3i ba_idx_; ///< Indices for accelerometer bias state variables.
+ // Input indices
+ Eigen::Array3i ng_idx_; ///< Indices for IMU input variables.
+ Eigen::Array3i na_idx_; ///< Indices for kinematic input variables.
+ Eigen::Array3i nbg_idx_; ///< Indices for gyro bias input variables.
+ Eigen::Array3i nba_idx_; ///< Indices for accelerometer bias input variables.
+ std::optional last_imu_timestamp_; ///< Timestamp of the last IMU measurement.
+
+ /// Error Covariance, Linearized state transition model, Identity matrix, state uncertainty
+ /// matrix 15 x 15
+ Eigen::Matrix I_, P_;
+ /// Linearized state-input model 15 x 12
+ Eigen::Matrix Lc_;
+
+ OutlierDetector contact_outlier_detector; ///< Outlier detector instance.
+
+ /**
+ * @brief Computes discrete dynamics for the prediction step of the EKF.
+ * @param state Current state of the robot.
+ * @param dt Time step for prediction.
+ * @param angular_velocity Angular velocity measurements.
+ * @param linear_acceleration Linear acceleration measurements.
+ * @return Predicted state after applying discrete dynamics.
+ */
+ BaseState computeDiscreteDynamics(
+ const BaseState& state, double dt, Eigen::Vector3d angular_velocity,
+ Eigen::Vector3d linear_acceleration);
+
+ /**
+ * @brief Computes Jacobians for the prediction step of the EKF.
+ * @param state Current state of the robot.
+ * @param angular_velocity Angular velocity measurements.
+ * @return Tuple containing prediction Jacobians (state transition and input models).
+ */
+ std::tuple computePredictionJacobians(
+ const BaseState& state, Eigen::Vector3d angular_velocity);
+
+ BaseState updateWithTwist(const BaseState& state,
+ const Eigen::Vector3d& base_linear_velocity,
+ const Eigen::Matrix3d& base_linear_velocity_cov);
+
+ /**
+ * @brief Updates the robot's state based on odometry measurements.
+ * @param state Current state of the robot.
+ * @param base_position Position of the robot's base.
+ * @param base_orientation Orientation of the robot's base.
+ * @param base_position_cov Covariance of base position measurements.
+ * @param base_orientation_cov Covariance of base orientation measurements.
+ * @return Updated state after applying odometry updates.
+ */
+ BaseState updateWithOdometry(const BaseState& state, const Eigen::Vector3d& base_position,
+ const Eigen::Quaterniond& base_orientation,
+ const Eigen::Matrix3d& base_position_cov,
+ const Eigen::Matrix3d& base_orientation_cov);
+
+ /**
+ * @brief Updates the state of the robot with the provided state change and covariance matrix.
+ * @param state Current state of the robot (will be updated in-place).
+ * @param dx State change vector.
+ * @param P Covariance matrix of the state change.
+ */
+ void updateState(BaseState& state, const Eigen::VectorXd& dx, const Eigen::MatrixXd& P) const;
+
+ /**
+ * @brief Updates a copy of the robot's state with the provided state change and covariance
+ * matrix.
+ * @param state Current state of the robot (will not be modified).
+ * @param dx State change vector.
+ * @param P Covariance matrix of the state change.
+ * @return Updated state after applying the state change.
+ */
+ BaseState updateStateCopy(const BaseState& state, const Eigen::VectorXd& dx,
+ const Eigen::MatrixXd& P) const;
+};
+
+} // namespace serow
diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt
index 8fe88f8..aecc5b9 100644
--- a/core/src/CMakeLists.txt
+++ b/core/src/CMakeLists.txt
@@ -5,16 +5,19 @@ find_package(nlohmann_json REQUIRED)
set(serow_src ${serow_src}
Serow.cpp
State.cpp
+ BaseEKF.cpp
ContactEKF.cpp
CoMEKF.cpp
LegOdometry.cpp
ButterworthLPF.cpp
Differentiator.cpp
- DerivativeEstimator.cpp)
+ DerivativeEstimator.cpp
+ OutlierDetector.cpp)
set(serow_headers ${serow_headers}
Serow.hpp
State.hpp
+ BaseEKF.hpp
ContactEKF.hpp
CoMEKF.hpp
LegOdometry.hpp
@@ -26,7 +29,8 @@ set(serow_headers ${serow_headers}
Mahony.hpp
Measurement.hpp
MovingMedianFilter.hpp
- RobotKinematics.hpp)
+ RobotKinematics.hpp
+ OutlierDetector.hpp)
add_library(serow SHARED ${serow_headers} ${serow_src})
target_link_libraries(serow PRIVATE Eigen3::Eigen pinocchio::pinocchio nlohmann_json::nlohmann_json)
diff --git a/core/src/ContactEKF.cpp b/core/src/ContactEKF.cpp
index 26b1951..ec9fa8a 100644
--- a/core/src/ContactEKF.cpp
+++ b/core/src/ContactEKF.cpp
@@ -16,51 +16,6 @@
namespace serow {
-double OutlierDetector::computePsi(double xxx) {
- double result = 0;
- double xx, xx2, xx4;
- for (; xxx < 7; ++xxx) {
- result -= 1 / xxx;
- }
-
- xxx -= 1.0 / 2.0;
- xx = 1.0 / xxx;
- xx2 = xx * xx;
- xx4 = xx2 * xx2;
- result += std::log(xxx) + (1.0 / 24.0) * xx2 - (7.0 / 960.0) * xx4 +
- (31.0 / 8064.0) * xx4 * xx2 - (127.0 / 30720.0) * xx4 * xx4;
- return result;
-}
-
-void OutlierDetector::init() {
- zeta = 1.0;
- e_t = e_0;
- f_t = f_0;
-}
-
-void OutlierDetector::estimate(const Eigen::Matrix3d& BetaT, const Eigen::Matrix3d& R) {
- double efpsi = computePsi(e_t + f_t);
- double lnp = computePsi(e_t) - efpsi;
- double ln1_p = computePsi(f_t) - efpsi;
-
- double pzeta_1 = std::exp(lnp - 0.5 * (BetaT * R.inverse()).trace());
- double pzeta_0 = std::exp(ln1_p);
-
- // Normalization factor
- double norm_factor = 1.0 / (pzeta_1 + pzeta_0);
-
- // p(zeta) are now proper probabilities
- pzeta_1 = norm_factor * pzeta_1;
- pzeta_0 = norm_factor * pzeta_0;
-
- // Mean of Bernulli
- zeta = pzeta_1 / (pzeta_1 + pzeta_0);
-
- // Update epsilon and f
- e_t = e_0 + zeta;
- f_t = f_0 + 1.0 - zeta;
-}
-
void ContactEKF::init(const BaseState& state, std::set contacts_frame, bool point_feet,
double g, double imu_rate, bool outlier_detection) {
num_leg_end_effectors_ = contacts_frame.size();
@@ -414,9 +369,9 @@ BaseState ContactEKF::updateWithOdometry(const BaseState& state,
R.topLeftCorner<3, 3>() = base_position_cov;
R.bottomRightCorner<3, 3>() = base_orientation_cov;
- const Eigen::Matrix3d& s = R + H * P_ * H.transpose();
- const Eigen::MatrixXd& K = P_ * H.transpose() * s.inverse();
- const Eigen::VectorXd& dx = K * z;
+ const Eigen::Matrix s = R + H * P_ * H.transpose();
+ const Eigen::MatrixXd K = P_ * H.transpose() * s.inverse();
+ const Eigen::VectorXd dx = K * z;
P_ = (I_ - K * H) * P_;
updateState(updated_state, dx, P_);
diff --git a/core/src/ContactEKF.hpp b/core/src/ContactEKF.hpp
index 3a9565a..e3c92b6 100644
--- a/core/src/ContactEKF.hpp
+++ b/core/src/ContactEKF.hpp
@@ -32,45 +32,12 @@
#include
+#include "OutlierDetector.hpp"
#include "Measurement.hpp" // Includes various sensor measurements
#include "State.hpp" // Includes definitions of robot state variables
namespace serow {
-/**
- * @struct OutlierDetector
- * @brief Implements outlier detection mechanisms using beta distribution parameters for state
- * estimation in humanoid robots.
- */
-struct OutlierDetector {
- double zeta = 1.0; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
- double f_0 = 0.1; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
- double e_0 = 0.9; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
- double f_t = 0.1; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
- double e_t = 0.9; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
- double threshold = 1e-5;
- size_t iters = 4; ///< Number of iterations for outlier detection
-
- /**
- * @brief Computes the digamma function approximation.
- * @param x Argument for digamma function.
- * @return Computed value of digamma function.
- */
- double computePsi(double x);
-
- /**
- * @brief Initializes the outlier detection process.
- */
- void init();
-
- /**
- * @brief Estimates the outlier indicator zeta based on provided matrices.
- * @param BetaT Matrix used in outlier estimation.
- * @param R Matrix used in outlier estimation.
- */
- void estimate(const Eigen::Matrix3d& BetaT, const Eigen::Matrix3d& R);
-};
-
/**
* @class ContactEKF
* @brief Implements an Extended Kalman Filter (EKF) for state estimation in humanoid robots,
diff --git a/core/src/Measurement.hpp b/core/src/Measurement.hpp
index 29a68e4..5346244 100644
--- a/core/src/Measurement.hpp
+++ b/core/src/Measurement.hpp
@@ -73,6 +73,7 @@ struct GroundReactionForceMeasurement {
*/
struct KinematicMeasurement {
double timestamp{}; ///< Timestamp of the measurement (s)
+ Eigen::Vector3d base_linear_velocity{Eigen::Vector3d::Zero()}; ///< Base linear velocity (m/s)
std::map contacts_status; ///< Map of contact status for different parts (0 or 1)
std::map contacts_probability; ///< Map of contact probabilities ([0, 1])
std::map contacts_position; ///< Map of contact positions relative to base frame (m)
@@ -82,6 +83,7 @@ struct KinematicMeasurement {
Eigen::Vector3d com_angular_momentum_derivative{Eigen::Vector3d::Zero()}; ///< Derivative of center of mass (COM) angular momentum (Nm)
Eigen::Vector3d com_position{Eigen::Vector3d::Zero()}; ///< Center of mass (COM) position (m)
Eigen::Vector3d com_linear_acceleration{Eigen::Vector3d::Zero()}; ///< Center of mass (COM) linear acceleration (m/s^2)
+ Eigen::Matrix3d base_linear_velocity_cov{Eigen::Matrix3d::Identity()}; ///< Covariance of base linear velocity (m^2/s^2)
Eigen::Matrix3d position_slip_cov{Eigen::Matrix3d::Identity()}; ///< Covariance of position slip (m^2)
Eigen::Matrix3d orientation_slip_cov{Eigen::Matrix3d::Identity()}; ///< Covariance of orientation slip (rad^2)
Eigen::Matrix3d position_cov{Eigen::Matrix3d::Identity()}; ///< Covariance of position (m^2)
diff --git a/core/src/OutlierDetector.cpp b/core/src/OutlierDetector.cpp
new file mode 100644
index 0000000..c6fd015
--- /dev/null
+++ b/core/src/OutlierDetector.cpp
@@ -0,0 +1,64 @@
+/**
+ * Copyright (C) 2024 Stylianos Piperakis, Ownage Dynamics L.P.
+ * Serow is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * General Public License as published by the Free Software Foundation, version 3.
+ *
+ * Serow is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with Serow. If not,
+ * see .
+ **/
+#include "OutlierDetector.hpp"
+
+
+namespace serow {
+
+double OutlierDetector::computePsi(double xxx) {
+ double result = 0;
+ double xx, xx2, xx4;
+ for (; xxx < 7; ++xxx) {
+ result -= 1 / xxx;
+ }
+
+ xxx -= 1.0 / 2.0;
+ xx = 1.0 / xxx;
+ xx2 = xx * xx;
+ xx4 = xx2 * xx2;
+ result += std::log(xxx) + (1.0 / 24.0) * xx2 - (7.0 / 960.0) * xx4 +
+ (31.0 / 8064.0) * xx4 * xx2 - (127.0 / 30720.0) * xx4 * xx4;
+ return result;
+}
+
+void OutlierDetector::init() {
+ zeta = 1.0;
+ e_t = e_0;
+ f_t = f_0;
+}
+
+void OutlierDetector::estimate(const Eigen::Matrix3d& BetaT, const Eigen::Matrix3d& R) {
+ double efpsi = computePsi(e_t + f_t);
+ double lnp = computePsi(e_t) - efpsi;
+ double ln1_p = computePsi(f_t) - efpsi;
+
+ double pzeta_1 = std::exp(lnp - 0.5 * (BetaT * R.inverse()).trace());
+ double pzeta_0 = std::exp(ln1_p);
+
+ // Normalization factor
+ double norm_factor = 1.0 / (pzeta_1 + pzeta_0);
+
+ // p(zeta) are now proper probabilities
+ pzeta_1 = norm_factor * pzeta_1;
+ pzeta_0 = norm_factor * pzeta_0;
+
+ // Mean of Bernulli
+ zeta = pzeta_1 / (pzeta_1 + pzeta_0);
+
+ // Update epsilon and f
+ e_t = e_0 + zeta;
+ f_t = f_0 + 1.0 - zeta;
+}
+
+
+} // namespace serow
diff --git a/core/src/OutlierDetector.hpp b/core/src/OutlierDetector.hpp
new file mode 100644
index 0000000..2a5292e
--- /dev/null
+++ b/core/src/OutlierDetector.hpp
@@ -0,0 +1,76 @@
+/**
+ * Copyright (C) 2024 Stylianos Piperakis, Ownage Dynamics L.P.
+ * Serow is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * General Public License as published by the Free Software Foundation, version 3.
+ *
+ * Serow is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with Serow. If not,
+ * see .
+ **/
+
+/**
+ * @file ContactEKF.hpp
+ * @brief Implementation of Extended Kalman Filter (EKF) for state estimation in humanoid robots.
+ * This EKF fuses data from an Inertial Measurement Unit (IMU), relative to the base leg
+ * contact measurements, and optionally external odometry (e.g., Visual Odometry or Lidar
+ * Odometry). It models the state of the robot including base position, velocity,
+ * orientation, gyro and accelerometer biases, leg contact positions, and contact orientations in
+ * world frame coordinates.
+ * @author Stylianos Piperakis
+ * @details The state estimation involves predicting and updating the robot's state based on sensor
+ * measurements and the robot's dynamics. The EKF integrates outlier detection mechanisms
+ * to improve state estimation robustness, incorporating beta distribution parameters and
+ * digamma function approximations. More information on the nonlinear state estimation for
+ * humanoid robot walking can be found in:
+ * https://www.researchgate.net/publication/326194869_Nonlinear_State_Estimation_for_Humanoid_Robot_Walking
+ */
+
+#pragma once
+
+#ifdef __linux__
+#include
+#else
+#include
+#endif
+#include
+
+namespace serow {
+
+/**
+ * @struct OutlierDetector
+ * @brief Implements outlier detection mechanisms using beta distribution parameters for state
+ * estimation in humanoid robots.
+ */
+struct OutlierDetector {
+ double zeta = 1.0; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
+ double f_0 = 0.1; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
+ double e_0 = 0.9; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
+ double f_t = 0.1; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
+ double e_t = 0.9; ///< Parameter for outlier detection: https://www.researchgate.net/publication/334745931_Outlier-Robust_State_Estimation_for_Humanoid_Robots
+ double threshold = 1e-5;
+ size_t iters = 4; ///< Number of iterations for outlier detection
+
+ /**
+ * @brief Computes the digamma function approximation.
+ * @param x Argument for digamma function.
+ * @return Computed value of digamma function.
+ */
+ double computePsi(double x);
+
+ /**
+ * @brief Initializes the outlier detection process.
+ */
+ void init();
+
+ /**
+ * @brief Estimates the outlier indicator zeta based on provided matrices.
+ * @param BetaT Matrix used in outlier estimation.
+ * @param R Matrix used in outlier estimation.
+ */
+ void estimate(const Eigen::Matrix3d& BetaT, const Eigen::Matrix3d& R);
+};
+
+} // namespace serow
diff --git a/core/src/Serow.cpp b/core/src/Serow.cpp
index ccdfc14..fbc1157 100644
--- a/core/src/Serow.cpp
+++ b/core/src/Serow.cpp
@@ -302,6 +302,18 @@ bool Serow::initialize(const std::string& config_file) {
return false;
}
+ if (!config["use_contacts_in_base_estimation"].is_boolean()) {
+ std::cerr << "Configuration: use_contacts_in_base_estimation must be boolean \n";
+ return false;
+ }
+ params_.is_contact_ekf = config["use_contacts_in_base_estimation"];
+
+ if (!config["base_linear_velocity_covariance"].is_array() ||
+ config["base_linear_velocity_covariance"].size() != 3) {
+ std::cerr << "Configuration: base_linear_velocity_covariance must be an array of 3 elements \n";
+ return false;
+ }
+
if (!config["contact_position_covariance"].is_array() ||
config["contact_position_covariance"].size() != 3) {
std::cerr << "Configuration: contact_position_covariance must be an array of 3 elements \n";
@@ -421,11 +433,16 @@ bool Serow::initialize(const std::string& config_file) {
return false;
}
params_.linear_acceleration_bias_cov[i] = config["imu_linear_acceleration_bias_covariance"][i];
+ if (!config["base_linear_velocity_covariance"][i].is_number_float()) {
+ std::cerr << "Configuration: base_linear_velocity_covariance[" << i << "] must be float \n";
+ return false;
+ }
+ params_.base_linear_velocity_cov[i] = config["base_linear_velocity_covariance"][i];
if (!config["contact_position_covariance"][i].is_number_float()) {
std::cerr << "Configuration: contact_position_covariance[" << i << "] must be float \n";
return false;
}
- params_.contact_position_cov[i] = config["contact_position_covariance"][i];
+ params_.contact_position_cov[i] = config["contact_position_covariance"][i];
if (!config["contact_orientation_covariance"].is_null()) {
if (!config["contact_orientation_covariance"][i].is_number_float()) {
std::cerr << "Configuration: contact_orientation_covariance[" << i
@@ -805,8 +822,13 @@ void Serow::filter(ImuMeasurement imu, std::map j
leg_odometry_ = std::make_unique(
base_to_foot_positions, base_to_foot_orientations, params_.mass, params_.tau_0,
params_.tau_1, params_.joint_rate, params_.g, params_.eps);
- base_estimator_.init(state.base_state_, state.getContactsFrame(), state.isPointFeet(),
- params_.g, params_.imu_rate, params_.outlier_detection);
+
+ if (params_.is_contact_ekf) {
+ base_estimator_con_.init(state.base_state_, state.getContactsFrame(), state.isPointFeet(),
+ params_.g, params_.imu_rate, params_.outlier_detection);
+ } else {
+ base_estimator_.init(state.base_state_, params_.g, params_.imu_rate, params_.outlier_detection);
+ }
com_estimator_.init(state.centroidal_state_, params_.mass, params_.g,
params_.force_torque_rate);
}
@@ -828,6 +850,8 @@ void Serow::filter(ImuMeasurement imu, std::map j
KinematicMeasurement kin;
kin.contacts_status = state.contact_state_.contacts_status;
kin.contacts_probability = state.contact_state_.contacts_probability;
+ kin.base_linear_velocity = leg_odometry_->getBaseLinearVelocity();
+ kin.base_linear_velocity_cov = params_.base_linear_velocity_cov.asDiagonal();
kin.contacts_position = leg_odometry_->getContactPositions();
kin.position_cov = params_.contact_position_cov.asDiagonal();
kin.position_slip_cov = params_.contact_position_slip_cov.asDiagonal();
@@ -855,8 +879,11 @@ void Serow::filter(ImuMeasurement imu, std::map j
// Call the base estimator predict step utilizing imu and contact status measurements
state.base_state_.timestamp = imu.timestamp;
- state.base_state_ = base_estimator_.predict(state.base_state_, imu, kin);
-
+ if (params_.is_contact_ekf) {
+ state.base_state_ = base_estimator_con_.predict(state.base_state_, imu, kin);
+ } else {
+ state.base_state_ = base_estimator_.predict(state.base_state_, imu);
+ }
// Call the base estimator update step by employing relative contact pose, odometry and terrain
// measurements
if (odom.has_value()) {
@@ -869,7 +896,25 @@ void Serow::filter(ImuMeasurement imu, std::map j
params_.T_base_to_odom.linear().transpose();
}
state.base_state_.timestamp = kin.timestamp;
- state.base_state_ = base_estimator_.update(state.base_state_, kin, odom, terrain_);
+
+ if (params_.is_contact_ekf) {
+ state.base_state_ = base_estimator_con_.update(state.base_state_, kin, odom, terrain_);
+ } else {
+ state.base_state_ = base_estimator_.update(state.base_state_, kin, odom);
+ std::map con_orient;
+ for (const auto& frame : state.getContactsFrame()) {
+ const Eigen::Isometry3d base_pose = state.getBasePose();
+ state.base_state_.contacts_position[frame] =
+ base_pose * kin.contacts_position.at(frame);
+ if (!state.isPointFeet()) {
+ con_orient[frame] = Eigen::Quaterniond(
+ base_pose.linear() * kin.contacts_orientation->at(frame).toRotationMatrix());
+ }
+ }
+ if (!state.isPointFeet()) {
+ state.base_state_.contacts_orientation = std::move(con_orient);
+ }
+ }
// Estimate the angular acceleration using the gyro velocity
if (!gyro_derivative_estimator) {
@@ -935,10 +980,11 @@ void Serow::filter(ImuMeasurement imu, std::map j
double den = 0.0;
for (const auto& frame : state.getContactsFrame()) {
grf.timestamp = ft->at(frame).timestamp;
+ const Eigen::Isometry3d foot_pose = state.getFootPose(frame);
if (state.contact_state_.contacts_probability.at(frame) > 0.0) {
- grf.force += state.getContactPose(frame)->linear() * ft->at(frame).force;
+ grf.force += foot_pose.linear() * ft->at(frame).force;
grf.cop += state.contact_state_.contacts_probability.at(frame) *
- (*state.getContactPose(frame) * ft->at(frame).cop);
+ (foot_pose * ft->at(frame).cop);
den += state.contact_state_.contacts_probability.at(frame);
}
}
diff --git a/core/src/Serow.hpp b/core/src/Serow.hpp
index 1fbc0af..7c77312 100644
--- a/core/src/Serow.hpp
+++ b/core/src/Serow.hpp
@@ -16,6 +16,7 @@
#include