Redundant calculation around EKF yaw

[IshitaTakeshi]

@TakaHoribe @YamatoAndo @kminoda

I found this code on ekf_localizer.cpp

  double yaw = tf2::getYaw(pose.pose.pose.orientation);
  const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW);
  const double yaw_error = normalizeYaw(yaw - ekf_yaw);  // normalize the error not to exceed 2 pi
  yaw = yaw_error + ekf_yaw;

Considering that normalizeYaw does the normalization so that the passed angle does not exceed the range [-pi, pi], the calculation in this part looks redundant.

yaw + normalizeYaw(yaw - ekf_yaw) + ekf_yaw

What is the idea behind this?

[kminoda]

(Just a minor comment)
What you mean is this, right?

yaw + normalizeYaw(yaw - ekf_yaw) + ekf_yaw

to

yaw = normalizeYaw(yaw - ekf_yaw) + ekf_yaw

[IshitaTakeshi]

Yes. Thank you for the update