ImuFilter Class Reference

Estimates IMU angles from acceleration and gyro data. More...

#include <imufilter.hpp>

List of all members.

Public Member Functions
bool	getAngleIntegration (double &irx, double &iry, double &irz)
	Get estimated integrated Euler angles since last reset.
bool	getAngles (double &rx, double &ry, double &rz)
	Get estimated Euler angles in radians interval [-PI,PI] rad.
bool	getBIAS (double &gbx, double &gby, double &gbz)
	Get estimated gyro biases in rad/s.
void	imuDataCallback (const sensor_msgs::Imu::ConstPtr &msg)
	ImuFilter (double T, std::string imuTopic, double calibTime=5.0)
	Constructor.
bool	initialize (void)
	Initialize EKF Input: vector of sensor_msgs::Imu The user must continue calling this function with new gyro data until it returns true.
bool	isInit (void)
	IMU initialization return function.
bool	predict (double gx, double gy, double gz)
bool	resetAngleIntegration (void)
	Reset integrated angles.
bool	update (double ax, double ay, double az)
bool	update (double ax, double ay, double az, double mx, double my, double mz)
Protected Member Functions
double	floorAbsolute (double value)
	Round down absolute function.
double	Pi2PiRange (double contAngle)
	Convert angles into interval [-PI,PI] rad.
Protected Attributes
double	accDev_
double	accTh_
double	accVar_ [3]
double	biaDev_
double	biaTh_
double	biaVar_ [3]
std::vector< sensor_msgs::Imu >	calibData_
double	calibTime_
double	gbx_
double	gby_
double	gbz_
double	gyrDev_
double	gyrVar_ [3]
bool	init_
double	irx_
double	iry_
double	irz_
double	magCal_ [6]
double	magDev_
double	magVar_ [3]
double	magXo_
double	magXs_
double	magYo_
double	magYs_
double	magZo_
double	magZs_
ros::NodeHandle	nh_
Eigen::MatrixXd	P_
double	rx_
double	ry_
double	rz_
ros::Subscriber	sub_
double	T2_
double	T_

---

Detailed Description

Estimates IMU angles from acceleration and gyro data.

Definition at line 54 of file imufilter.hpp.

---

Constructor & Destructor Documentation

ImuFilter::ImuFilter	(	double	T,
		std::string	imuTopic,
		double	calibTime = 5.0
	)		[inline]

Constructor.

Parameters:

T	Prediction period (seconds)
imuTopic	Name of the IMU data topic
calibTime	Initial calibration time (seconds)

Definition at line 63 of file imufilter.hpp.

---

Member Function Documentation

double ImuFilter::floorAbsolute

(

double

value

)

[inline, protected]

Round down absolute function.

Parameters:

[in]

value

Input value

Returns:

Input value floored

Definition at line 446 of file imufilter.hpp.

bool ImuFilter::getAngleIntegration	(	double &	irx,
		double &	iry,
		double &	irz
	)		[inline]

Get estimated integrated Euler angles since last reset.

Parameters:

[out]	rx	Estimated integrated roll (rad)
[out]	ry	Estimated integrated pitch (rad)
[out]	rz	Estimated integrated yaw (rad)

Definition at line 377 of file imufilter.hpp.

bool ImuFilter::getAngles	(	double &	rx,
		double &	ry,
		double &	rz
	)		[inline]

Get estimated Euler angles in radians interval [-PI,PI] rad.

Parameters:

[out]	rx	Estimated roll (rad)
[out]	ry	Estimated pitch (rad)
[out]	rz	Estimated yaw (rad)

Definition at line 364 of file imufilter.hpp.

bool ImuFilter::getBIAS	(	double &	gbx,
		double &	gby,
		double &	gbz
	)		[inline]

Get estimated gyro biases in rad/s.

Parameters:

[out]	gbx	Estimated X gyro bias (rad/s)
[out]	gby	Estimated Y gyro bias (rad/s)
[out]	gbz	Estimated Z gyro bias (rad/s)

Definition at line 398 of file imufilter.hpp.

void ImuFilter::imuDataCallback

(

const sensor_msgs::Imu::ConstPtr &

msg

)

[inline]

IMU sensor data callback

Parameters:

[in]

msg

IMU data message

Definition at line 418 of file imufilter.hpp.

bool ImuFilter::initialize

(

void

)

[inline]

Initialize EKF Input: vector of sensor_msgs::Imu The user must continue calling this function with new gyro data until it returns true.

Definition at line 92 of file imufilter.hpp.

bool ImuFilter::isInit

(

void

)

[inline]

IMU initialization return function.

Returns:

Returns true if IMU initialized

Definition at line 410 of file imufilter.hpp.

double ImuFilter::Pi2PiRange

(

double

contAngle

)

[inline, protected]

Convert angles into interval [-PI,PI] rad.

Parameters:

[in]

contAngle

Input angle (rad)

Returns:

Input angle in interval [-PI,PI] rad

Definition at line 458 of file imufilter.hpp.

bool ImuFilter::predict	(	double	gx,
		double	gy,
		double	gz
	)		[inline]

EKF prediction stage based on gyro information

Parameters:

[in]	gx	Raw X gyro data (rad/s)
[in]	gy	Raw Y gyro data (rad/s)
[in]	gz	Raw Z gyro data (rad/s)

