diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
index bf687fc7c..244c00561 100644
--- a/mavros/CMakeLists.txt
+++ b/mavros/CMakeLists.txt
@@ -37,6 +37,25 @@ endif()
 
 include(EnableCXX11)
 include(MavrosMavlink)
+include(ExternalProject)
+
+# The configure step fails at catkin_package() if this directory
+# doesn't exist yet. Create it at configure time!
+file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include)
+
+ExternalProject_Add(
+    geographic_lib
+    GIT_REPOSITORY git://git.code.sourceforge.net/p/geographiclib/code
+    PATCH_COMMAND mkdir -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/data
+    CONFIGURE_COMMAND cd ${CMAKE_BINARY_DIR}/geographic_lib-prefix/src/geographic_lib && cmake -D CMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} -D GEOGRAPHICLIB_LIB_TYPE=SHARED -D GEOGRAPHICLIB_DATA=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data .
+    BUILD_COMMAND cd ${CMAKE_BINARY_DIR}/geographic_lib-prefix/src/geographic_lib && make -j1
+    INSTALL_COMMAND cd ${CMAKE_BINARY_DIR}/geographic_lib-prefix/src/geographic_lib && make install &&
+        ./tools/geographiclib-get-geoids -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96-5 &&
+        ./tools/geographiclib-get-gravity -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96 &&
+        ./tools/geographiclib-get-magnetic -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data emm2015
+)
+set(GEOGRAPHICLIB_INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include)
+set(GeographicLib_LIBRARIES ${CATKIN_DEVEL_PREFIX}/lib/libGeographic${CMAKE_SHARED_LIBRARY_SUFFIX})
 
 # detect if sensor_msgs has BatteryState.msg
 # http://answers.ros.org/question/223769/how-to-check-that-message-exists-with-catkin-for-conditional-compilation-sensor_msgsbatterystate/
