Fix head pose matrix update