Custom extention GPS Kalman filter