diff --git a/flight/Libraries/WorldMagModel.c b/flight/Libraries/WorldMagModel.c
index cae0ec106..83f413200 100644
--- a/flight/Libraries/WorldMagModel.c
+++ b/flight/Libraries/WorldMagModel.c
@@ -242,7 +242,7 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
     {
         CoordGeodetic->lambda = Lon;
         CoordGeodetic->phi = Lat;
-        CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid;
+        CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0; // convert to km
 
         // Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report
         if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0)
diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c
index 5851ac10d..0428cb629 100644
--- a/flight/Modules/GPS/GPS.c
+++ b/flight/Modules/GPS/GPS.c
@@ -66,7 +66,7 @@ static float GravityAccel(float latitude, float longitude, float altitude);
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 // Unfortunately need a good size stack for the WMM calculation
-	#define STACK_SIZE_BYTES            750
+	#define STACK_SIZE_BYTES            784
 #else
 #if defined(PIOS_GPS_MINIMAL)
 	#define STACK_SIZE_BYTES            500
diff --git a/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp b/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp
index ce8201ece..207d4b051 100644
--- a/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp
+++ b/ground/openpilotgcs/src/libs/utils/worldmagmodel.cpp
@@ -158,7 +158,7 @@ namespace Utils {
     {
         double Lat = LLA[0];
         double Lon = LLA[1];
-        double AltEllipsoid = LLA[2];
+        double AltEllipsoid = LLA[2]/1000.0; // convert to km
 
         // ***********
         // range check supplied params
diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m b/matlab/GCSPlugin/GCSControl.m
similarity index 100%
rename from ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m
rename to matlab/GCSPlugin/GCSControl.m