Skip to content

Commit

Permalink
Estimate test (#197)
Browse files Browse the repository at this point in the history
* initial commit on state estimation

* ekf can publish estimated msgs by copy ground_truth msgs

* added update imu step

* initial estimation using imu. Tested using 002.bag file

* robot is able to estimate rotation

* clean up the code, estimate and ground truth starts at the same position

* estimate using imu data, comment out quadKD code

* Update ekf_estimator.h

* Made changes and tested the code according to Joe's review.

* rename Subscriber for joint encoder, revert example_with_terrain.rviz
  • Loading branch information
qishuny authored Feb 15, 2022
1 parent 3e02aba commit e4f92e1
Showing 14 changed files with 3,833 additions and 39 deletions.
3 changes: 2 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
@@ -4,4 +4,5 @@ docs/
**/build/
*.vscode
external/ipopt/*
!external/ipopt/.gitkeep
!external/ipopt/.gitkeep
*.csv
1 change: 1 addition & 0 deletions ekf_estimator/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
sensor_msgs
quad_msgs
quad_utils
)

## System dependencies are found with CMake's conventions
5 changes: 4 additions & 1 deletion ekf_estimator/ekf_estimator.yaml
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
ekf_estimator:
joint_state_max_time: 20 # Milliseconds until we disregard last joint state message
update_rate: 100
update_rate: 100
bias_x: 0.08 # actual measurement of x is 0.083616044
bias_y: 0.06 # actual measurement of y is 0.060205998
bias_z: -9.800917657 # actual measurement of z is 0.060205998
42 changes: 42 additions & 0 deletions ekf_estimator/include/ekf_estimator/ekf_estimator.h
Original file line number Diff line number Diff line change
@@ -9,6 +9,10 @@
#include <quad_msgs/RobotState.h>
#include <quad_msgs/ContactMode.h>

#include <quad_utils/ros_utils.h>
#include <quad_utils/quad_kd.h>
#include <vector>

//! Implements online EKF based state estimation
/*!
EKFEstimator implements all estimator logic. It should expose a constructor that does any initialization required and an update method called at some frequency.
@@ -28,6 +32,13 @@ class EKFEstimator {
void spin();

private:

/**
* @brief Callback function to handle new ground truth data
* @param[in] msg ground_truth_msg quad_msgs<RobotState>
*/
void groundtruthCallback(const quad_msgs::RobotState::ConstPtr& msg);

/**
* @brief Callback function to handle new joint encoder data
* @param[in] joint_encoder_msg sensor_msgs<JointState> containing joint pos,vel,current
@@ -52,6 +63,16 @@ class EKFEstimator {
*/
quad_msgs::RobotState updateStep();


const int numofStates = 6;
const int num_feet_ = 4;

/// Subscriber for ground_truth RobotState messages
ros::Subscriber state_ground_truth_sub_;

/// Last state ground_truth
quad_msgs::RobotState::ConstPtr last_state_msg_;

/// Subscriber for joint encoder messages
ros::Subscriber joint_encoder_sub_;

@@ -85,5 +106,26 @@ class EKFEstimator {
/// Maximum amount of time to still use joint state message in EKF data
double joint_state_msg_time_diff_max_;

// state vector
Eigen::VectorXd x;

// old state vector
Eigen::VectorXd last_x;

// previous time variable
ros::Time last_time;

// IMU bias vector
Eigen::VectorXd imu_bias;

// IMU bias in acc x
double bias_x_;

// IMU bias in acc x
double bias_y_;

// IMU bias in acc z
double bias_z_;

};
#endif // EKF_ESTIMATOR_H
3 changes: 3 additions & 0 deletions ekf_estimator/package.xml
Original file line number Diff line number Diff line change
@@ -53,17 +53,20 @@
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>quad_msgs</build_depend>
<build_depend>quad_utils</build_depend>
<build_depend>eigen</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>quad_msgs</build_export_depend>
<build_export_depend>quad_utils</build_export_depend>
<build_export_depend>eigen</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>eigen</exec_depend>
<exec_depend>quad_msgs</exec_depend>
<exec_depend>quad_utils</exec_depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
173 changes: 138 additions & 35 deletions ekf_estimator/src/ekf_estimator.cpp
Original file line number Diff line number Diff line change
@@ -1,22 +1,42 @@
#include "ekf_estimator/ekf_estimator.h"



EKFEstimator::EKFEstimator(ros::NodeHandle nh) {
nh_ = nh;

// Load rosparams from parameter server
std::string joint_encoder_topic, imu_topic, contact_topic, state_estimate_topic;
nh.param<std::string>("topics/joint_encoder", joint_encoder_topic, "/joint_encoder");
nh.param<std::string>("topics/imu", imu_topic, "/imu");
nh.param<std::string>("topics/mcu/state/jointURDF", joint_encoder_topic, "/mcu/state/jointURDF");
nh.param<std::string>("topics/mcu/state/imu", imu_topic, "/mcu/state/imu");
nh.param<std::string>("topics/state/estimate", state_estimate_topic, "/state/estimate");
nh.param<std::string>("topics/contact_mode", contact_topic, "/contact_mode");
nh.param<double>("ekf_estimator/update_rate", update_rate_, 200);
nh.param<double>("ekf_estimator/joint_state_max_time",joint_state_msg_time_diff_max_,20);

// Load IMU bias
nh.getParam("/ekf_estimator/bias_x", bias_x_);
nh.getParam("/ekf_estimator/bias_y", bias_y_);
nh.getParam("/ekf_estimator/bias_z", bias_z_);

// load ground_truth state rosparams and setup subs
std::string state_ground_truth_topic;
nh.param<std::string>("topic/state/ground_truth", state_ground_truth_topic, "/state/ground_truth");
state_ground_truth_sub_ = nh_.subscribe(state_ground_truth_topic,1,&EKFEstimator::groundtruthCallback,this);


// Setup pubs and subs
joint_encoder_sub_ = nh_.subscribe(joint_encoder_topic,1,&EKFEstimator::jointEncoderCallback, this);
imu_sub_ = nh_.subscribe(imu_topic,1,&EKFEstimator::imuCallback, this);
contact_sub_ = nh_.subscribe(contact_topic,1,&EKFEstimator::contactCallback, this);
state_estimate_pub_ = nh_.advertise<quad_msgs::RobotState>(state_estimate_topic,1);

// QuadKD class
// quadKD_ = std::make_shared<quad_utils::QuadKD>();
}

void EKFEstimator::groundtruthCallback(const quad_msgs::RobotState::ConstPtr& msg) {
last_state_msg_ = msg;
}

void EKFEstimator::jointEncoderCallback(const sensor_msgs::JointState::ConstPtr& msg) {
@@ -38,56 +58,138 @@ quad_msgs::RobotState EKFEstimator::updateStep() {

// Create skeleton message to send out
quad_msgs::RobotState new_state_est;

// Record whether we have good imu and joint state data
bool good_imu = false;
bool good_joint_state = false;
bool good_ground_truth_state = false;

// Collect info from last imu message
if (last_imu_msg_ != NULL)
{
new_state_est.body.pose.orientation = (*last_imu_msg_).orientation;
//calculate dt
double dt = (start_time - last_time).toSec();
last_time = start_time;


/// Collect Data

// IMU reading linear xyz (0-2) angular (3-5)
Eigen::VectorXd imu_data = Eigen::VectorXd::Zero(6);
// IMU orientation
geometry_msgs::Quaternion imu_ori;
imu_ori.x = 0;
imu_ori.y = 0;
imu_ori.z = 0;
imu_ori.w = 1;
if (last_imu_msg_ != NULL){
good_imu = true;
imu_data << (*last_imu_msg_).linear_acceleration.x,
(*last_imu_msg_).linear_acceleration.y,
(*last_imu_msg_).linear_acceleration.z,
(*last_imu_msg_).angular_velocity.x,
(*last_imu_msg_).angular_velocity.y,
(*last_imu_msg_).angular_velocity.z;
imu_ori = (*last_imu_msg_).orientation;
}
else {
geometry_msgs::Quaternion quat;
quat.x = 0;
quat.y = 0;
quat.z = 0;
quat.w = 1;
new_state_est.body.pose.orientation = quat;
}
// Collect info from last joint state message, making sure info is not out of date

// Joint data reading 3 joints * 4 legs
std::vector<double> joints_data(12, 0);
if (last_joint_state_msg_ != NULL)
{
new_state_est.joints = *last_joint_state_msg_;
double joint_state_msg_time_diff = 0;
if (joint_state_msg_time_diff > joint_state_msg_time_diff_max_)
{
// Don't use this info in EKF update!
ROS_WARN("Haven't received a recent joint state message, skipping EKF measurement step");
}
else
{
good_joint_state = true;
}
good_joint_state = true;
for (int i = 0; i < 12; i++){
joints_data[i] = (*last_joint_state_msg_).position[i];
}
}
else {
ROS_DEBUG_THROTTLE(0.5,"Still waiting for first joint state message");
new_state_est.joints.header.stamp = ros::Time::now();
new_state_est.joints.name = {"8", "0", "1", "9","2", "3", "10", "4","5",
"11", "6", "7"};
new_state_est.joints.position = {0,0,0,0,0,0,0,0,0,0,0,0};
new_state_est.joints.velocity = {0,0,0,0,0,0,0,0,0,0,0,0};
new_state_est.joints.effort = {0,0,0,0,0,0,0,0,0,0,0,0};
else
{
good_joint_state = false;
joints_data = {0.767431, 1.591838, 0.804742, 1.612967, 0.721895, 1.562485, 0.729919, 1.514980, -0.012140, -0.024205, 0.050132, 0.058434};
}



// Collect position and velocity info from previous state vector
Eigen::VectorXd r = last_x.block<3, 1>(0, 0);
Eigen::VectorXd v = last_x.block<3, 1>(3, 0);

// imu bias measured
imu_bias = Eigen::VectorXd(3,1);
imu_bias << bias_x_,
bias_y_,
bias_z_;

// calculate acceleration set z acceleration to 0 for now
Eigen::VectorXd a = Eigen::VectorXd::Zero(3);
a = imu_data.head(3) - imu_bias;
a[2] = 0.0;


/// update state x and last_x
// Zero order hold to predict next state
// Hold rotation, foothold placements constant
x.block<3, 1>(0, 0) = r + dt * v + dt * dt * 0.5 * a;
x.block<3, 1>(3, 0) = v + dt * a;

last_x = x;


// Update when I have good data, otherwise stay at the origin
if (last_state_msg_ != NULL){
good_ground_truth_state = true;

}else{
good_ground_truth_state = false;
x = Eigen::VectorXd::Zero(6);
last_x = Eigen::VectorXd::Zero(6);
a = Eigen::VectorXd::Zero(3);
x << - 1.457778,
1.004244,
0.308681,
0,
0,
0;
last_x = x;
}

/// publish new message
new_state_est.header.stamp = ros::Time::now();

// body
new_state_est.body.header.stamp = ros::Time::now();
new_state_est.body.pose.orientation.x = imu_ori.x;
new_state_est.body.pose.orientation.y = imu_ori.y;
new_state_est.body.pose.orientation.z = imu_ori.z;
new_state_est.body.pose.orientation.w = imu_ori.w;
new_state_est.body.pose.position.x = x[0];
new_state_est.body.pose.position.y = x[1];
new_state_est.body.pose.position.z = x[2];

// joint
new_state_est.joints.header.stamp = ros::Time::now();
new_state_est.joints.name = {"0", "1", "2","3", "4", "5", "6","7",
"8", "9", "10", "11"};
new_state_est.joints.position = joints_data;
new_state_est.joints.velocity = {0,0,0,0,0,0,0,0,0,0,0,0};
new_state_est.joints.effort = {0,0,0,0,0,0,0,0,0,0,0,0};


return new_state_est;
}

void EKFEstimator::spin() {
ros::Rate r(update_rate_);

// Initial linear position body (0:2)
// Initial linear velocity body (3:5)
Eigen::MatrixXd x0 = Eigen::MatrixXd::Zero(6, 1);
x0 << - 1.457778,
1.004244,
0.308681,
0,
0,
0;
x = x0;
last_x = x;

while (ros::ok()) {

// Collect new messages on subscriber topics
@@ -101,6 +203,7 @@ void EKFEstimator::spin() {

// Store new state estimate for next iteration
last_state_est_ = new_state_est;


// Enforce update rate
r.sleep();
1 change: 1 addition & 0 deletions quad_utils/data/EKF_tests_2021_12_3/.~lock.test2.csv#
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
,qishuny,qishuny-5510,04.02.2022 17:11,file:///home/qishuny/.config/libreoffice/4;
Binary file not shown.
Binary file not shown.
Loading

0 comments on commit e4f92e1

Please sign in to comment.