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">
<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"/> -->

33
src/att_ekf.cpp
View File

@ -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();

11
src/att_ekf.h
View File

@ -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;
};

39
src/att_ekf_node.cpp
View File

@ -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