diff --git a/EKF/ekf.h b/EKF/ekf.h
index cb0d78c5b3..770887a348 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -201,6 +201,9 @@ private:
 	// return true if successful
 	bool resetMagHeading(Vector3f &mag_init);
 
+	// calculate the magnetic declination to be used by the alignment and fusion processing
+	void calcMagDeclination();
+
 	// reset position states of the ekf (only vertical position)
 	void resetPosition();
 
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index d74ad67cf2..8b82b2b501 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -124,6 +124,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	return true;
 }
 
+// Calculate the magnetic declination to be used by the alignment and fusion processing
+void Ekf::calcMagDeclination()
+{
+	// set source of magnetic declination for internal use
+	if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
+		// use parameter value until GPS is available, then use value returned by geo library
+		if (_NED_origin_initialised) {
+			_mag_declination = _mag_declination_gps;
+			_mag_declination_to_save_deg = math::degrees(_mag_declination);
+
+		} else {
+			_mag_declination = math::radians(_params.mag_declination_deg);
+			_mag_declination_to_save_deg = _params.mag_declination_deg;
+		}
+
+	} else {
+		// always use the parameter value
+		_mag_declination = math::radians(_params.mag_declination_deg);
+		_mag_declination_to_save_deg = _params.mag_declination_deg;
+	}
+}
+
 // This function forces the covariance matrix to be symmetric
 void Ekf::makeSymmetrical()
 {
diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp
index 16a3e502e7..5cb52924f5 100644
--- a/EKF/gps_checks.cpp
+++ b/EKF/gps_checks.cpp
@@ -40,6 +40,7 @@
  */
 
 #include "ekf.h"
+#include <mathlib/mathlib.h>
 
 // GPS pre-flight check bit locations
 #define MASK_GPS_NSATS  (1<<0)
@@ -67,6 +68,8 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 			_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
 			_NED_origin_initialised = true;
 			_last_gps_origin_time_us = _time_last_imu;
+			// set the magnetic declination returned by the geo library using the current GPS position
+			_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
 		}
 	}