Ask Your Question

How to do a frequency analysis of some velocity profiles?

asked 2012-10-08 17:05:57 -0500

Astronaut gravatar image

updated 2012-10-08 21:08:15 -0500


I have some question regarding analysing some post-processing data. I have extracted a linear and angular velocity of the robot moving around. The velocity is coming from AMCL and angular velocity is calculating with the help of IMU unit (just use the position/orientation) and AMCL as well. I made different bag files( so different robot runs) and I have significant peeks (noises) in both profiles( in linear velocity not so much noises bit the angular velocity have significant noises). So thought a frequency analysis of the velocity profiles might reveal more high components in both velocity profiles. But dont know exactly how to do it? Also where this noises can come from??

Im posting one of my profiles. Angular speedimage description

And the linear one!!image description

This IS the code for the speed calculation

void speedCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)

    btScalar r,p,y;
    btQuaternion q(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
    std_msgs::Float64 yaw; = y;

    if (count2 == 0){
        time_prev = ros::Time::now().toSec();

    double time_now = ros::Time::now().toSec();
    double curr_theta =;
    double curr_x = msg->pose.pose.position.x;
    double curr_y = msg->pose.pose.position.y;

    if (curr_x < 0){
        curr_x = curr_x * -1;
    if (curr_y < 0){
        curr_y = curr_y * -1;
    double time_dif = time_now - time_prev;
    if (prev_x != 0 || prev_y != 0){
        if (time_dif != 0){
            double dist_theta = (curr_theta - prev_theta);
            if (dist_theta < 0){
                dist_theta = dist_theta * -1;
            double dist_x = (curr_x - prev_x);
            if (dist_x < 0){
                dist_x = dist_x * -1;
            double dist_y = (curr_y - prev_y);
            if (dist_y < 0){
                dist_y = dist_y * -1;
            double dist_tot = sqrt((dist_x * dist_x) + (dist_y * dist_y));// * 10;
            double speed = (dist_tot / time_dif);// * 3.6;
            double speed_theta = dist_theta /time_dif;
            if (speed < 0.12){
                speed = 0;
            if (speed_theta < 0.07){
                speed_theta = 0;

            ROS_INFO("linear speed= %f, anglular speed=  %f", speed, speed_theta);
            myfile <<  speed << "   " << speed_theta << "\n";
            speed_sum = speed_sum + speed;
            speed_sum_theta = speed_sum_theta + speed_theta;
            if (speed_count == 20){
                speed = speed_sum / speed_count;
                speed_theta = speed_sum_theta / speed_count;
                if (zero_count > 7){
                    speed = 0;
                speed_count = 0;
                speed_sum = 0;
                speed_sum_theta = 0;
                zero_count = 0;

                count = count + 1;


    prev_theta = curr_theta;
    prev_x = curr_x;
    prev_y = curr_y;

    time_prev = time_now;



edit retag flag offensive close merge delete


Why are you trying to calculate velocities based on the positon/orientation from AMCL?

Eric Perko gravatar image Eric Perko  ( 2012-10-08 17:14:30 -0500 )edit

I just use the velocity formula to calculate it, And for that I need a time and the position(which I get from amcl). Only that . Whats wrong with that??

Astronaut gravatar image Astronaut  ( 2012-10-08 18:13:22 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2012-10-08 18:32:41 -0500

Chad Rockey gravatar image

Amcl has the ability to "snap" the odometry to a new reference point. This can produce the uneven results you're seeing. (It's as if your robot instantly jumped to the new position, so your output is the true instantaneous velocity according to AMCL.)

Typically, Odometry is actually fairly well estimated velocities and angular velocities integrated. AMCL is there to handle the accumulated integral error. Most applications do not require more accurate velocities and angular velocities than wheel encoders/gyros/IMUs produce. Are you trying to calibrate the system in an online manner?

Also, I suspect that the large jumps in angular velocity may be subtracting radians instead of quaternions. The angular difference between 3.1 and -3.1 is 0.08 radians, not -6.2 radians (Unless your robot spins very quickly).

edit flag offensive delete link more


Sorry. So dont know what you mean exactly? SO if not use amcl than how to calculate the speed?I can post my code for speed calculation

Astronaut gravatar image Astronaut  ( 2012-10-08 19:15:10 -0500 )edit

Do you have an /odom topic? That should include an estimate of linear and angular velocity. If not, using your current method on /odom instead of /amcl_pose should produce much better results.

Chad Rockey gravatar image Chad Rockey  ( 2012-10-08 19:23:47 -0500 )edit

No, I dont have odom topic. Only amcl . So what you suggest??

Astronaut gravatar image Astronaut  ( 2012-10-08 19:25:59 -0500 )edit

What publishes the tf estimate of odometry from your robot? AMCL takes in a laser scan, odometry (through tf), and a map, and publishes tf and amcl_pose. Does your robot have wheel odometry (encoders, IMU)?

Chad Rockey gravatar image Chad Rockey  ( 2012-10-08 19:28:39 -0500 )edit

My robot has IMU, laser scan and kinect. How to see what publishes the tf estimate of odometry from the robot??

Astronaut gravatar image Astronaut  ( 2012-10-08 19:38:10 -0500 )edit

Is this a Turtlebot?

On any robot, you can view the various rates and publishers of /tf by using the view_frames tool:

You're looking for the Broadcaster of /odom -> /base_link.

Chad Rockey gravatar image Chad Rockey  ( 2012-10-08 19:41:11 -0500 )edit

No. No Im using fuerte and my robot is just a electrical wheelchair.

Astronaut gravatar image Astronaut  ( 2012-10-08 19:48:56 -0500 )edit

I dont have that . Have Broadcaster : amcl

Astronaut gravatar image Astronaut  ( 2012-10-08 20:31:22 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools


Asked: 2012-10-08 17:05:57 -0500

Seen: 492 times

Last updated: Oct 08 '12