High Fidelity Localization And Map Building From An Instrumented Probe Vehicle