Yaw error normalization was happening at the wrong place

Duh! This probably was the cause of issue #2, I haven't tested the fix
in a real situation yet.