Added GPS error correction code. Fixed Matlab tests.