Re-implemented Observation class to work with OMPL
Addeed tinyxml to read XML config files