diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index d0d0025b2e..f37adb933d 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -57,6 +57,9 @@
 #endif
 
 
+const float Ekf::_k_earth_rate = 0.000072921f;
+const float Ekf::_gravity_mss = 9.80665f;
+
 Ekf::Ekf():
 	_filter_initialised(false),
 	_earth_rate_initialised(false),
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 55ffa5c363..f276f6ed75 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -133,8 +133,8 @@ public:
 private:
 
 	static const uint8_t _k_num_states = 24;
-	const float _k_earth_rate = 0.000072921f;
-	const float _gravity_mss = 9.80665f;
+	static const float _k_earth_rate;
+	static const float _gravity_mss;
 
 	float _dt_ekf_avg;		// average update rate of the ekf
 
diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index 81fb4d13cc..fe2dc0ba76 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -226,7 +226,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float *data)
 	}
 
 	if (time_usec - _time_last_range > 45000) {
-		rangeSample range_sample_new;
+		rangeSample range_sample_new = {};
 		range_sample_new.rng = *data;
 		rng = *data;
 		range_sample_new.time_us -= _params.range_delay_ms * 1000;
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index e4c09b125f..ddc06aa5d4 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -260,11 +260,11 @@ protected:
 	bool _imu_updated;      // true if the ekf should update (completed downsampling process)
 	bool _initialised;      // true if the ekf interface instance (data buffering) is initialized
 
-	bool _NED_origin_initialised = false;
-	bool _gps_speed_valid = false;
-	float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin
-	float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin
-	struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
+	bool _NED_origin_initialised;
+	bool _gps_speed_valid;
+	float _gps_origin_eph; // horizontal position uncertainty of the GPS origin
+	float _gps_origin_epv; // vertical position uncertainty of the GPS origin
+	struct map_projection_reference_s _pos_ref;    // Contains WGS-84 position latitude and longitude (radians)
 
 	bool _mag_healthy;              // computed by mag innovation test
 	bool _airspeed_healthy;			// computed by airspeed innovation test
diff --git a/EKF/geo.cpp b/EKF/geo.cpp
index 55cd5ca47f..a1fa93ab1b 100644
--- a/EKF/geo.cpp
+++ b/EKF/geo.cpp
@@ -42,14 +42,23 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 #ifdef POSIX_SHARED
-#include <unistd.h>
-#include <pthread.h>
+//#include <unistd.h>
+//#include <pthread.h>
 #include <stdio.h>
 #include <math.h>
 #include <stdbool.h>
 #include <string.h>
 #include <float.h>
 
+#ifndef __PX4_QURT
+#if defined(__cplusplus) && !defined(__PX4_NUTTX)
+#include <cmath>
+#define ISFINITE(x) std::isfinite(x)
+#else
+#define ISFINITE(x) isfinite(x)
+#endif
+#endif
+
 /****************************************************************************
  *
  *   Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
@@ -95,6 +104,7 @@
 *
 */
 
+#include <stdint.h>
 #include "geo.h"
 
 /** set this always to the sampling in degrees for the table below */
@@ -708,7 +718,7 @@ float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
 float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!math::isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -738,7 +748,7 @@ float _wrap_pi(float bearing)
 float _wrap_2pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!math::isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -768,7 +778,7 @@ float _wrap_2pi(float bearing)
 float _wrap_180(float bearing)
 {
 	/* value is inf or NaN */
-	if (!math::isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -798,7 +808,7 @@ float _wrap_180(float bearing)
 float _wrap_360(float bearing)
 {
 	/* value is inf or NaN */
-	if (!math::isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}
 
diff --git a/EKF/mathlib.cpp b/EKF/mathlib.cpp
index 2cd1190cf0..c72b959730 100644
--- a/EKF/mathlib.cpp
+++ b/EKF/mathlib.cpp
@@ -56,7 +56,7 @@ float max(float val1, float val2)
 	return (val1 > val2) ? val1 : val2;
 }
 
-float constrain(float &val, float min, float max)
+float constrain(float val, float min, float max)
 {
 	return (val < min) ? min : ((val > max) ? max : val);
 }
diff --git a/EKF/mathlib.h b/EKF/mathlib.h
index a20adad331..0c96a8f9d4 100644
--- a/EKF/mathlib.h
+++ b/EKF/mathlib.h
@@ -41,18 +41,22 @@
 #ifndef MATHLIB_H
 #define MATHLIB_H
 #ifdef POSIX_SHARED
-#include <Eigen/Dense>
-#include <algorithm>
+// #include <Eigen/Dense>
+// #include <algorithm>
 #define M_PI_F 3.14159265358979323846f
 
+#ifndef M_PI
+#define M_PI (3.14159265358979323846f)
+#endif
+
 namespace math
 {
-using namespace Eigen;
+// using namespace Eigen;
 using namespace std;
 
 float min(float val1, float val2);
 float max(float val1, float val2);
-float constrain(float &val, float min, float max);
+float constrain(float val, float min, float max);
 float radians(float degrees);
 float degrees(float radians);