From 6770c16d6cea4911abe8d1b53d06917774692e04 Mon Sep 17 00:00:00 2001 From: libing64 Date: Mon, 29 Feb 2016 17:02:30 +0800 Subject: [PATCH] remove imu_bias --- launch/att_ekf.launch | 1 + src/att_ekf.cpp | 33 ++++++++++++++++----------------- src/att_ekf.h | 11 ++++++----- src/att_ekf_node.cpp | 39 +++++++++++++++++++++++++++++---------- 4 files changed, 52 insertions(+), 32 deletions(-) diff --git a/launch/att_ekf.launch b/launch/att_ekf.launch index 0c3ce3c..1f744c8 100644 --- a/launch/att_ekf.launch +++ b/launch/att_ekf.launch @@ -2,6 +2,7 @@ + diff --git a/src/att_ekf.cpp b/src/att_ekf.cpp index aca2559..61e8019 100644 --- a/src/att_ekf.cpp +++ b/src/att_ekf.cpp @@ -1,11 +1,9 @@ #include "att_ekf.h" #include #include - using namespace std; using namespace Eigen; - Matrix3d skew_symmetric(Vector3d& v) { Matrix3d m; @@ -21,11 +19,15 @@ Att_ekf::Att_ekf() imu_initialized = false; mag_initialized = false; - R_imu = MatrixXd::Zero(3, 3); - R_imu.block<3, 3>(0, 0) = R_acc; - R_imu.block<3, 3>(3, 3) = R_gyro; + R_imu.setZero(); + R_imu.block<3, 3>(0, 0) = R_gyro; + R_imu.block<3, 3>(3, 3) = R_acc; - P = MatrixXd::Identity(12, 12)*10; + P.setZero(); + P.block<3, 3>(0, 0) = R_gyro; + P.block<3, 3>(3, 3) = R_gyro*1000; + P.block<3, 3>(6, 6) = R_acc; + P.block<3, 3>(9, 9) = R_mag; } Att_ekf::~Att_ekf() { @@ -33,15 +35,13 @@ Att_ekf::~Att_ekf() } void Att_ekf::predict(double t) { - cout << "before predict: " << x.transpose() << endl; double dt = t - curr_t; - cout << "dt :" << dt << endl; + //cout << "dt :" << dt << endl; Vector3d w = x.head(3); Vector3d wa = x.segment<3>(3); Vector3d ra = x.segment<3>(6); Vector3d rm = x.tail(3); - x.segment<3>(0) += wa*dt; x.segment<3>(6) += -skew_symmetric(w)*ra*dt; x.segment<3>(9) += -skew_symmetric(w)*rm*dt; @@ -53,9 +53,11 @@ void Att_ekf::predict(double t) A.block<3, 3>(6, 0) += skew_symmetric(ra)*dt; A.block<3, 3>(9, 0) += skew_symmetric(rm)*dt; - P = A*P*A.transpose() + Q; + + + P = A*P*A.transpose() + Q;//Q? curr_t = t; - cout << "predict: " << x.transpose() << endl; + //cout << "predict: " << x.transpose() << endl; } void Att_ekf::update_magnetic(Vector3d& mag, double t) { @@ -71,7 +73,6 @@ void Att_ekf::update_magnetic(Vector3d& mag, double t) cout << "t is smaller than curr_t" << endl; return; } - predict(t); MatrixXd H = MatrixXd::Zero(3, 12); @@ -84,7 +85,7 @@ void Att_ekf::update_magnetic(Vector3d& mag, double t) x = x + K*(z - H*x); P = (I- K*H)*P.inverse(); - cout << "update mag: " << x.transpose() << endl; + //cout << "update mag: " << x.transpose() << endl; } void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t) { @@ -103,7 +104,7 @@ void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t) return; } predict(t); - + MatrixXd H = MatrixXd::Zero(6, 12); VectorXd z(6); z.head(3) = gyro; @@ -115,13 +116,12 @@ void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t) MatrixXd I = MatrixXd::Identity(12, 12); x = x + K*(z - H*x); P = (I- K*H)*P.inverse(); - cout << "update imu: " << x.transpose() << endl; + //cout << "update imu: " << x.transpose() << endl; } Matrix3d Att_ekf::get_rotation_matrix() { - if(!(mag_initialized && imu_initialized)) return Matrix3d::Identity(); Matrix3d Rbn;//ENU to body Vector3d ra = x.segment<3>(6); @@ -129,7 +129,6 @@ Matrix3d Att_ekf::get_rotation_matrix() Vector3d Iz = ra;//ENU coordinate Vector3d Iy = skew_symmetric(Iz)*rm;//why rm in ENU coordinate is [1 0 x], it should be [0 1 x] Vector3d Ix = skew_symmetric(Iy)*Iz; - Ix /= Ix.norm(); Iy /= Iy.norm(); Iz /= Iz.norm(); Rbn.col(0) = Ix; Rbn.col(1) = Iy; Rbn.col(2) = Iz; return Rbn.transpose(); diff --git a/src/att_ekf.h b/src/att_ekf.h index 674410e..7e20cd1 100644 --- a/src/att_ekf.h +++ b/src/att_ekf.h @@ -15,20 +15,21 @@ public: Matrix3d get_rotation_matrix(); double get_time() {return curr_t;} private: + const double acc_cov = 0.35; + const double gyro_cov = 0.05; + const double mag_cov = 0.2; + double curr_t; Matrix x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field Matrix P;//covariance - const Matrix Q = MatrixXd::Identity(12, 12)*0.01; + const Matrix Q = MatrixXd::Identity(12, 12)*0.5; const Matrix R_acc = MatrixXd::Identity(3, 3)*acc_cov; const Matrix R_gyro = MatrixXd::Identity(3, 3)*gyro_cov; const Matrix R_mag = MatrixXd::Identity(3, 3)*mag_cov; - Matrix R_imu;//measurement noise bool imu_initialized; bool mag_initialized; - const double acc_cov = 0.5; - const double gyro_cov = 0.05; - const double mag_cov = 0.5; + }; diff --git a/src/att_ekf_node.cpp b/src/att_ekf_node.cpp index 049885e..0bc4283 100644 --- a/src/att_ekf_node.cpp +++ b/src/att_ekf_node.cpp @@ -3,9 +3,7 @@ #include #include #include -#include -#include -#include +#include #include #include "att_ekf.h" @@ -20,21 +18,24 @@ Att_ekf att_ekf; deque > mag_q; deque >imu_q; -Quaterniond q_gt; +ros::Subscriber imu_bias_sub; +bool imu_bias_initialized = false; +Vector3d gyro_bias; +Vector3d acc_bias; + +Quaterniond q_gt; ros::Publisher pose_pub, pose_gt_pub; void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg) { - geometry_msgs::Vector3Stamped mag_msg = *msg; double t = msg->header.stamp.toSec(); mag_q.push_back(make_pair(t, mag_msg)); - - Vector3d mag; - mag(0) = msg->vector.x; - mag(1) = msg->vector.y; - mag(2) = msg->vector.z; + // Vector3d mag; + // mag(0) = msg->vector.x; + // mag(1) = msg->vector.y; + // mag(2) = msg->vector.z; //cout << "mag: " << mag.transpose() << endl; } @@ -53,6 +54,18 @@ void imuCallback(const sensor_msgs::ImuConstPtr & msg) //cout << "q_gt: " << q_gt.w() << " " << q_gt.vec().transpose() << endl; } +void imuBiasCallback(const sensor_msgs::ImuConstPtr & msg) +{ + acc_bias(0) = msg->linear_acceleration.x; + acc_bias(1) = msg->linear_acceleration.y; + acc_bias(2) = msg->linear_acceleration.z; + gyro_bias(0) = msg->angular_velocity.x; + gyro_bias(1) = msg->angular_velocity.y; + gyro_bias(2) = msg->angular_velocity.z; + imu_bias_initialized = true; + imu_bias_sub.shutdown(); +} + void publish_pose() { geometry_msgs::PoseStamped pose; @@ -81,6 +94,7 @@ int main(int argc, char **argv) ros::init(argc, argv, "laser_localization"); ros::NodeHandle n("~"); ros::Subscriber imu_sub = n.subscribe("/imu", 100, imuCallback); + imu_bias_sub = n.subscribe("/imu_bias", 100, imuBiasCallback); ros::Subscriber mag_sub = n.subscribe("/magnetic_field", 100, magCallback); pose_pub = n.advertise("/pose", 10); pose_gt_pub = n.advertise("/pose_gt", 10); @@ -103,6 +117,11 @@ int main(int argc, char **argv) gyro(1) = imu_q.front().second.angular_velocity.y; gyro(2) = imu_q.front().second.angular_velocity.z; double t = imu_q.front().first; + if(imu_bias_initialized) + { + acc -= acc_bias; + gyro -= gyro_bias; + } att_ekf.update_imu(acc, gyro, t); imu_q.pop_front(); }else