Calculating position

Some code for estimating position (not calibrated)