diff --git a/AirLib/include/common/GeodeticConverter.hpp b/AirLib/include/common/GeodeticConverter.hpp
index 30d27b0f41..cbeb223931 100644
--- a/AirLib/include/common/GeodeticConverter.hpp
+++ b/AirLib/include/common/GeodeticConverter.hpp
@@ -20,6 +20,11 @@ namespace airlib
             setHome(home_latitude, home_longitude, home_altitude);
         }
 
+        GeodeticConverter(const GeoPoint& home_geopoint)
+        {
+            setHome(home_geopoint);
+        }
+
         void setHome(double home_latitude, double home_longitude, float home_altitude)
         {
             home_latitude_ = home_latitude;
@@ -34,10 +39,13 @@ namespace airlib
             geodetic2Ecef(home_latitude_, home_longitude_, home_altitude_, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_);
 
             // Compute ECEF to NED and NED to ECEF matrices
-            double phiP = atan2(home_ecef_z_, sqrt(pow(home_ecef_x_, 2) + pow(home_ecef_y_, 2)));
+            ecef_to_ned_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_);
+            ned_to_ecef_matrix_ = ecef_to_ned_matrix_.inverse();
+        }
 
-            ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
-            ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();
+        void setHome(const GeoPoint& home_geopoint)
+        {
+            setHome(home_geopoint.latitude, home_geopoint.longitude, home_geopoint.altitude);
         }
 
         void getHome(double* latitude, double* longitude, float* altitude)
@@ -133,10 +141,11 @@ namespace airlib
             double x, y, z;
             ned2Ecef(north, east, down, &x, &y, &z);
             ecef2Geodetic(x, y, z, latitude, longitude, altitude);
+        }
 
-            //TODO: above returns wrong altitude if down was positive. This is because sqrt return value would be -ve
-            //but normal sqrt only return +ve. For now we just override it.
-            *altitude = home_altitude_ - down;
+        void ned2Geodetic(const Vector3r& ned_pos, GeoPoint& geopoint)
+        {
+            ned2Geodetic(ned_pos[0], ned_pos[1], ned_pos[2], &geopoint.latitude, &geopoint.longitude, &geopoint.altitude);
         }
 
         void geodetic2Enu(const double latitude, const double longitude, const double altitude,
diff --git a/AirLib/include/physics/Environment.hpp b/AirLib/include/physics/Environment.hpp
index f3e7837ccf..9649ea0a8f 100644
--- a/AirLib/include/physics/Environment.hpp
+++ b/AirLib/include/physics/Environment.hpp
@@ -8,6 +8,7 @@
 #include "common/UpdatableObject.hpp"
 #include "common/CommonStructs.hpp"
 #include "common/EarthUtils.hpp"
+#include "common/GeodeticConverter.hpp"
 
 namespace msr
 {
@@ -53,12 +54,13 @@ namespace airlib
 
             setHomeGeoPoint(initial_.geo_point);
 
-            updateState(initial_, home_geo_point_);
+            updateState(initial_);
         }
 
         void setHomeGeoPoint(const GeoPoint& home_geo_point)
         {
             home_geo_point_ = HomeGeoPoint(home_geo_point);
+            geodetic_converter_.setHome(home_geo_point);
         }
 
         GeoPoint getHomeGeoPoint() const
@@ -87,7 +89,7 @@ namespace airlib
 
         virtual void update() override
         {
-            updateState(current_, home_geo_point_);
+            updateState(current_);
         }
 
     protected:
@@ -106,9 +108,9 @@ namespace airlib
         }
 
     private:
-        static void updateState(State& state, const HomeGeoPoint& home_geo_point)
+        void updateState(State& state)
         {
-            state.geo_point = EarthUtils::nedToGeodetic(state.position, home_geo_point);
+            geodetic_converter_.ned2Geodetic(state.position, state.geo_point);
 
             real_T geo_pot = EarthUtils::getGeopotential(state.geo_point.altitude / 1000.0f);
             state.temperature = EarthUtils::getStandardTemperature(geo_pot);
@@ -122,6 +124,7 @@ namespace airlib
     private:
         State initial_, current_;
         HomeGeoPoint home_geo_point_;
+        GeodeticConverter geodetic_converter_;
     };
 }
 } //namespace