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 @@
 	<node pkg="att_ekf" type="att_ekf" name="att_ekf" output="screen">
 		<remap from="/imu" to="/raw_imu"/>
 		<remap from="/magnetic_field" to="/magnetic" />
+		<remap from="/imu_bias" to="/raw_imu/bias" />
 	</node>
   	<!-- Start rviz visualization with preset config -->
 <!--   	<node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_ekf.rviz"/> -->
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 <iostream>
 #include <Eigen/Eigen>
-
 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<double, 12, 1> x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field 
 	Matrix<double, 12, 12> P;//covariance
-	const Matrix<double, 12, 12> Q = MatrixXd::Identity(12, 12)*0.01;
+	const Matrix<double, 12, 12> Q = MatrixXd::Identity(12, 12)*0.5;
 
 	const Matrix<double, 3, 3> R_acc = MatrixXd::Identity(3, 3)*acc_cov;
 	const Matrix<double, 3, 3> R_gyro = MatrixXd::Identity(3, 3)*gyro_cov;
 	const Matrix<double, 3, 3> R_mag = MatrixXd::Identity(3, 3)*mag_cov;
-
 	Matrix<double, 6, 6> 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 <sensor_msgs/Imu.h>
 #include <sensor_msgs/MagneticField.h>
 #include <geometry_msgs/PoseStamped.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <nav_msgs/Path.h>
-#include <tf/transform_listener.h>
+#include <geometry_msgs/Vector3Stamped.h>
 
 #include <Eigen/Eigen>
 #include "att_ekf.h"
@@ -20,21 +18,24 @@ Att_ekf att_ekf;
 deque<pair<double, geometry_msgs::Vector3Stamped> > mag_q;
 deque<pair<double, sensor_msgs::Imu> >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<geometry_msgs::PoseStamped>("/pose", 10);
 	pose_gt_pub = n.advertise<geometry_msgs::PoseStamped>("/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