p71924506
/
att_ekf
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

remove imu_bias

This commit is contained in:
libing64 2016-02-29 17:02:30 +08:00
parent ec15a57ffc
commit 6770c16d6c
4 changed files with 52 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
launch/att_ekf.launch
View File

@ -2,6 +2,7 @@
<node pkg="att_ekf" type="att_ekf" name="att_ekf" output="screen"> <node pkg="att_ekf" type="att_ekf" name="att_ekf" output="screen">
<remap from="/imu" to="/raw_imu"/> <remap from="/imu" to="/raw_imu"/>
<remap from="/magnetic_field" to="/magnetic" /> <remap from="/magnetic_field" to="/magnetic" />
<remap from="/imu_bias" to="/raw_imu/bias" />
</node> </node>
<!-- Start rviz visualization with preset config --> <!-- Start rviz visualization with preset config -->
<!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_ekf.rviz"/> --> <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_ekf.rviz"/> -->

33
src/att_ekf.cpp
View File

@ -1,11 +1,9 @@
#include "att_ekf.h" #include "att_ekf.h"
#include <iostream> #include <iostream>
#include <Eigen/Eigen> #include <Eigen/Eigen>
using namespace std; using namespace std;
using namespace Eigen; using namespace Eigen;
Matrix3d skew_symmetric(Vector3d& v) Matrix3d skew_symmetric(Vector3d& v)
{ {
Matrix3d m; Matrix3d m;
@ -21,11 +19,15 @@ Att_ekf::Att_ekf()
imu_initialized = false; imu_initialized = false;
mag_initialized = false; mag_initialized = false;
R_imu = MatrixXd::Zero(3, 3); R_imu.setZero();
R_imu.block<3, 3>(0, 0) = R_acc; R_imu.block<3, 3>(0, 0) = R_gyro;
R_imu.block<3, 3>(3, 3) = 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() Att_ekf::~Att_ekf()
{ {
@ -33,15 +35,13 @@ Att_ekf::~Att_ekf()
} }
void Att_ekf::predict(double t) void Att_ekf::predict(double t)
{ {
cout << "before predict: " << x.transpose() << endl;
double dt = t - curr_t; double dt = t - curr_t;
cout << "dt :" << dt << endl; //cout << "dt :" << dt << endl;
Vector3d w = x.head(3); Vector3d w = x.head(3);
Vector3d wa = x.segment<3>(3); Vector3d wa = x.segment<3>(3);
Vector3d ra = x.segment<3>(6); Vector3d ra = x.segment<3>(6);
Vector3d rm = x.tail(3); Vector3d rm = x.tail(3);
x.segment<3>(0) += wa*dt; x.segment<3>(0) += wa*dt;
x.segment<3>(6) += -skew_symmetric(w)*ra*dt; x.segment<3>(6) += -skew_symmetric(w)*ra*dt;
x.segment<3>(9) += -skew_symmetric(w)*rm*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>(6, 0) += skew_symmetric(ra)*dt;
A.block<3, 3>(9, 0) += skew_symmetric(rm)*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; curr_t = t;
cout << "predict: " << x.transpose() << endl; //cout << "predict: " << x.transpose() << endl;
} }
void Att_ekf::update_magnetic(Vector3d& mag, double t) 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; cout << "t is smaller than curr_t" << endl;
return; return;
} }
predict(t); predict(t);
MatrixXd H = MatrixXd::Zero(3, 12); 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); x = x + K*(z - H*x);
P = (I- K*H)*P.inverse(); 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) 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; return;
} }
predict(t); predict(t);
MatrixXd H = MatrixXd::Zero(6, 12); MatrixXd H = MatrixXd::Zero(6, 12);
VectorXd z(6); VectorXd z(6);
z.head(3) = gyro; 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); MatrixXd I = MatrixXd::Identity(12, 12);
x = x + K*(z - H*x); x = x + K*(z - H*x);
P = (I- K*H)*P.inverse(); P = (I- K*H)*P.inverse();
cout << "update imu: " << x.transpose() << endl; //cout << "update imu: " << x.transpose() << endl;
} }
Matrix3d Att_ekf::get_rotation_matrix() Matrix3d Att_ekf::get_rotation_matrix()
{ {
if(!(mag_initialized && imu_initialized)) return Matrix3d::Identity(); if(!(mag_initialized && imu_initialized)) return Matrix3d::Identity();
Matrix3d Rbn;//ENU to body Matrix3d Rbn;//ENU to body
Vector3d ra = x.segment<3>(6); Vector3d ra = x.segment<3>(6);
@ -129,7 +129,6 @@ Matrix3d Att_ekf::get_rotation_matrix()
Vector3d Iz = ra;//ENU coordinate 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 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; Vector3d Ix = skew_symmetric(Iy)*Iz;
Ix /= Ix.norm(); Iy /= Iy.norm(); Iz /= Iz.norm(); Ix /= Ix.norm(); Iy /= Iy.norm(); Iz /= Iz.norm();
Rbn.col(0) = Ix; Rbn.col(1) = Iy; Rbn.col(2) = Iz; Rbn.col(0) = Ix; Rbn.col(1) = Iy; Rbn.col(2) = Iz;
return Rbn.transpose(); return Rbn.transpose();

11
src/att_ekf.h
View File

@ -15,20 +15,21 @@ public:
Matrix3d get_rotation_matrix(); Matrix3d get_rotation_matrix();
double get_time() {return curr_t;} double get_time() {return curr_t;}
private: private:
const double acc_cov = 0.35;
const double gyro_cov = 0.05;
const double mag_cov = 0.2;
double curr_t; double curr_t;
Matrix<double, 12, 1> x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field Matrix<double, 12, 1> x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field
Matrix<double, 12, 12> P;//covariance 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_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_gyro = MatrixXd::Identity(3, 3)*gyro_cov;
const Matrix<double, 3, 3> R_mag = MatrixXd::Identity(3, 3)*mag_cov; const Matrix<double, 3, 3> R_mag = MatrixXd::Identity(3, 3)*mag_cov;
Matrix<double, 6, 6> R_imu;//measurement noise Matrix<double, 6, 6> R_imu;//measurement noise
bool imu_initialized; bool imu_initialized;
bool mag_initialized; bool mag_initialized;
const double acc_cov = 0.5;
const double gyro_cov = 0.05;
const double mag_cov = 0.5;
}; };

