matlab: yaw angle filter wrapping, debugging plot.