Added Comments
Added angle calculating and preliminary heading