CFixing some erros - to make heading detection more robust
Adding prelim planning doc of tasks