diff --git a/src/lib/lat_lon_alt/lat_lon_alt.cpp b/src/lib/lat_lon_alt/lat_lon_alt.cpp
index d9ac442e3a..40b86a635a 100644
--- a/src/lib/lat_lon_alt/lat_lon_alt.cpp
+++ b/src/lib/lat_lon_alt/lat_lon_alt.cpp
@@ -77,7 +77,7 @@ Vector3d LatLonAlt::toEcef() const
 
 	return Vector3d(r_total * cos_lat * cos_lon,
 			r_total * cos_lat * sin_lon,
-			((1.0 - Wgs84::eccentricity2) * r_total) * sin_lat);
+			((1.0 - Wgs84::eccentricity2) * r_e + static_cast<double>(_altitude)) * sin_lat);
 }
 
 Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const
diff --git a/src/lib/lat_lon_alt/test_lat_lon_alt.cpp b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp
index b7a42ebd48..f2419951d5 100644
--- a/src/lib/lat_lon_alt/test_lat_lon_alt.cpp
+++ b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp
@@ -105,3 +105,17 @@ TEST(TestLatLonAlt, subLatLonAlt)
 	EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2);
 	EXPECT_EQ(delta_pos(2), delta_pos_true(2));
 }
+
+TEST(TestLatLonAlt, fromAndToECEF)
+{
+	for (double lat = -M_PI; lat < M_PI; lat += M_PI / 4.0) {
+		for (double lon = -M_PI; lon < M_PI; lon += M_PI / 4.0) {
+			for (float alt = -500.f; alt < 8000.f; alt += 500.f) {
+				LatLonAlt lla(lat, lon, alt);
+
+				LatLonAlt res = LatLonAlt::fromEcef(lla.toEcef());
+				EXPECT_TRUE(!(lla - res).longerThan(10e-6f)) << "lat: " << lat << ", lon: " << lon << ", alt: " << alt;
+			}
+		}
+	}
+}