00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "imu_filter_madgwick/imu_filter.h"
00026
00027 ImuFilter::ImuFilter(ros::NodeHandle nh, ros::NodeHandle nh_private):
00028 nh_(nh),
00029 nh_private_(nh_private),
00030 initialized_(false),
00031 q0(1.0), q1(0.0), q2(0.0), q3(0.0)
00032 {
00033 ROS_INFO ("Starting ImuFilter");
00034
00035
00036
00037 if (!nh_private_.getParam ("gain", gain_))
00038 gain_ = 0.1;
00039 if (!nh_private_.getParam ("zeta", zeta_))
00040 zeta_ = 0;
00041 if (!nh_private_.getParam ("use_mag", use_mag_))
00042 use_mag_ = true;
00043 if (!nh_private_.getParam ("publish_tf", publish_tf_))
00044 publish_tf_ = true;
00045 if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
00046 fixed_frame_ = "odom";
00047 if (!nh_private_.getParam ("constant_dt", constant_dt_))
00048 constant_dt_ = 0.0;
00049
00050
00051 if (constant_dt_ < 0.0)
00052 {
00053 ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
00054 constant_dt_ = 0.0;
00055 }
00056
00057
00058
00059 if (constant_dt_ == 0.0)
00060 ROS_INFO("Using dt computed from message headers");
00061 else
00062 ROS_INFO("Using constant dt of %f sec", constant_dt_);
00063
00064
00065
00066 FilterConfigServer::CallbackType f = boost::bind(&ImuFilter::reconfigCallback, this, _1, _2);
00067 config_server_.setCallback(f);
00068
00069
00070
00071 imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
00072 "imu/data", 5);
00073
00074
00075
00076
00077 int queue_size = 5;
00078
00079 imu_subscriber_.reset(new ImuSubscriber(
00080 nh_, "imu/data_raw", queue_size));
00081
00082 if (use_mag_)
00083 {
00084 mag_subscriber_.reset(new MagSubscriber(
00085 nh_, "imu/mag", queue_size));
00086
00087 sync_.reset(new Synchronizer(
00088 SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
00089 sync_->registerCallback(boost::bind(&ImuFilter::imuMagCallback, this, _1, _2));
00090 }
00091 else
00092 {
00093 imu_subscriber_->registerCallback(&ImuFilter::imuCallback, this);
00094 }
00095 }
00096
00097 ImuFilter::~ImuFilter()
00098 {
00099 ROS_INFO ("Destroying ImuFilter");
00100 }
00101
00102 void ImuFilter::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
00103 {
00104 boost::mutex::scoped_lock(mutex_);
00105
00106 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00107 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00108
00109 ros::Time time = imu_msg_raw->header.stamp;
00110 imu_frame_ = imu_msg_raw->header.frame_id;
00111
00112 if (!initialized_)
00113 {
00114
00115 double sign = copysignf(1.0, lin_acc.z);
00116 double roll = atan2(lin_acc.y, sign * sqrt(lin_acc.x*lin_acc.x + lin_acc.z*lin_acc.z));
00117 double pitch = -atan2(lin_acc.x, sqrt(lin_acc.y*lin_acc.y + lin_acc.z*lin_acc.z));
00118 double yaw = 0.0;
00119
00120 tf::Quaternion init_q = tf::createQuaternionFromRPY(roll, pitch, yaw);
00121
00122 q1 = init_q.getX();
00123 q2 = init_q.getY();
00124 q3 = init_q.getZ();
00125 q0 = init_q.getW();
00126
00127
00128 last_time_ = time;
00129 initialized_ = true;
00130 }
00131
00132
00133 float dt;
00134 if (constant_dt_ > 0.0)
00135 dt = constant_dt_;
00136 else
00137 dt = (time - last_time_).toSec();
00138
00139 last_time_ = time;
00140
00141 madgwickAHRSupdateIMU(
00142 ang_vel.x, ang_vel.y, ang_vel.z,
00143 lin_acc.x, lin_acc.y, lin_acc.z,
00144 dt);
00145
00146 publishFilteredMsg(imu_msg_raw);
00147 if (publish_tf_)
00148 publishTransform(imu_msg_raw);
00149 }
00150
00151 void ImuFilter::imuMagCallback(
00152 const ImuMsg::ConstPtr& imu_msg_raw,
00153 const MagMsg::ConstPtr& mag_msg)
00154 {
00155 boost::mutex::scoped_lock(mutex_);
00156
00157 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00158 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00159 const geometry_msgs::Vector3& mag_fld = mag_msg->vector;
00160
00161 ros::Time time = imu_msg_raw->header.stamp;
00162 imu_frame_ = imu_msg_raw->header.frame_id;
00163
00164 if (!initialized_)
00165 {
00166
00167 double sign = copysignf(1.0, lin_acc.z);
00168 double roll = atan2(lin_acc.y, sign * sqrt(lin_acc.x*lin_acc.x + lin_acc.z*lin_acc.z));
00169 double pitch = -atan2(lin_acc.x, sqrt(lin_acc.y*lin_acc.y + lin_acc.z*lin_acc.z));
00170 double yaw = 0.0;
00171
00172 tf::Quaternion init_q = tf::createQuaternionFromRPY(roll, pitch, yaw);
00173
00174 q1 = init_q.getX();
00175 q2 = init_q.getY();
00176 q3 = init_q.getZ();
00177 q0 = init_q.getW();
00178
00179 w_bx_ = 0;
00180 w_by_ = 0;
00181 w_bz_ = 0;
00182
00183 last_time_ = time;
00184 initialized_ = true;
00185 }
00186
00187
00188 float dt;
00189 if (constant_dt_ > 0.0)
00190 dt = constant_dt_;
00191 else
00192 dt = (time - last_time_).toSec();
00193
00194 last_time_ = time;
00195
00196 madgwickAHRSupdate(
00197 ang_vel.x, ang_vel.y, ang_vel.z,
00198 lin_acc.x, lin_acc.y, lin_acc.z,
00199 mag_fld.x, mag_fld.y, mag_fld.z,
00200 dt);
00201
00202 publishFilteredMsg(imu_msg_raw);
00203 if (publish_tf_)
00204 publishTransform(imu_msg_raw);
00205 }
00206
00207 void ImuFilter::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
00208 {
00209 tf::Quaternion q(q1, q2, q3, q0);
00210 tf::Transform transform;
00211 transform.setOrigin( tf::Vector3( 0.0, 0.0, 0.0 ) );
00212 transform.setRotation( q );
00213 tf_broadcaster_.sendTransform( tf::StampedTransform( transform,
00214 imu_msg_raw->header.stamp,
00215 fixed_frame_,
00216 imu_frame_ ) );
00217
00218 }
00219
00220 void ImuFilter::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
00221 {
00222
00223
00224 tf::Quaternion q(q1, q2, q3, q0);
00225
00226
00227 boost::shared_ptr<ImuMsg> imu_msg =
00228 boost::make_shared<ImuMsg>(*imu_msg_raw);
00229
00230 imu_msg->header.frame_id = fixed_frame_;
00231 tf::quaternionTFToMsg(q, imu_msg->orientation);
00232 imu_publisher_.publish(imu_msg);
00233 }
00234
00235 void ImuFilter::madgwickAHRSupdate(
00236 float gx, float gy, float gz,
00237 float ax, float ay, float az,
00238 float mx, float my, float mz,
00239 float dt)
00240 {
00241 float recipNorm;
00242 float s0, s1, s2, s3;
00243 float qDot1, qDot2, qDot3, qDot4;
00244 float hx, hy;
00245 float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00246 float _w_err_x, _w_err_y, _w_err_z;
00247
00248
00249 if(std::isnan(mx) || std::isnan(my) || std::isnan(mz))
00250 {
00251 madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
00252 return;
00253 }
00254
00255
00256
00257 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
00258 {
00259
00260 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00261 ax *= recipNorm;
00262 ay *= recipNorm;
00263 az *= recipNorm;
00264
00265
00266 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
00267 mx *= recipNorm;
00268 my *= recipNorm;
00269 mz *= recipNorm;
00270
00271
00272 _2q0mx = 2.0f * q0 * mx;
00273 _2q0my = 2.0f * q0 * my;
00274 _2q0mz = 2.0f * q0 * mz;
00275 _2q1mx = 2.0f * q1 * mx;
00276 _2q0 = 2.0f * q0;
00277 _2q1 = 2.0f * q1;
00278 _2q2 = 2.0f * q2;
00279 _2q3 = 2.0f * q3;
00280 _2q0q2 = 2.0f * q0 * q2;
00281 _2q2q3 = 2.0f * q2 * q3;
00282 q0q0 = q0 * q0;
00283 q0q1 = q0 * q1;
00284 q0q2 = q0 * q2;
00285 q0q3 = q0 * q3;
00286 q1q1 = q1 * q1;
00287 q1q2 = q1 * q2;
00288 q1q3 = q1 * q3;
00289 q2q2 = q2 * q2;
00290 q2q3 = q2 * q3;
00291 q3q3 = q3 * q3;
00292
00293
00294 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
00295 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
00296 _2bx = sqrt(hx * hx + hy * hy);
00297 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
00298 _4bx = 2.0f * _2bx;
00299 _4bz = 2.0f * _2bz;
00300
00301
00302 s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00303 s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00304 s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00305 s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
00306 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
00307 s0 *= recipNorm;
00308 s1 *= recipNorm;
00309 s2 *= recipNorm;
00310 s3 *= recipNorm;
00311
00312
00313 _w_err_x = _2q0 * s1 - _2q1 * s0 - _2q2 * s3 + _2q3 * s2;
00314 _w_err_y = _2q0 * s2 + _2q1 * s3 - _2q2 * s0 - _2q3 * s1;
00315 _w_err_z = _2q0 * s3 - _2q1 * s2 + _2q2 * s1 - _2q3 * s0;
00316
00317 w_bx_ += _w_err_x * dt * zeta_;
00318 w_by_ += _w_err_y * dt * zeta_;
00319 w_bz_ += _w_err_z * dt * zeta_;
00320
00321 gx -= w_bx_;
00322 gy -= w_by_;
00323 gz -= w_bz_;
00324
00325
00326 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00327 qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
00328 qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
00329 qDot4 = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);
00330
00331
00332 qDot1 -= gain_ * s0;
00333 qDot2 -= gain_ * s1;
00334 qDot3 -= gain_ * s2;
00335 qDot4 -= gain_ * s3;
00336 } else{
00337
00338 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00339 qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
00340 qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
00341 qDot4 = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);
00342 }
00343
00344
00345 q0 += qDot1 * dt;
00346 q1 += qDot2 * dt;
00347 q2 += qDot3 * dt;
00348 q3 += qDot4 * dt;
00349
00350
00351 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00352 q0 *= recipNorm;
00353 q1 *= recipNorm;
00354 q2 *= recipNorm;
00355 q3 *= recipNorm;
00356 }
00357
00358
00359 void ImuFilter::madgwickAHRSupdateIMU(
00360 float gx, float gy, float gz,
00361 float ax, float ay, float az,
00362 float dt)
00363 {
00364 float recipNorm;
00365 float s0, s1, s2, s3;
00366 float qDot1, qDot2, qDot3, qDot4;
00367 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
00368
00369
00370 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
00371 qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
00372 qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
00373 qDot4 = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);
00374
00375
00376 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
00377 {
00378
00379 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00380 ax *= recipNorm;
00381 ay *= recipNorm;
00382 az *= recipNorm;
00383
00384
00385 _2q0 = 2.0f * q0;
00386 _2q1 = 2.0f * q1;
00387 _2q2 = 2.0f * q2;
00388 _2q3 = 2.0f * q3;
00389 _4q0 = 4.0f * q0;
00390 _4q1 = 4.0f * q1;
00391 _4q2 = 4.0f * q2;
00392 _8q1 = 8.0f * q1;
00393 _8q2 = 8.0f * q2;
00394 q0q0 = q0 * q0;
00395 q1q1 = q1 * q1;
00396 q2q2 = q2 * q2;
00397 q3q3 = q3 * q3;
00398
00399
00400 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
00401 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
00402 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
00403 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
00404 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
00405 s0 *= recipNorm;
00406 s1 *= recipNorm;
00407 s2 *= recipNorm;
00408 s3 *= recipNorm;
00409
00410
00411 qDot1 -= gain_ * s0;
00412 qDot2 -= gain_ * s1;
00413 qDot3 -= gain_ * s2;
00414 qDot4 -= gain_ * s3;
00415 }
00416
00417
00418 q0 += qDot1 * dt;
00419 q1 += qDot2 * dt;
00420 q2 += qDot3 * dt;
00421 q3 += qDot4 * dt;
00422
00423
00424 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00425 q0 *= recipNorm;
00426 q1 *= recipNorm;
00427 q2 *= recipNorm;
00428 q3 *= recipNorm;
00429 }
00430
00431 void ImuFilter::reconfigCallback(FilterConfig& config, uint32_t level)
00432 {
00433 boost::mutex::scoped_lock(mutex_);
00434 gain_ = config.gain;
00435 zeta_ = config.zeta;
00436 ROS_INFO("Imu filter gain set to %f", gain_);
00437 ROS_INFO("Gyro drift bias set to %f", zeta_);
00438 }
00439
00440