# How to do a frequency analysis of some velocity profiles?

Hello

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 speed

And the linear one!!

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);
btMatrix3x3(q).getEulerYPR(y,p,r);
std_msgs::Float64 yaw;
yaw.data = y;

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

double time_now = ros::Time::now().toSec();
double curr_theta = yaw.data;
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;
zero_count++;
}
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_count++;
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;

}


Frames.pdf

Originally posted by Astronaut on ROS Answers with karma: 330 on 2012-10-08

Post score: 0

Comment by Eric Perko on 2012-10-08:
Why are you trying to calculate velocities based on the positon/orientation from AMCL?

Comment by Astronaut on 2012-10-08:
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??

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).

Originally posted by Chad Rockey with karma: 4541 on 2012-10-08

This answer was ACCEPTED on the original site

Post score: 6

Comment by Astronaut on 2012-10-08:
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

Comment by Chad Rockey on 2012-10-08:
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.

Comment by Astronaut on 2012-10-08:
No, I dont have odom topic. Only amcl . So what you suggest??

Comment by Chad Rockey on 2012-10-08:
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)?

Comment by Astronaut on 2012-10-08:
My robot has IMU, laser scan and kinect. How to see what publishes the tf estimate of odometry from the robot??

Comment by Chad Rockey on 2012-10-08:
Is this a Turtlebot?

On any robot, you can view the various rates and publishers of /tf by using the view_frames tool: http://www.ros.org/wiki/tf/Tutorials/Introduction%20to%20tf#Using_view_frames

Comment by Astronaut on 2012-10-08:
No. No Im using fuerte and my robot is just a electrical wheelchair.

Comment by Astronaut on 2012-10-08:
I dont have that . Have Broadcaster : amcl

Comment by Astronaut on 2012-10-08:
Why cant upload the tf view image?????

Comment by Chad Rockey on 2012-10-08:
If you're having trouble posting the image, try http://imgur.com/ and post the link here.

Comment by Astronaut on 2012-10-08:
ok. have done it. Can you see it??But as already said im not using odom. Im using amcl. dont have any odom topic

Comment by Chad Rockey on 2012-10-08:
No, I cannot see the view_frames. view frames outputs a pdf. You could try taking a screenshot.

Comment by Astronaut on 2012-10-08:
I put it on the link. http://imgur.com/edit?deletehash=67Ho6HJKfk3hh5Q

Comment by Astronaut on 2012-10-08:
Can you see now?

Comment by Chad Rockey on 2012-10-08:
I see, you're using entirely laserscan for Odometry. You may have better luck trying this technique on the raw /pose2D.

Comment by Astronaut on 2012-10-08:
So which technique and how to implement it??

Comment by Astronaut on 2012-10-08:
Everything is laser scan in my application. So for Odometry also using laser scan. At the moment can use only laser scan. So whats the problem?How they come such a big peaks?

Comment by Lorenz on 2012-10-09:

Comment by Astronaut on 2012-10-14:
What do you mean by the angular difference between 3.1 and -3.1 is 0.08 radians, not -6.2 radians?? Where do you see that Im subtracting radians instead of quaternions? Can you help me with that?Ok I saw that linear speed peaks are cause of amcl jumps but what about angular speed?

Comment by Astronaut on 2012-10-14:
And which technique you mean by trying this technique on the raw /pose2D??

Comment by Astronaut on 2012-10-15:
Probably you mean the angular difference between two angles 3.1 and -3.1 (both in degree) in radians is 0.108 . Not 0.08. Its correct but for my angular speed calculation Im using yaw which is in radians. So how come such a huge peaks.

Comment by Astronaut on 2012-10-15:
I solve the problems with the linear speed, Now I dont have any huge peaks, I just tuned the amcl parameters, so my amcl do not jump. But still have the angular speed peaks. Any helps on it?

Comment by Chad Rockey on 2012-10-15:
double dist_theta does not account for the difference between rotating across 0: An angle of 350 degrees could be expressed -10. You're using the 350 degree value, which would result in values much larger than what you expected than how your robot actually rotated (-10).

Comment by Chad Rockey on 2012-10-15:
double dist_theta = (curr_theta - prev_theta); if (dist_theta < 0){ dist_theta = dist_theta * -1; }

Comment by Astronaut on 2012-10-15:
So what to change in the code???

Comment by Astronaut on 2012-10-15:
Just to comment the line if (dist_theta < 0){ dist_theta = dist_theta * -1; }?? so dont need if (dist_theta < 0){ dist_theta = dist_theta * -1; }, wright?

Comment by Astronaut on 2012-10-15: