# C++ (ROS node) for Euler Integration to estimate Velocity knowing the acceleration

Hi I have IMU reading and would like to estimate the linear Velocity knowing the linear acceleration from the IMU using the Euler method. Im aware of the accumulated error. So need a C++(python) code ( possible a ROS node) that takes the IMU data reading from the IMU sensor and gives the estimated Linear velocity using the Euler Integration.

I know that acceleration a is

a=dv/dt

and that will be the function.

dv/dt = f(t) . i.e. v(n+1) = v(n) + f(n) * dt

So how are the steps calculated? I have the time stamps from the ros topic.. So any help about the C++ (ROS node) code?

Thanks

• On Robotics we are fortunate enough to have MathJax support enabled, allowing you to easily create subscripts, superscripts, fractions, square roots, greek letters and more. This allows you to add both inline and block element mathematical expressions in robotics questions and answers. For a quick tutorial, take a look at How can I format mathematical expressions here, using MathJax?. Please edit your question to make it clearer.
– Ben
Oct 11 at 12:03
• I'm not clear on what you're asking for help with here. You've got the equation for integration, v(n+1) = v(n) + f(n) * dt, you state you've got the time stamps from the ros topic, so do you need help with the integration or do you need help writing the ros node that will do the work? Oct 11 at 12:07
• both bit help . With the integration and and the rose node.
– bob
Oct 11 at 14:39

I find it easiest to make a class for stuff like this, where you have data you want to use between callbacks, or your want to publish a message inside of another callback. For example, your class might look something like (in the header:)

class IMUIntegrator
{
public:
void start();
private:
void imu_callback(const your_imu::Message::ConstPtr &msg);
void send_velocity();
ros::Publisher  m_velocity_pub;///< Publisher for the stopping estimate marker
ros::Subscriber m_imu_sub; ///< Velocity subscriber
float m_velocity = 0.0f;
float m_prev_timestamp = 0.0f;
}


then your cpp file does something like:

void IMUIntegrator::start()
{
ROS_INFO("Starting...");
// Setup the publisher first so it's ready to go before we trigger the first sub callback.
ros::NodeHandle velocity_node;
ros::NodeHandle imu_node;
m_imu_sub = imu_node.subscribe("/imu", 10, &IMUIntegrator::imu_callback, this);
ros::spin();
}

void IMUIntegrator::imu_callback(const your_imu::Message::ConstPtr &msg)
{
if(m_prev_timestamp==0.0f)
{
m_prev_timestamp = timestamp;
return;
}
float dt = timestamp - m_prev_timestamp;
m_prev_timestamp = timestamp;
m_velocity += msg.acceleration*dt;
send_velocity();
}
void IMUIntegrator::send_velocity()
{
your_velocity::Message message;
message.velocity = m_velocity;
m_velocity_pub.publish(message);
}


and finally you start it from your main cpp file:

int main(int argc, char **argv)
{
ros::init(argc, argv, "velocity_integrator");
IMUIntegrator integrator;
integrator.start();
return 0;
}


I used placeholder names in there for your messages, etc., but that's generally the way it goes. This is all generally taken from the ros tutorials, which are actually pretty excellent, with the only exception that they're mashed together into a class and then used as a class. The class is nice because it lets you use member variables to pass information from the callback to the publisher. Member variables are kind of like global variables, but they're exclusive to the particular class, so you're not polluting your whole project with the information they contain (and other things aren't allowed to touch your class' data without permission!)

• Ok .Thanks. One more question. I like to get the velocity respect to body frame of the robot. So means everything to keep it local. So how to do that transform? This velocity I got is respect the world frame or? I need the actual velocity of the robot. Linear acceleration comes from the magnetometer of the IMU right? So what transformation are need it in order to get the velocity of the body frame. Do I need something like this tf::StampedTransform transform; try{ listener.lookupTransform("/imu_frame", "/base_link_frame", ros::Time(0), transform); }
– bob
Oct 12 at 8:24
• Im not clear the void IMUIntegrator::send_velocity()  part where your_velocity::Message message;  message.header.frame_id = "your_frame"; How exactly should be ?
– bob
Oct 12 at 12:00
• i got this error rror: ‘geometry_msgs::Twist {aka struct geometry_msgs::Twist_<std::allocator<void> >}’ has no member named ‘header’ message.header.frame_id = "your_frame"; ^~~~~~ /home/bojan/uuv_simulator_ws/src/ros_services/src/IntegralVelocity.cpp:62:13: error: ‘geometry_msgs::Twist {aka struct geometry_msgs::Twist_<std::allocator<void> >}’ has no member named ‘header’ message.header.stamp = ros::Time();
– bob
Oct 12 at 12:55
• In the void IMUIntegrator::send_velocity() geometry_msgs::Twist message; message.header.frame_id = "your_frame"; message.header.stamp = ros::Time(); message.linear.x=m_velocity; m_velocity_pub.publish(message);
– bob
Oct 12 at 12:56
• @bob the IMU is always going to report in body coordinates because it's attached to the body of the robot/vehicle. You say you want the actual velocity of the robot`, but... relative to what? In the local frame, x-axis acceleration will give you x-axis speed, but if you're rotated (pitched) then you might actually be going up, or down, etc. Accelerations come from the accelerometers; headings come from the magnetometers. I'm assuming you're wanting this information to set a frame (what is a frame?) so you'd need to set that. Oct 12 at 13:20