ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# How can I compute yaw rate from yaw angle?

Hi, I want to find a way to compute the yaw rate using the yaw angle. I have the idea of using the Euler forward method, however, I have encountered some issues like the fact that a full rotation is given by [0-pi, pi-0] and also considering multiple rotations (positives or negatives). The following code only works fine for 1 rotation.

void BebopVel::attitudeCallback(const bebop_msgs::Ardrone3PilotingStateAttitudeChanged &att_msg){

double yaw = normalize_angle(-att_msg.yaw);
std::cout << "Curr heading: " << yaw*180/M_PI << std::endl;

if(!got_first_att_msg_){

ROS_INFO("got first attitude message");

prev_yaw_ = yaw;

got_first_att_msg_ = true;
return;
}

// get current values
curr_yaw_ = yaw;

// compute time difference between samples
ros::Duration delta_t_attitude = curr_attitude_time_ - prev_attitude_time_;
double ts_attitude = delta_t_attitude.toSec();      // convert from ROS time to seconds

// compute yaw difference between samples
double delta_yaw = curr_yaw_ - prev_yaw_;

// compute yaw rate
yaw_rate_ = delta_yaw / ts_attitude;

// assign current values to previous
prev_attitude_time_ = curr_attitude_time_;
prev_yaw_ = curr_yaw_;
}

double BebopVel::normalize_angle(double ang){
// Normalizes the angle to be 0 to 2*pi
// It takes and returns radians.

double norm_ang = fmod(ang, 2.0*M_PI);

if(norm_ang < 0){
return norm_ang + 2.0*M_PI;
}

return norm_ang;
}


PD: Sorry for my poor English.

edit retag close merge delete

1

Well when you normalize the angle you are throwing away all the information about multiple rotations, is that function necessary in this context? Or, you could modify the function to return the number of rotations (forward or backward) then use that in the euler equation.

( 2020-11-18 17:01:21 -0600 )edit

Sort by ยป oldest newest most voted

If I'm not mistakent, with your current code, you should get correct resuts when you are not just at this moment finishing one turn.

I would suggest you replace

// compute yaw difference between samples
double delta_yaw = curr_yaw_ - prev_yaw_;


with

// compute yaw difference between samples
double delta_yaw = fmod(curr_yaw_ - prev_yaw_ ,  2.0*M_PI);


It should then be able to deal with finishing one turn.

The only problem that will remain, is if you do more that pi rads between 2 successive calls to the function (not sure if it can happen in your system). If you need to be able to do more than half a turn between 2 calls, then you might try to remove the normalization (normalize_angle) and remove my fmod : if this works or not will depend if your att_msg.yaw evolving continuously or if it can have 2*pi jumps (in this last case, I think nothing can be done)

more