Definition at line 168 of file imufilter.hpp.

bool ImuFilter::resetAngleIntegration

(

void

)

[inline]

Reset integrated angles.

Definition at line 387 of file imufilter.hpp.

bool ImuFilter::update	(	double	ax,
		double	ay,
		double	az
	)		[inline]

EKF update stage based on accelerometer information

Parameters:

[in]	ax	Raw X accelerometer data (g)
[in]	ay	Raw Y accelerometer data (g)
[in]	az	Raw Z accelerometer data (g)

Definition at line 205 of file imufilter.hpp.

bool ImuFilter::update	(	double	ax,
		double	ay,
		double	az,
		double	mx,
		double	my,
		double	mz
	)		[inline]

EKF update stage based on accelerometer information

Parameters:

[in]	ax	Raw X accelerometer data (g)
[in]	ay	Raw Y accelerometer data (g)
[in]	az	Raw Z accelerometer data (g)
[in]	mx	Raw X magnetometer data ()
[in]	my	Raw Y magnetometer data ()
[in]	mz	Raw Z magnetometer data ()

Definition at line 276 of file imufilter.hpp.

---

Member Data Documentation

double ImuFilter::accDev_ [protected]

Definition at line 495 of file imufilter.hpp.

double ImuFilter::accTh_ [protected]

Accelerometer threshold for filter updating

Definition at line 490 of file imufilter.hpp.

double ImuFilter::accVar_[3] [protected]

Accelerometers variances [varX, varY, varZ]

Definition at line 485 of file imufilter.hpp.

double ImuFilter::biaDev_ [protected]

Definition at line 498 of file imufilter.hpp.

double ImuFilter::biaTh_ [protected]

Definition at line 499 of file imufilter.hpp.

double ImuFilter::biaVar_[3] [protected]

Bias variances [varX, varY, varZ]

Definition at line 488 of file imufilter.hpp.

std::vector<sensor_msgs::Imu> ImuFilter::calibData_ [protected]

IMU data for initialization

Definition at line 507 of file imufilter.hpp.

double ImuFilter::calibTime_ [protected]

IMU calibration time

Definition at line 476 of file imufilter.hpp.

double ImuFilter::gbx_ [protected]

Definition at line 480 of file imufilter.hpp.

double ImuFilter::gby_ [protected]

Definition at line 480 of file imufilter.hpp.

double ImuFilter::gbz_ [protected]

IMU KF state vector x = [rx, ry, rz, gbx, gby, gbz]

Definition at line 480 of file imufilter.hpp.

double ImuFilter::gyrDev_ [protected]

Definition at line 496 of file imufilter.hpp.

double ImuFilter::gyrVar_[3] [protected]

Gyroscopes variances [varX, varY, varZ]

Definition at line 487 of file imufilter.hpp.

bool ImuFilter::init_ [protected]

Flag indicating if IMU has been initialized

Definition at line 492 of file imufilter.hpp.

double ImuFilter::irx_ [protected]

Definition at line 512 of file imufilter.hpp.

double ImuFilter::iry_ [protected]

Definition at line 512 of file imufilter.hpp.

double ImuFilter::irz_ [protected]

Integrated angles since last reset

Definition at line 512 of file imufilter.hpp.

double ImuFilter::magCal_[6] [protected]

Sensor calib info [gainX, gainY, gainZ, offsetX, offsetY, offsetZ]

Definition at line 483 of file imufilter.hpp.

double ImuFilter::magDev_ [protected]

Definition at line 497 of file imufilter.hpp.

double ImuFilter::magVar_[3] [protected]

Magnetometers variances [varX, varY, varZ]

Definition at line 486 of file imufilter.hpp.

double ImuFilter::magXo_ [protected]

Definition at line 503 of file imufilter.hpp.

double ImuFilter::magXs_ [protected]

Definition at line 500 of file imufilter.hpp.

double ImuFilter::magYo_ [protected]

Definition at line 504 of file imufilter.hpp.

double ImuFilter::magYs_ [protected]

Definition at line 501 of file imufilter.hpp.

double ImuFilter::magZo_ [protected]

Definition at line 505 of file imufilter.hpp.

double ImuFilter::magZs_ [protected]

Definition at line 502 of file imufilter.hpp.

ros::NodeHandle ImuFilter::nh_ [protected]

ROS node handler

Definition at line 509 of file imufilter.hpp.

Eigen::MatrixXd ImuFilter::P_ [protected]

IMU KF matrix

Definition at line 481 of file imufilter.hpp.

double ImuFilter::rx_ [protected]

Definition at line 480 of file imufilter.hpp.

double ImuFilter::ry_ [protected]

Definition at line 480 of file imufilter.hpp.

double ImuFilter::rz_ [protected]

Definition at line 480 of file imufilter.hpp.

ros::Subscriber ImuFilter::sub_ [protected]

IMU data subscriber

Definition at line 510 of file imufilter.hpp.

double ImuFilter::T2_ [protected]

Squared IMU KF prediction period

Definition at line 479 of file imufilter.hpp.

double ImuFilter::T_ [protected]

IMU KF prediction period

Definition at line 478 of file imufilter.hpp.

---

The documentation for this class was generated from the following file:

• imufilter.hpp