@@ -79,6 +98,7 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
   ${mavlink_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
+  ${GEOGRAPHICLIB_INCLUDE_DIRS}
 )
 
 add_library(mavros
@@ -97,7 +117,9 @@ target_link_libraries(mavros
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
 )
-add_dependencies(mavros mavros_msgs_generate_messages_cpp)
+add_dependencies(mavros
+  mavros_msgs_generate_messages_cpp
+)
 
 add_library(mavros_plugins
   src/plugins/dummy.cpp
@@ -127,11 +149,13 @@ add_library(mavros_plugins
 )
 add_dependencies(mavros_plugins
   mavros
+  geographic_lib
 )
 target_link_libraries(mavros_plugins
   mavros
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${GeographicLib_LIBRARIES}
 )
 
 ## Declare a cpp executable
@@ -140,11 +164,13 @@ add_executable(mavros_node
 )
 add_dependencies(mavros_node
   mavros
+  geographic_lib
 )
 target_link_libraries(mavros_node
   mavros
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${GeographicLib_LIBRARIES}
 )
 
 add_executable(gcs_bridge
diff --git a/mavros/package.xml b/mavros/package.xml
index eb828f348..925d60564 100644
--- a/mavros/package.xml
+++ b/mavros/package.xml
@@ -41,6 +41,7 @@
   <depend>tf2_ros</depend>
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <depend>geographic_lib</depend>
 
   <!-- message packages -->
   <depend>diagnostic_msgs</depend>
diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp
index 16ef92d89..805627f04 100644
--- a/mavros/src/plugins/global_position.cpp
+++ b/mavros/src/plugins/global_position.cpp
@@ -20,6 +20,7 @@
 #include <mavros/mavros_plugin.h>
 #include <mavros/gps_conversions.h>
 #include <eigen_conversions/eigen_msg.h>
+#include <GeographicLib/Geoid.hpp>
 
 #include <std_msgs/Float64.h>
 #include <nav_msgs/Odometry.h>
@@ -30,8 +31,6 @@
 
 namespace mavros {
 namespace std_plugins {
-
-
 /**
  * @brief Global position plugin.
  *
@@ -75,9 +74,9 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 	Subscriptions get_subscriptions()
 	{
 		return {
-				make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
+			       make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
 				// GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
-				make_handler(&GlobalPositionPlugin::handle_global_position_int)
+			       make_handler(&GlobalPositionPlugin::handle_global_position_int)
 		};
 	}
 
@@ -99,16 +98,19 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 
 	template<typename MsgT>
 	inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) {
+		GeographicLib::Geoid egm96("egm96-5");
 		fix->latitude = msg.lat / 1E7;		// deg
 		fix->longitude = msg.lon / 1E7;		// deg
-		fix->altitude = msg.alt / 1E3;		// m
+		fix->altitude = msg.alt / 1E3 +		// conversion from height abov geoid (AMSL)
+					GeographicLib::Geoid::GEOIDTOELLIPSOID	// to height above ellipsoid (WGS-84)
+					* egm96(fix->latitude, fix->longitude);		// in meters
 	}
 
 	inline void fill_unknown_cov(sensor_msgs::NavSatFix::Ptr fix) {
 		fix->position_covariance.fill(0.0);
 		fix->position_covariance[0] = -1.0;
 		fix->position_covariance_type =
-			sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
+					sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
 	}
 
 	/* -*- message handlers -*- */
@@ -137,10 +139,10 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 
 			// From nmea_navsat_driver
 			fix->position_covariance[0 + 0] = \
-				fix->position_covariance[3 + 1] = std::pow(hdop, 2);
+						fix->position_covariance[3 + 1] = std::pow(hdop, 2);
 			fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
 			fix->position_covariance_type =
-					sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
+						sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
 		}
 		else {
 			fill_unknown_cov(fix);
@@ -151,7 +153,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 		raw_fix_pub.publish(fix);
 
 		if (raw_gps.vel != UINT16_MAX &&
-				raw_gps.cog != UINT16_MAX) {
+					raw_gps.cog != UINT16_MAX) {
 			double speed = raw_gps.vel / 1E2;				// m/s
 			double course = angles::from_degrees(raw_gps.cog / 1E2);	// rad
 
@@ -218,8 +220,8 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 
 		// Velocity
 		tf::vectorEigenToMsg(
-				Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
-				odom->twist.twist.linear);
+					Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
+					odom->twist.twist.linear);
 
 		// Velocity covariance unknown
 		ftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
@@ -244,12 +246,12 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 		ftf::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
 		ftf::EigenMapCovariance6d pos_cov_out(odom->pose.covariance.data());
 		pos_cov_out <<
-			gps_cov(0, 0) , gps_cov(0, 1) , gps_cov(0, 2) , 0.0     , 0.0     , 0.0     ,
-			gps_cov(1, 0) , gps_cov(1, 1) , gps_cov(1, 2) , 0.0     , 0.0     , 0.0     ,
-			gps_cov(2, 0) , gps_cov(2, 1) , gps_cov(2, 2) , 0.0     , 0.0     , 0.0     ,
-			0.0           , 0.0           , 0.0           , rot_cov , 0.0     , 0.0     ,
-			0.0           , 0.0           , 0.0           , 0.0     , rot_cov , 0.0     ,
-			0.0           , 0.0           , 0.0           , 0.0     , 0.0     , rot_cov ;
+		gps_cov(0, 0), gps_cov(0, 1), gps_cov(0, 2), 0.0, 0.0, 0.0,
+		gps_cov(1, 0), gps_cov(1, 1), gps_cov(1, 2), 0.0, 0.0, 0.0,
+		gps_cov(2, 0), gps_cov(2, 1), gps_cov(2, 2), 0.0, 0.0, 0.0,
+		0.0, 0.0, 0.0, rot_cov, 0.0, 0.0,
+		0.0, 0.0, 0.0, 0.0, rot_cov, 0.0,
+		0.0, 0.0, 0.0, 0.0, 0.0, rot_cov;
 
 		// publish
 		gp_fix_pub.publish(fix);