Partial fix of ptam orientation bug. Started adding lqg to main loop