39
src/att_ekf_node.cpp
View File

@ -3,9 +3,7 @@
#include <sensor_msgs/Imu.h> #include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h> #include <sensor_msgs/MagneticField.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/OccupancyGrid.h> #include <geometry_msgs/Vector3Stamped.h>
#include <nav_msgs/Path.h>
#include <tf/transform_listener.h>
#include <Eigen/Eigen> #include <Eigen/Eigen>
#include "att_ekf.h" #include "att_ekf.h"
@ -20,21 +18,24 @@ Att_ekf att_ekf;
deque<pair<double, geometry_msgs::Vector3Stamped> > mag_q; deque<pair<double, geometry_msgs::Vector3Stamped> > mag_q;
deque<pair<double, sensor_msgs::Imu> >imu_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; ros::Publisher pose_pub, pose_gt_pub;
void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg) void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg)
{ {
geometry_msgs::Vector3Stamped mag_msg = *msg; geometry_msgs::Vector3Stamped mag_msg = *msg;
double t = msg->header.stamp.toSec(); double t = msg->header.stamp.toSec();
mag_q.push_back(make_pair(t, mag_msg)); mag_q.push_back(make_pair(t, mag_msg));
// Vector3d mag;
Vector3d mag; // mag(0) = msg->vector.x;
mag(0) = msg->vector.x; // mag(1) = msg->vector.y;
mag(1) = msg->vector.y; // mag(2) = msg->vector.z;
mag(2) = msg->vector.z;
//cout << "mag: " << mag.transpose() << endl; //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; //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() void publish_pose()
{ {
geometry_msgs::PoseStamped pose; geometry_msgs::PoseStamped pose;
@ -81,6 +94,7 @@ int main(int argc, char **argv)
ros::init(argc, argv, "laser_localization"); ros::init(argc, argv, "laser_localization");
ros::NodeHandle n("~"); ros::NodeHandle n("~");
ros::Subscriber imu_sub = n.subscribe("/imu", 100, imuCallback); 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); ros::Subscriber mag_sub = n.subscribe("/magnetic_field", 100, magCallback);
pose_pub = n.advertise<geometry_msgs::PoseStamped>("/pose", 10); pose_pub = n.advertise<geometry_msgs::PoseStamped>("/pose", 10);
pose_gt_pub = n.advertise<geometry_msgs::PoseStamped>("/pose_gt", 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(1) = imu_q.front().second.angular_velocity.y;
gyro(2) = imu_q.front().second.angular_velocity.z; gyro(2) = imu_q.front().second.angular_velocity.z;
double t = imu_q.front().first; double t = imu_q.front().first;
if(imu_bias_initialized)
{
acc -= acc_bias;
gyro -= gyro_bias;
}
att_ekf.update_imu(acc, gyro, t); att_ekf.update_imu(acc, gyro, t);
imu_q.pop_front(); imu_q.pop_front();
}else }else