sync : Fused pedestrian mode

- Fusion Engine internal changes
- speed from internal pedometer sensor.

Change-Id: Iccc3087ef2e60e0a241cb229c92c4be7a0f3d8bc
Signed-off-by: Marcin Masternak <m.masternak@samsung.com>

64 files changed, 5258 insertions, 4481 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e42a4a3..8323827 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,7 +8,7 @@ SET(BIN_DIR "${PREFIX}/bin")
 
 #Dependencies
 SET(common_dp "glib-2.0 lbs-dbus dlog gio-2.0 lbs-location")
-SET(server_dp "${common_dp} network tapi vconf vconf-internal-keys gthread-2.0  gio-unix-2.0 capi-network-wifi")
+SET(server_dp "${common_dp} network tapi vconf vconf-internal-keys gthread-2.0  gio-unix-2.0 capi-network-wifi capi-system-info")
 SET(module_dp "${common_dp} gmodule-2.0")
 
 # Set required packages
diff --git a/lbs-server/include/gps_plugin_data_types.h b/lbs-server/include/gps_plugin_data_types.h
index db4f10c..635127a 100644
--- a/lbs-server/include/gps_plugin_data_types.h
+++ b/lbs-server/include/gps_plugin_data_types.h
@@ -90,6 +90,7 @@ typedef struct {
  */
 typedef struct {
 	int	num_of_location;	/**< Number of batch data */
+	pos_data_t *data;
 } batch_data_t;
 
 /**
diff --git a/lbs-server/src/fused.c b/lbs-server/src/fused.c
index b6c2f6c..a241e40 100644
--- a/lbs-server/src/fused.c
+++ b/lbs-server/src/fused.c
@@ -19,190 +19,197 @@
 #define __LOCATION_FUSED_C__
 
 #define  _USE_MATH_DEFINES
+#include <time.h>
 #include <math.h>
 #include <stdlib.h>
 #include <sensor.h>
-#include "fused/types.h"
-#include "fused/conversions.h"
-#include "fused/kalman-filter.h"
+#include <vconf.h>
+#include "fused/FusionEngine.h"
 #include "fused.h"
 #include "debug_util.h"
 
 #define PTR2ENUM(ptr, type)   ((type)(int)((char*)(ptr) - ((char*)0)))
 #define ENUM2PTR(value)       (((char*)0) + (int)(value))
 
-/** Container of the sensor reference data */
+#define PEDOMETER_URI "http://samsung.com/sensor/healthinfo/pedometer/samsung_pedometer"
+
+/** Container of the standard sensor reference data */
 typedef struct {
+	sensor_type_e type;
 	sensor_h handle;
 	sensor_listener_h listener;
+	gboolean is_available;
+	gboolean is_started;
 } _fl_sensor;
 
+/** Labels for a device coordinate system (right-handed) */
+typedef enum {
+	DEV_SPATIAL_X         = Y,
+	DEV_SPATIAL_Y         = X,
+	DEV_SPATIAL_Z         = Z,
+	DEV_SPATIAL_DIMENSION = GEO_DIMENSION
+} fl_dev_spatial;
+
+
 /** Combined status flags and bit-field */
 typedef union {
 	struct {
-		unsigned started:     1;
-		unsigned paused:      1;
+		unsigned started:1;
+		unsigned paused:1;
 	};
 } _fl_status;
 
 /** Fused location data */
 typedef struct {
-	_fl_status			status;
-	gboolean			enabled;
-	gboolean			is_active_sensor[SENSOR_SOURCE_NUM];
-	fl_location			last_location;
-	_fl_sensor			sensor[SENSOR_SOURCE_NUM];
-	fl_sensory_flags	attached_sensors;
-	fl_sensory_flags	started_sensors;
-	gpointer			lbs_server_handle;
-	fused_updated_cb	fused_pos_cb;
+	_fl_status                status;
+	WgsLocation               last_location;
+	_fl_sensor                sensors[SENSOR_SOURCE_COUNT];
+	gpointer                  lbs_server_handle;
+	fused_updated_cb          fused_pos_cb;
+	FusionEngine              fusion_engine;
+	double                    last_gps_speed;
+	time_t                    last_gps_timestamp;
+	time_t                    last_wps_timestamp;
+	double                    utc_offset_sec;
 } LocationFusedData;
 
 static LocationFusedData fused;
 
-static fl_sensory_flags sensor_func_detect()
+static void sensors_create()
 {
 	LOG_FUSED_FUNC;
-	fl_sensory_flags active_sensors = NO_SENSOR_FLAG;
-
-	if (sensor_get_default_sensor(SENSOR_ACCELEROMETER, &fused.sensor[ACCELEROMETER].handle) == SENSOR_ERROR_NONE)
-		active_sensors |= ACCELEROMETER_FLAG;
-
-	if (sensor_get_default_sensor(SENSOR_GYROSCOPE, &fused.sensor[GYROSCOPE].handle) == SENSOR_ERROR_NONE)
-		active_sensors |= GYROSCOPE_FLAG;
+	sensor_error_e ret;
+	unsigned int i;
+	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
+		if (i == PEDOMETER)
+			ret = sensor_get_default_sensor_by_uri(PEDOMETER_URI, &fused.sensors[i].handle);
+		else
+			ret = sensor_get_default_sensor(fused.sensors[i].type, &fused.sensors[i].handle);
+
+		if (ret != SENSOR_ERROR_NONE) {
+			LOG_FUSED(LOG_ERROR, "sensor_get_default_sensor() FAILED, [%d]", i);
+			fused.sensors[i].is_available = FALSE;
+		}
 
-	return active_sensors;
+		ret = sensor_create_listener(fused.sensors[i].handle, &fused.sensors[i].listener);
+		if (ret == SENSOR_ERROR_NONE) {
+			ret = sensor_listener_set_event_cb(fused.sensors[i].listener, FL_SENSOR_SAMPLING_INTERVAL, on_sensor_event, ENUM2PTR(i));
+			if (ret == SENSOR_ERROR_NONE) {
+				sensor_listener_set_option(fused.sensors[i].listener, SENSOR_OPTION_ALWAYS_ON);
+				fused.sensors[i].is_available = TRUE;
+			}
+		}
+	}
 }
 
-static void sensor_func_create()
+static void sensors_clear()
 {
 	LOG_FUSED_FUNC;
-	fl_sensory_flags active_sensors;
-	unsigned i;
-
-	fused.attached_sensors = NO_SENSOR_FLAG;
-	for (i = 0, active_sensors = sensor_func_detect();  active_sensors != NO_SENSOR_FLAG; active_sensors >>= 1) {
-		if (active_sensors & 1) {
-			sensor_error_e ret = sensor_create_listener(fused.sensor[i].handle, &fused.sensor[i].listener);
-			if (ret == SENSOR_ERROR_NONE) {
-				ret = sensor_listener_set_event_cb(fused.sensor[i].listener, FL_SENSOR_SAMPLING_INTERVAL, on_sensor_event, ENUM2PTR(i));
-				if (ret == SENSOR_ERROR_NONE) {
-					sensor_listener_set_option(fused.sensor[i].listener, SENSOR_OPTION_ALWAYS_ON);
-					fused.attached_sensors |= (1 << i);
-					i++;
-					continue;
-				}
-				/* undo when sensor api failed*/
-//				sensor_destroy_listener(fused.sensor[i].listener);
-//				fused.sensor[i].listener = NULL;
-			}
-			/* undo */
-			while (i) {
-				--i;
-				if (fused.sensor[i].listener) {
-					sensor_listener_unset_event_cb(fused.sensor[i].listener);
-					sensor_destroy_listener(fused.sensor[i].listener);
-					fused.sensor[i].listener = NULL;
-				}
-			}
-			fused.attached_sensors = NO_SENSOR_FLAG;
-			LOG_FUSED(LOG_ERROR, "no sensors");
-			return;
-		}
+	unsigned int i;
+	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
+		fused.sensors[i].handle = NULL;
+		fused.sensors[i].listener = NULL;
+		fused.sensors[i].is_available = FALSE;
+		fused.sensors[i].is_started = FALSE;
 	}
-	LOG_FUSED(LOG_DEBUG, "OK, attached_sensors=0x%X", fused.attached_sensors);
 }
 
-static void sensor_func_destroy()
+static void sensors_reset_listeners()
 {
-	LOG_FUNC;
-	fl_sensory_flags f;
-	unsigned i;
-
-	LOG_FUSED(LOG_DEBUG, "attached_sensors=0x%X", fused.attached_sensors);
-	for (i = SENSOR_SOURCE_NUM, f = fused.attached_sensors;  (f & SUPPORTED_SENSORS_MASK) != NO_SENSOR_FLAG && i;) {
-		--i;
-		f <<= 1;
-		if (f & (1 << SENSOR_SOURCE_NUM)) {
-			sensor_listener_unset_event_cb(fused.sensor[i].listener);
-			sensor_destroy_listener(fused.sensor[i].listener);
-			fused.sensor[i].handle   = NULL;
-			fused.sensor[i].listener = NULL;
+	LOG_FUSED_FUNC;
+	unsigned int i;
+	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
+		if (fused.sensors[i].listener) {
+			sensor_listener_unset_event_cb(fused.sensors[i].listener);
+			sensor_destroy_listener(fused.sensors[i].listener);
+			fused.sensors[i].listener = NULL;
 		}
 	}
-	fused.attached_sensors = NO_SENSOR_FLAG;
 }
 
-static LocationError sensor_func_start(fl_sensory_flags sensors)
+static void sensors_destroy()
 {
 	LOG_FUSED_FUNC;
-	if ((sensors & ~fused.attached_sensors) == NO_SENSOR_FLAG) {
-		fl_sensory_flags flag;
-		unsigned i;
-		sensor_error_e ret;
-
-		sensors &= ~fused.started_sensors;
-		for (i = 0, flag = sensors;  flag != NO_SENSOR_FLAG; flag >>= 1) {
-			if (flag & 1) {
-				ret = sensor_listener_start(fused.sensor[i].listener);
-				if (ret == SENSOR_ERROR_NONE) {
-					i++;
-					continue;
-				 } else {
-					/* undo */
-					while (i) {
-						--i;
-						if (sensors & (1 << i)) // fused.sensor[i].listener)
-							sensor_listener_stop(fused.sensor[i].listener);
-					}
-					LOG_FUSED(LOG_ERROR, "UNKNOWN, sensor_listener_start(): %d", ret);
-					return LOCATION_ERROR_CONFIGURATION;
-				}
-			}
+	sensors_reset_listeners();
+	sensors_clear();
+}
+
+static void sensor_start(fl_sensor_source sensor)
+{
+	LOG_FUSED_FUNC;
+	if (fused.sensors[sensor].handle
+			&& fused.sensors[sensor].listener
+			&& fused.sensors[sensor].is_available
+			&& !fused.sensors[sensor].is_started) {
+		sensor_error_e ret = sensor_listener_start(fused.sensors[sensor].listener);
+		if (ret == SENSOR_ERROR_NONE) {
+			fused.sensors[sensor].is_started = TRUE;
 		}
-		fused.started_sensors |= sensors;
-		LOG_FUSED(LOG_DEBUG, "(0x%X): OK", sensors);
-		return LOCATION_ERROR_NONE;
-	} else {
-		LOG_FUSED(LOG_ERROR, "(0x%X): NOT_SUPPORTED", sensors);
-		return LOCATION_ERROR_NOT_SUPPORTED;
 	}
 }
 
-static void sensor_func_stop(fl_sensory_flags sensors)
+static void sensor_stop(fl_sensor_source sensor)
 {
 	LOG_FUSED_FUNC;
-	fl_sensory_flags f;
-	unsigned         s;
-
-	LOG_FUSED(LOG_DEBUG, "(0x%X)", sensors);
-	sensors &= fused.started_sensors;
-	for (s = SENSOR_SOURCE_NUM, f = sensors;  (f & SUPPORTED_SENSORS_MASK) != NO_SENSOR_FLAG && s;) {
-		--s;
-		f <<= 1;
-		if (f & (1 << SENSOR_SOURCE_NUM))
-			sensor_listener_stop(fused.sensor[s].listener);
+	if (fused.sensors[sensor].handle
+			&& fused.sensors[sensor].listener
+			&& fused.sensors[sensor].is_available
+			&& fused.sensors[sensor].is_started) {
+		sensor_error_e ret = sensor_listener_stop(fused.sensors[sensor].listener);
+		if (ret == SENSOR_ERROR_NONE) {
+			fused.sensors[sensor].is_started = FALSE;
+		}
 	}
-	fused.started_sensors &= ~sensors;
+}
+
+unsigned long long get_current_boot_time()
+{
+	struct timespec time;
+#ifdef CLOCK_BOOTTIME
+	clock_gettime(CLOCK_BOOTTIME, &time);
+#else
+	clock_gettime(CLOCK_MONOTONIC_RAW, &time);
+#endif
+	return (unsigned long long) time.tv_sec * 1000000000 + time.tv_nsec;
 }
 
 void location_fused_start()
 {
 	LOG_FUSED_FUNC;
-	fl_sensory_flags sensors = GYROSCOPE_FLAG | ACCELEROMETER_FLAG;
-	int ret = sensor_func_start(sensors);
-	LBS_FUSED_CHECK(ret, "sensor_func_start");
 
-	fused_engine_start();
+	char *calibration = vconf_get_str(VCONFKEY_LOCATION_GYRO_DRIFT);
+	LOG_FUSED(LOG_DEBUG, "calibration: vconf_get_str(VCONFKEY_LOCATION_GYRO_DRIFT) %s = \"%s\"",
+			calibration != NULL ? "SUCCESS" : "FAILED",
+			calibration != NULL ? calibration : "EMPTY");
+	fusion_engine_set_calibration(&fused.fusion_engine, calibration);
+
 	fused.status.started = TRUE;
+	fusion_engine_start(&fused.fusion_engine);
+	unsigned int i;
+	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
+		sensor_start(i);
+	}
 }
 
 void location_fused_stop()
 {
 	LOG_FUSED_FUNC;
-	fused_engine_stop();
+	unsigned int i;
+	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
+		sensor_stop(i);
+	}
+	fusion_engine_stop(&fused.fusion_engine);
 	fused.status.started = FALSE;
-	sensor_func_stop(fused.started_sensors);
+
+	char *calibration = fusion_engine_get_calibration(&fused.fusion_engine);
+	if (calibration) {
+		int ret = vconf_set_str(VCONFKEY_LOCATION_GYRO_DRIFT, calibration);
+		LOG_FUSED(LOG_DEBUG, "calibration: vconf_set_str(VCONFKEY_LOCATION_GYRO_DRIFT) %s = \"%s\"",
+				ret == VCONF_OK ? "SUCCESS" : "FAILED", calibration);
+		free(calibration);
+	} else {
+		LOG_FUSED(LOG_DEBUG, "fusion_engine_get_calibration() FAILED");
+	}
 }
 
 void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server)
@@ -212,31 +219,40 @@ void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server)
 		LOG_FUSED(LOG_ERROR, "location_fused_init failed !!!");
 		return;
 	}
-
-	fused.attached_sensors = NO_SENSOR_FLAG;
-	fused.started_sensors = NO_SENSOR_FLAG;
+	fused.sensors[ACCELEROMETER].type = SENSOR_ACCELEROMETER;
+	fused.sensors[GYROSCOPE].type = SENSOR_GYROSCOPE;
+	fused.sensors[PEDOMETER].type = SENSOR_HUMAN_PEDOMETER;
 	fused.fused_pos_cb = _fused_pos_cb;
 	fused.lbs_server_handle = lbs_server;
-
-	sensor_func_create();
-	fused_engine_init(on_motion_state, on_engine_position, lbs_server);
+	fused.status.started = FALSE;
+	fused.status.paused = FALSE;
+	fused.last_gps_speed = -1.0;
+	fused.last_gps_timestamp = 0;
+	fused.last_wps_timestamp = 0;
+	sensors_clear();
+	sensors_create();
+
+	time_t now = time(NULL);
+	unsigned long long boot_time_ns = get_current_boot_time();
+	fused.utc_offset_sec = (double)now - (double)boot_time_ns / 1000000000.0;
+	LOG_FUSED(LOG_DEBUG, "now = %lu, boot_time_ns = %llu, utc_offset_sec = %.10f", now, boot_time_ns, fused.utc_offset_sec);
+	fusion_engine_init(&fused.fusion_engine, on_motion_state, fused.utc_offset_sec);
 }
 
 void location_fused_deinit()
 {
 	LOG_FUSED_FUNC;
-
-	// stop sensors
-	if (fused.status.started) {
-		fused.status.started = FALSE;
-		fused_engine_stop();
-		sensor_func_stop(fused.started_sensors);
-	}
-	fused_engine_exit();
-	sensor_func_destroy();
-
-	fused.fused_pos_cb		= NULL;
+	if (fused.status.started)
+		location_fused_stop();
+	sensors_destroy();
+	fusion_engine_exit(&fused.fusion_engine);
+	fused.fused_pos_cb = NULL;
 	fused.lbs_server_handle = NULL;
+	fused.status.started = FALSE;
+	fused.status.paused = FALSE;
+	fused.last_gps_speed = -1.0;
+	fused.last_gps_timestamp = 0;
+	fused.last_wps_timestamp = 0;
 }
 
 void send_gps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double altitude,
@@ -244,60 +260,118 @@ void send_gps_position_to_fused_engine(time_t timestamp, double latitude, double
 {
 //	LOG_FUSED_FUNC;
 
-	LocationPosition *pos = location_position_new(timestamp, latitude, longitude, altitude, LOCATION_STATUS_3D_FIX);
-	LocationVelocity *vel = location_velocity_new(timestamp, speed, bearing, 0.0);
-	LocationAccuracy *accuracy = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy);
+	if (!fused.status.started)
+		return;
+
+	fused.last_gps_timestamp = timestamp;
+	fused.last_gps_speed = speed;
 
-	static fl_uncertainty_union __u;
-	__u.accuracy = *accuracy;
-	__u.sigma.of_speed = __u.sigma.of_horizontal_pos * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA;
-	__u.sigma.of_climb = __u.sigma.of_altitude       * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA;
-	const fl_sigma* sigma = &__u.sigma;
-	fused_engine_process_position_2x3d_event(pos, vel, sigma);
+	bool is_new_position = fusion_engine_position_3d_event(&fused.fusion_engine,
+						(double)timestamp, latitude, longitude, altitude, hor_accuracy, 100.0, speed / 3.6,
+						hor_accuracy * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA, bearing, 0.0,
+						100.0 * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA);
 
-	location_position_free(pos);
-	location_velocity_free(vel);
-	location_accuracy_free(accuracy);
+	if (is_new_position)
+		on_engine_new_position();
 }
 
-void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy, double ver_accuracy)
+void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy)
 {
-	LOG_FUSED_FUNC;
-	LocationPosition *pos = location_position_new(timestamp, latitude, longitude, 0.0, LOCATION_STATUS_2D_FIX);
-	LocationAccuracy *accuracy = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy);
-
-	static fl_uncertainty_union __u;
-	__u.accuracy = *accuracy;
-	__u.sigma.of_speed = -1;
-	__u.sigma.of_climb = -1;
-	const fl_sigma* sigma = &__u.sigma;
-	fused_engine_process_position_2d_event(pos, sigma);
-
-	location_position_free(pos);
-	location_accuracy_free(accuracy);
+//	LOG_FUSED_FUNC;
+
+	if (!fused.status.started)
+		return;
+
+	fused.last_wps_timestamp = timestamp;
+
+	bool is_new_position = fusion_engine_position_2d_event(&fused.fusion_engine,
+						(double)timestamp, latitude, longitude, hor_accuracy);
+
+	if (is_new_position)
+		on_engine_new_position();
 }
 
-static void on_engine_position(const fl_position_4d* position, gpointer user_data)
+static void on_engine_new_position()
 {
 //	LOG_FUSED_FUNC;
 
-	fused_engine_get_location(&fused.last_location.pos, &fused.last_location.vel, &fused.last_location.sigma);
+	time_t now = time(NULL);
+	double seconds_from_last_gps = difftime(now, fused.last_gps_timestamp);
+	double seconds_from_last_wps = difftime(now, fused.last_wps_timestamp);
+	double seconds_from_last_position = fmin(seconds_from_last_gps, seconds_from_last_wps);
+	LOG_FUSED(LOG_DEBUG, "max time %.0f/%.0f[s]", seconds_from_last_position, FL_MAX_TIME_FROM_LAST_POS);
+	if (seconds_from_last_position > FL_MAX_TIME_FROM_LAST_POS) {
+		LOG_FUSED(LOG_DEBUG, "max time %.0f/%.0f[s] have passed -> skip output location",
+				seconds_from_last_position, FL_MAX_TIME_FROM_LAST_POS);
+		return;
+	}
 
+	fusion_engine_get_wgs_location(&fused.fusion_engine, &fused.last_location);
 	if (fused.fused_pos_cb)
-		fused.fused_pos_cb(&fused.last_location, user_data);
+		fused.fused_pos_cb((gint)fused.last_location.time,
+				(gdouble)fused.last_location.latitude,
+				(gdouble)fused.last_location.longitude,
+				(gdouble)fused.last_location.altitude,
+				(gdouble)fused.last_location.speed,
+				(gdouble)fused.last_location.direction,
+				(gdouble)fused.last_location.climb,
+				(gdouble)fused.last_location.hor_dev,
+				(gdouble)fused.last_location.ver_dev,
+				fused.lbs_server_handle);
 	else
 		LOG_FUSED(LOG_ERROR, "fused_pos_cb == NULL");
 }
 
 static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id)
 {
+//	LOG_FUSED_FUNC;
+	fl_sensor_source sensor_type = PTR2ENUM(sensor_id, fl_sensor_source);
 	if (event) {
-		fused_engine_process_sensory_event((fl_seconds)((double)(long long)event->timestamp / 1000000.0),
-				PTR2ENUM(sensor_id, fl_sensory_source), event->values);
-	} else {
-		LOG_FUSED(LOG_ERROR, "(handle=%p, event=%p, sensor_id=%d): UNSUPPORTED",
-				handle, event, PTR2ENUM(sensor_id, fl_sensory_source));
-	}
+		bool is_new_position = false;
+		switch (sensor_type) {
+		case ACCELEROMETER:
+			is_new_position = fusion_engine_acc_event(&fused.fusion_engine,
+					(double)(long long)event->timestamp / 1000000.0,
+					event->values[DEV_SPATIAL_X],
+					event->values[DEV_SPATIAL_Y],
+					event->values[DEV_SPATIAL_Z]);
+			break;
+		case GYROSCOPE:
+			is_new_position = fusion_engine_gyro_event(&fused.fusion_engine,
+					(double)(long long)event->timestamp / 1000000.0,
+					conversions_degrees_to_radians(event->values[DEV_SPATIAL_X]),
+					conversions_degrees_to_radians(event->values[DEV_SPATIAL_Y]),
+					conversions_degrees_to_radians(event->values[DEV_SPATIAL_Z]));
+			break;
+		case PEDOMETER:
+			LOG_FUSED(LOG_DEBUG, "[PEDOMETER] timestamp[%llu] accuracy[%d] Step[%.0f] Step[%.0f] [%.1f] Dist[%.3f m] [%.1f] Speed[%.5f m/s] [%.1f] [%.1f]",
+			event->timestamp, event->accuracy,
+			event->values[0], event->values[1], event->values[2], event->values[3],
+			event->values[4], event->values[5], event->values[6], event->values[7]);
+
+			if (fused.last_gps_timestamp > 0 && fused.last_gps_speed > 0) {
+				double seconds_from_last_gps = (double)(long long)event->timestamp / 1000000.0
+						+ fused.utc_offset_sec - (double)fused.last_gps_timestamp;
+				if (seconds_from_last_gps < FL_MAX_PEDOMETER_SKIP_TIME && fused.last_gps_speed >= FL_MIN_PEDOMETER_SKIP_SPEED) {
+					LOG_FUSED(LOG_DEBUG, "seconds_from_last_gps = %.1f/%.1f[s] && last_gps_speed = %.2f/%.1f[km/h] -> PEDOMETER SKIP",
+						seconds_from_last_gps, FL_MAX_PEDOMETER_SKIP_TIME, fused.last_gps_speed, FL_MIN_PEDOMETER_SKIP_SPEED);
+					break;
+				}
+			}
+
+			float speed = event->values[5];
+			if (fabs(speed) >= 0.001)
+				is_new_position = fusion_engine_speed_event(&fused.fusion_engine,
+					(double)(long long)event->timestamp / 1000000.0, event->values[5], 1.0);
+			break;
+		default:
+			LOG_FUSED(LOG_ERROR, "unsupported sensor type[%d]", sensor_type);
+			break;
+		}
+		if (is_new_position)
+			on_engine_new_position();
+	} else
+		LOG_FUSED(LOG_ERROR, "event == NULL");
 }
 
 #if (FL_MOTION_DETECTOR)
@@ -305,28 +379,23 @@ static void location_fused_pause()
 {
 	LOG_FUSED_FUNC;
 	if (fused.status.paused == FALSE) {
-		sensor_func_stop(fused.started_sensors & ~ACCELEROMETER_FLAG);
+		sensor_stop(GYROSCOPE);
+		sensor_stop(PEDOMETER);
 	}
+	fused.status.paused = TRUE;
 }
 
-static LocationError location_fused_resume()
+static void location_fused_resume()
 {
 	LOG_FUSED_FUNC;
-	LocationError ret;
-
 	if (fused.status.paused) {
-		ret = sensor_func_start(GYROSCOPE_FLAG);
-		if (ret == LOCATION_ERROR_NONE)
-			sensor_func_stop(GYROSCOPE_FLAG);
-	} else {
-		ret = LOCATION_ERROR_NOT_AVAILABLE;
+		sensor_start(GYROSCOPE);
+		sensor_start(PEDOMETER);
 	}
-
-	LOG_FUSED(LOG_DEBUG, "0x%X", ret);
-	return ret;
+	fused.status.paused = FALSE;
 }
 
-static void on_motion_state(fl_motion_state state, gpointer data)
+static void on_motion_state(MotionDetectorState state)
 {
 	LOG_FUSED(LOG_DEBUG, "(state=%u)", state);
 	if (fused.status.started) {
diff --git a/lbs-server/src/fused.h b/lbs-server/src/fused.h
index b34a7e1..df7dcf5 100644
--- a/lbs-server/src/fused.h
+++ b/lbs-server/src/fused.h
@@ -23,159 +23,94 @@
 #ifndef __LOCATION_FUSED_H__
 #define __LOCATION_FUSED_H__
 
-#include "gps_plugin_data_types.h"
-#include "fused/types.h"
-#include "lbs_dbus_server.h"
+#if (FL_MOTION_DETECTOR)
+#include "fused/MotionDetector.h"
+#endif
 #include <glib.h>
 
-/***************************************************************************//**
- * This callback is called with position data.
- *
- * @param[in]	location	Location data
- * @param[in]	user_data	User defined data
- */
-typedef void (*fused_updated_cb)(fl_location *location, gpointer user_data);
+/** Labels for standard sensors */
+typedef enum {
+	ACCELEROMETER = 0,
+	GYROSCOPE,
+	PEDOMETER,
+	SENSOR_SOURCE_COUNT
+} fl_sensor_source;
 
-/***************************************************************************//**
- * TODO: TBD
+/**
+ * @brief This callback is called with position data.
  */
-void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server);
+typedef void (*fused_updated_cb)(gint timestamp, gdouble latitude, gdouble longitude, gdouble altitude,
+		gdouble speed, gdouble direction, gdouble climb, gdouble hor_accuracy, gdouble ver_accuracy, gpointer user_data);
 
-/***************************************************************************//**
- * TODO: TBD
- */
+void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server);
 void location_fused_deinit();
-
-/***************************************************************************//**
- * TODO: TBD
- */
 void location_fused_start();
-
-/***************************************************************************//**
- * TODO: TBD
- */
 void location_fused_stop();
 
-/***************************************************************************//**
- * [callback] Pass the GPS location data to the engine for processing.
- *
- * @param[in]	timestamp		timestamp
- * @param[in]	latitude		latitude
- * @param[in]	longitude		longitude
- * @param[in]	altitude		altitude
- * @param[in]	speed			speed
- * @param[in]	bearing			bearing
- * @param[in]	hor_accuracy	hor_accuracy
- * @param[in]	ver_accuracy	ver_accuracy
+/**
+ * @brief [callback] Pass the GPS location data to the engine for processing.
+ * @param[in] timestamp		timestamp
+ * @param[in] latitude		latitude
+ * @param[in] longitude		longitude
+ * @param[in] altitude		altitude
+ * @param[in] speed			speed
+ * @param[in] bearing		bearing
+ * @param[in] hor_accuracy	hor_accuracy
+ * @param[in] ver_accuracy	ver_accuracy
  */
 void send_gps_position_to_fused_engine(time_t timestamp, double latitude, double longitude,
 		double altitude, double speed, double bearing, double hor_accuracy,	double ver_accuracy);
 
-/***************************************************************************//**
- * [callback] Pass the WPS location data to the engine for processing.
- *
- * @param[in]	timestamp		timestamp
- * @param[in]	latitude		latitude
- * @param[in]	longitude		longitude
- * @param[in]	hor_accuracy	hor_accuracy
- * @param[in]	ver_accuracy	ver_accuracy
+/**
+ * @brief [callback] Pass the WPS location data to the engine for processing.
+ * @param[in] timestamp		timestamp
+ * @param[in] latitude		latitude
+ * @param[in] longitude		longitude
+ * @param[in] hor_accuracy	hor_accuracy
+ * @param[in] ver_accuracy	ver_accuracy
  */
-void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude,
-		double hor_accuracy, double ver_accuracy);
+void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy);
 
 #if defined(__LOCATION_FUSED_C__)
 
-/***************************************************************************//**
- * [private] Detect sensors used for dead-reckoning and power-saving.
- *
- * @return
- *      Conjunction of the detected and used sensors as bit-set.
- */
-static fl_sensory_flags sensor_func_detect();
+static void sensors_create();
 
-/***************************************************************************//**
- * [private] Detect and attach sensors used for dead-reckoning and power-saving.
- */
-static void sensor_func_create();
+const char* sensor_error_to_str(int error);
 
-/***************************************************************************//**
- * [private] Detach used sensors.
- */
-static void sensor_func_destroy();
+static void sensors_clear();
 
-/***************************************************************************//**
- * [private] Start selected subset of sensors.
- *
- * @param[in] sensors
- *      Bit-set indicator of the sensors to be started.
- *
- * @return
- *      - LOCATION_ERROR_NONE (zero) upon success;
- *      - LOCATION_ERROR_NOT_SUPPORTED if the sensors are not attached;
- *      - LOCATION_ERROR_CONFIGURATION if the sensors could not be started.
- */
-static LocationError sensor_func_start(fl_sensory_flags sensors);
+static void sensors_reset_listeners();
 
-/***************************************************************************//**
- * [private] Stop selected subset of sensors.
- *
- * @param[in] sensors
- *      Bit-set indicator of the sensors to be stopped.
- */
-static void sensor_func_stop(fl_sensory_flags sensors);
+static void sensors_destroy();
 
-/***************************************************************************//**
- * [private, callback] Pass the sensory data to the engine.
- *
- * @param[in] handle
- *      Sensor handle
- * @param[in] event
- *      VConf key name to watch for changes
- * @param[in] sensor_id
- *		TODO: TBD
- */
-static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id);
+static void sensor_start(fl_sensor_source sensor);
 
-/***************************************************************************//**
- * [private, callback] Invoked upon change of position reported by the engine.
- * There is no distinction between predicted and fused one; it can be either of
- * the two. Upon checking user-supplied conditions the notification is issued.
- *
- * @param[in] position
- *      New spatio-temporal (4D) position.
- */
-static void on_engine_position(const fl_position_4d *position, gpointer user_data);
+static void sensor_stop(fl_sensor_source sensor);
 
-#if (FL_MOTION_DETECTOR)
+static void on_engine_new_position();
 
-/***************************************************************************//**
- * [private] Enters power-saving mode by unsubscribing from location source, and
- * and unused sensor(s)).
+/**
+ * @brief Pass the sensory data to the engine.
+ * @param[in] handle  sensor handle
+ * @param[in] event  vconf key name to watch for changes
+ * @param[in] sensor_id  sensor id
  */
+static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id);
+
+#if (FL_MOTION_DETECTOR)
+/** Enters power-saving mode by unsubscribing from location source, and and unused sensor(s))  */
 static void location_fused_pause();
 
-/***************************************************************************//**
- * [private] Resumes normal-power operation mode by subscribing to location
- * source, and and used sensor(s).
- *
- * @return
- *      - LOCATION_ERROR_NONE (zero) upon success;
- *      - error code otherwise.
- */
-static LocationError location_fused_resume();
+/** Resumes normal-power operation mode by subscribing to location source, and and used sensor(s) */
+static void location_fused_resume();
 
-/***************************************************************************//**
- * [private, callback] Receive notifications about motion state changes, and
- * pause/resume accordingly.
- *
- * @param[in] state
- *      New motion state
+/**
+ * @brief Receive notifications about motion state changes, and pause/resume accordingly.
+ * @param[in] state  New motion state
  */
-static void on_motion_state(fl_motion_state state, gpointer data);
+static void on_motion_state(MotionDetectorState state);
 #else /* (FL_MOTION_DETECTOR) */
-/***************************************************************************//**
- * [private] Compilation stub.
- */
+/** Compilation stub */
 	#define on_motion_state NULL
 #endif /* (FL_MOTION_DETECTOR) */
 
diff --git a/lbs-server/src/fused/AccelerometerFilter.c b/lbs-server/src/fused/AccelerometerFilter.c
new file mode 100644
index 0000000..eb2b7df
--- /dev/null
+++ b/lbs-server/src/fused/AccelerometerFilter.c
@@ -0,0 +1,105 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "AccelerometerFilter.h"
+#include "Conversions.h"
+#include "math-ext.h"
+#include "debug_util.h"
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+#define DEFAULT_SAMPLING_FREQUENCY   10.0 /* [Hz] accelerometer sampling frequency */
+#define DEFAULT_ACCELERATION_SIGMA2  SQUARE(FL_ACCEL_NOISE_LEVEL)  /* [(m/s^2)^2] */
+
+static const Vector_3d av = {0, 0, EARTH_GRAVITY};
+
+void accelerometer_filter_init(AccelerometerFilter* self,
+		MotionDetectorStateChangeListener on_motion_state_changed)
+{
+	LOG_FUSED_FUNC;
+	frequency_estimator_init(&self->frequency_estimator, DEFAULT_SAMPLING_FREQUENCY);
+	butterworth_3d_filter_init(&self->lp_3d_filter, FL_LP3D_CUTOFF_FREQUENCY);
+	gravity_estimator_init(&self->gravity_estimator);
+	motion_detector_init(&self->motion_detector, on_motion_state_changed);
+
+	butterworth_3d_filter_reset_to(&self->lp_3d_filter, av);
+	butterworth_3d_filter_set_sampling_frequency(&self->lp_3d_filter, DEFAULT_SAMPLING_FREQUENCY);
+
+	LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
+}
+
+void accelerometer_filter_exit(AccelerometerFilter* self)
+{
+	LOG_FUSED_FUNC;
+	motion_detector_exit(&self->motion_detector);
+	gravity_estimator_exit(&self->gravity_estimator);
+	butterworth_3d_filter_exit(&self->lp_3d_filter);
+
+	LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
+}
+
+void accelerometer_filter_reset(AccelerometerFilter* self, const_Vector_3d_ref av)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+	frequency_estimator_init(&self->frequency_estimator, DEFAULT_SAMPLING_FREQUENCY);
+	motion_detector_reset(&self->motion_detector);
+	butterworth_3d_filter_reset_to(&self->lp_3d_filter, av);
+}
+
+void accelerometer_filter_process(AccelerometerFilter* self,
+		double time, const_Vector_3d_ref acc, Vector_3d_ref gu, double *w2gu)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.2f, acc=(% 5.2f, % 5.2f, % 5.2f))"), time, acc[X], acc[Y], acc[Z]);
+
+	if (frequency_estimator_process(&self->frequency_estimator, time))
+		butterworth_3d_filter_set_sampling_frequency(&self->lp_3d_filter, frequency_estimator_get_frequency(&self->frequency_estimator));
+
+	const_Vector_3d_ref af  = butterworth_3d_filter_process(&self->lp_3d_filter, acc);
+
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, af=(%.16g, %.16g, %.16g)"), time, af[0], af[1], af[2]);
+
+	/* the z-value is passed through 2nd order Butterworth, then 1st order EMA */
+	const double af_len = vector_3d_length(af);
+	const double overlap = vector_3d_dot_product(af, acc);
+	const double g2 = gravity_estimator_process(&self->gravity_estimator, overlap);
+
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=%.2f, |af|=%5.2f, <g>=%5.2f"), time, af_len, g2);
+
+	if (af_len > FL_ACCEL_NOISE_LEVEL && g2 > 1) {
+		const double norm_out = 1.0 / af_len;
+		vector_3d_mul_number(af, norm_out, gu);
+		*w2gu = self->gravity_estimator.base.decay_rate / (fl_square(gu[X]) + fl_square(gu[Y]) + fl_square(gu[Z] - 1) + GEO_DIMENSION * DEFAULT_ACCELERATION_SIGMA2);
+
+		LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, decay=%.16g, gu=(%.16g, %.16g, %.16g), af_len=%.16g, overlap=%.16g, g2=%.16g"),
+				time, self->gravity_estimator.base.decay_rate, gu[X], gu[Y], gu[Z], af_len, overlap, g2);
+	} else {
+		vector_3d_set(gu, 0, 0, 1); /* default */
+		*w2gu = self->gravity_estimator.base.decay_rate / (GEO_DIMENSION * DEFAULT_ACCELERATION_SIGMA2);
+
+		LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, decay=%.16g, gu=(%.16g, %.16g, %.16g), af_len=%.16g, overlap=%.16g, g2=%.16g"),
+				time, self->gravity_estimator.base.decay_rate, gu[X], gu[Y], gu[Z], af_len, overlap, g2);
+	}
+
+	Vector_3d al; /* Linear acceleration in global frame. */
+	const double g = sqrt(g2);
+	if (g > 1)
+		vector_3d_linear_combination(EARTH_GRAVITY / g, acc, -EARTH_GRAVITY, gu, al);
+	else
+		vector_3d_copy(al, acc);
+
+	motion_detector_process(&self->motion_detector, time, al);
+}
diff --git a/lbs-server/src/fused/AccelerometerFilter.h b/lbs-server/src/fused/AccelerometerFilter.h
new file mode 100644
index 0000000..81970aa
--- /dev/null
+++ b/lbs-server/src/fused/AccelerometerFilter.h
@@ -0,0 +1,81 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    AccelerometerFilter.h
+ * @brief   Accelerometer calibration (scale and sampling frequency), and
+ *          low-pass 3D filtering.
+ */
+
+#pragma once
+#ifndef __ACCELEROMETER_FILTER_H__
+#define __ACCELEROMETER_FILTER_H__
+
+#include "FrequencyEstimator.h"
+#include "Butterworth3dFilter.h"
+#include "MotionDetector.h"
+#include "GravityEstimator.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Accelerometer calibration (scale and sampling frequency), and low-pass 3D filtering */
+typedef struct {
+	FrequencyEstimator  frequency_estimator;
+	Butterworth3dFilter lp_3d_filter;
+	GravityEstimator    gravity_estimator;
+	MotionDetector      motion_detector;
+} AccelerometerFilter;
+
+/**
+ * @brief Initialization of the AccelerometerFilter object (constructor).
+ * @param[in] on_motion_state_changed  Callback to be invoked when the detected state of motion changes.
+ */
+void accelerometer_filter_init(AccelerometerFilter* self, MotionDetectorStateChangeListener on_motion_state_changed);
+
+void accelerometer_filter_exit(AccelerometerFilter* self);
+
+/**
+ * @brief Reset of the internal state back to initial one.
+ *        This function is used for repetitive testing and module soft restarts.
+ * @param[in] av  Stationary acceleration state to be set (typically {0, 0, g}).
+ */
+void accelerometer_filter_reset(AccelerometerFilter* self, const_Vector_3d_ref av);
+
+/**
+ * @brief Processing of a single sample from the accelerometer. This function performs:
+ * - estimation of the sampling frequency,
+ * - estimation of the gravity direction via low-pass 3D filter  (cf. LP3D unit),
+ * - estimation of the gravity value (cf. GRES unit) and scale correction,
+ * - estimation of the linear acceleration i.e. with subtracted gravity component,
+ * - motion state detection (cf. MOTI unit)
+ * @param[in] time       Time of the event in seconds.
+ * @param[in] acc        Measured 3D acceleration vector in global frame.
+ * @param[out] gu        Unit vector along gravity direction oriented upwards.
+ * @param[out] w2gu      Estimated inverse covariance (squared weight) of a @gu.
+ */
+void accelerometer_filter_process(AccelerometerFilter* self,
+		double time, const_Vector_3d_ref acc, Vector_3d_ref gu, double *w2gu);
+
+/** @return double */
+#define accelerometer_filter_get_update_rate(self) gravity_estimator_get_update_rate(&(self)->gravity_estimator)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __ACCELEROMETER_FILTER_H__ */
diff --git a/lbs-server/src/fused/Butterworth3dFilter.c b/lbs-server/src/fused/Butterworth3dFilter.c
new file mode 100644
index 0000000..0ccf05e
--- /dev/null
+++ b/lbs-server/src/fused/Butterworth3dFilter.c
@@ -0,0 +1,104 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#if !defined(LOG_THRESHOLD)
+	/* custom logging threshold - keep undefined on the repo */
+	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
+#endif
+
+#include "Butterworth3dFilter.h"
+#include "math-ext.h"
+#include "debug_util.h"
+
+#define LP3D_FORMAT     "% 6.2f"
+
+void butterworth_3d_filter_init(Butterworth3dFilter* self, double f)
+{
+	LOG_FUSED_DEV(DBG_LOW, INDENT("()"));
+	self->cut_off_frequency = f;
+	self->lp3d_G  =  0;
+	self->lp3d_B1 = -2;
+	self->lp3d_B2 =  1;
+	butterworth_3d_filter_reset(self);
+	LOG_FUSED_DEV(DBG_LOW, UNINDENT("(): OK"));
+}
+
+void butterworth_3d_filter_exit(Butterworth3dFilter* self)\
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+}
+
+void butterworth_3d_filter_set_sampling_frequency(Butterworth3dFilter* self, double f)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("frequency() f=%.16e, cut_off=%.16e"), f, self->cut_off_frequency);
+
+	const double omegaC  = tan(M_PI * self->cut_off_frequency / f);
+	const double omegaC2 = fl_square(omegaC);
+	const double _B0  = 1.0 / (1 + 2 * omegaC * cos(M_PI_4) + omegaC2);
+	self->lp3d_B1 = _B0 * 2*(omegaC2 - 1);
+	self->lp3d_B2 = _B0 * (1 - 2 * omegaC * cos(M_PI_4) + omegaC2);
+	self->lp3d_G  = _B0 * omegaC2;
+
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%g Hz): OK"), f);
+}
+
+void butterworth_3d_filter_reset(Butterworth3dFilter* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+	vector_3d_clear_array(self->lp3d_u);
+	vector_3d_clear_array(self->lp3d_v);
+}
+
+void butterworth_3d_filter_reset_to(Butterworth3dFilter* self, const_Vector_3d_ref stationary)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_FORMAT ", " LP3D_FORMAT ", " LP3D_FORMAT "})"), stationary[0], stationary[1], stationary[2]);
+	unsigned i = GEO_DIMENSION;
+	do {
+		--i;
+		/* up-conversion */
+		self->lp3d_u[TIME_SHIFT_1][i] = stationary[i];
+		self->lp3d_u[TIME_SHIFT_2][i] = stationary[i];
+		self->lp3d_v[TIME_SHIFT_1][i] = stationary[i];
+		self->lp3d_v[TIME_SHIFT_2][i] = stationary[i];
+	} while (i);
+}
+
+const_Vector_3d_ref butterworth_3d_filter_process(Butterworth3dFilter* self, const_Vector_3d_ref u)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_FORMAT ", " LP3D_FORMAT ", " LP3D_FORMAT "})"), u[0], u[1], u[2]);
+	unsigned i = GEO_DIMENSION;
+	do {
+		--i;
+#define u1 self->lp3d_u[TIME_SHIFT_1][i]
+#define u2 self->lp3d_u[TIME_SHIFT_2][i]
+#define v1 self->lp3d_v[TIME_SHIFT_1][i]
+#define v2 self->lp3d_v[TIME_SHIFT_2][i]
+		const double u0 = u[i];
+		const double v0 = self->lp3d_G * (u0 + 2 * u1 + u2) - self->lp3d_B1 * v1 - self->lp3d_B2 * v2;
+		LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("process() i=%i, lp3d_G=%.16e, lp3d_B1=%.16e, u0=%.16e, u1=%.16e, u2=%.16e, v0=%.16e, v1=%.16e, v2=%.16e"),
+				i, self->lp3d_G, self->lp3d_B1, u0, u1, u2, v0, v1, v2);
+
+		u2 = u1;
+		u1 = u0;
+		v2 = v1;
+		v1 = v0;
+#undef u1
+#undef u2
+#undef v1
+#undef v2
+	} while (i);
+	return self->lp3d_v[TIME_SHIFT_1];
+}
diff --git a/lbs-server/src/fused/Butterworth3dFilter.h b/lbs-server/src/fused/Butterworth3dFilter.h
new file mode 100644
index 0000000..993dab9
--- /dev/null
+++ b/lbs-server/src/fused/Butterworth3dFilter.h
@@ -0,0 +1,80 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    Butterworth3dFilter.h
+ * @brief   Low-pass 3D 2nd order Butterworth filter.
+ */
+
+#pragma once
+#ifndef __BUTTERWORTH_3D_FILTER_H__
+#define __BUTTERWORTH_3D_FILTER_H__
+
+#include "Vector.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef enum {
+	TIME_SHIFT_1,	/* t - 1 */
+	TIME_SHIFT_2,	/* t - 2 */
+	TIME_SHIFT_COUNT
+} TimeShift;
+
+typedef struct {
+	/* Filter coefficients. */
+	double cut_off_frequency;
+	double lp3d_G;  /* omega^2/B_0 */
+	double lp3d_B1; /*     B_1/B_0 */
+	double lp3d_B2; /*     B_2/B_0 */
+
+	/* Memory of past states. */
+	Vector_3d lp3d_u[TIME_SHIFT_COUNT];
+	Vector_3d lp3d_v[TIME_SHIFT_COUNT];
+} Butterworth3dFilter;
+
+void butterworth_3d_filter_init(Butterworth3dFilter* self, double f);
+
+void butterworth_3d_filter_exit(Butterworth3dFilter* self);
+
+void butterworth_3d_filter_reset(Butterworth3dFilter* self);
+
+/**
+ * @brief Adjust internal parameters to the given sampling frequency and reset the the past state memory.
+ * @param[in] f  The sampling frequency in [Hz].
+ */
+void butterworth_3d_filter_set_sampling_frequency(Butterworth3dFilter* self, double f);
+
+/**
+ * @brief Reset to the given stationary state (input = output).
+ * @param[in] stationary  3D vector of acceleration measured in tangent coordinate frame.
+ */
+void butterworth_3d_filter_reset_to(Butterworth3dFilter* self, const_Vector_3d_ref stationary);
+
+/**
+ * @brief Process single input sample through the 3D Butterworth filter.
+ * @param[in] u  3D acceleration input vector.
+ * @return       3D acceleration output vector.
+ */
+const_Vector_3d_ref butterworth_3d_filter_process(Butterworth3dFilter* self, const_Vector_3d_ref u);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BUTTERWORTH_3D_FILTER_H__ */
diff --git a/lbs-server/src/fused/CalibrationFilter.c b/lbs-server/src/fused/CalibrationFilter.c
new file mode 100644
index 0000000..b3bdbd3
--- /dev/null
+++ b/lbs-server/src/fused/CalibrationFilter.c
@@ -0,0 +1,165 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "CalibrationFilter.h"
+#include "Conversions.h"
+#include "math-ext.h"
+#include <stdio.h>
+
+static Vector_3d zero = { 0, 0, 0 };
+
+/** Minimum measurements in stable state to calculate new calibration parameters. */
+static const int MIN_MEAS = 100;
+
+static const double MAX_PRECISION = 100000;
+
+/** Time of decorrelation in [s], currently 100s. */
+static const double DT = 100;
+
+/** Days duration in [s], currently 24h. */
+static const double DAY = 24L * 60L * 60L;
+
+/** Length of buffer for calibration parameters. */
+static const int PARAMS_LEN = 100;
+
+/** Format of calibration parameters. */
+static const int PARAMS_VERSION = 1;
+
+void calibration_filter_init(CalibrationFilter* self, double utcOffset)
+{
+	self->utc_offset = utcOffset;
+	self->autocalibration = true;
+	calibration_filter_clear_calibration(self);
+	calibration_filter_reset(self);
+}
+
+void calibration_filter_exit(CalibrationFilter* self)
+{
+}
+
+void calibration_filter_reset(CalibrationFilter* self)
+{
+	self->calibrating = false;
+	self->calibration_time = FL_UNDEFINED_TIME;
+}
+
+Vector_3d_ref calibration_filter_filter(CalibrationFilter* self, double time,
+		const_Vector_3d_ref data, bool is_stable)
+{
+	if (self->autocalibration) {
+		unsigned i;
+		if (is_stable) {
+			if (!self->calibrating) {
+				self->cal_nr = 0;
+				vector_3d_clear(self->sum);
+				vector_3d_clear(self->sum2);
+				self->calibrating = true;
+			}
+			self->cal_nr++;
+			for (i = GEO_DIMENSION; i;) {
+				i--;
+				self->sum[i] += data[i];
+				self->sum2[i] += fl_square(data[i]);
+			}
+			return zero;
+
+		} else {
+
+			if (self->calibrating) {
+				self->calibrating = false;
+
+				if (self->cal_nr > MIN_MEAS) {
+					Vector_3d mean;
+					Vector_3d std_dev;
+					for (i = GEO_DIMENSION; i;) {
+						i--;
+						double m = self->sum[i] / self->cal_nr;
+						mean[i] = m;
+						double variance = self->sum2[i] / self->cal_nr - m * m;
+						std_dev[i] = variance > 0 ? sqrt(variance) : 0;
+					}
+					double dt = (!KNOWN_TIME(self->calibration_time)) || time < self->calibration_time
+							? DAY : time - self->calibration_time;
+
+					const double curPrec = self->cal_prec * exp(-dt / DT);
+					double newPrec = self->cal_nr / vector_3d_length2(std_dev);
+					if (newPrec > MAX_PRECISION) {
+						newPrec = MAX_PRECISION;
+					}
+					const double alpha = curPrec / (newPrec + curPrec);
+					const double alpha1 = 1. - alpha;
+
+					for (i = GEO_DIMENSION; i;) {
+						i--;
+						self->bias[i] = self->bias[i] * alpha + mean[i] * alpha1;
+					}
+					self->cal_prec = newPrec + curPrec;
+					self->calibration_time = time;
+				}
+			}
+		}
+	}
+	vector_3d_sub_vector_3d(data, self->bias, self->output);
+	return self->output;
+}
+
+void calibration_filter_set_enable_autocalibration(CalibrationFilter* self,
+		bool enable)
+{
+	self->autocalibration = enable;
+}
+
+void calibration_filter_clear_calibration(CalibrationFilter* self)
+{
+	self->calibration_time = FL_UNDEFINED_TIME;
+	self->cal_prec = 0;
+	vector_3d_clear(self->bias);
+}
+
+char* calibration_filter_get_calibration(CalibrationFilter* self)
+{
+	char* params = (char*) calloc(PARAMS_LEN, sizeof(char));
+	if (params != NULL) {
+		snprintf(params, PARAMS_LEN, "%i,%.9f,%.10f,%.10f,%.10f,%.10f",
+				PARAMS_VERSION,
+				self->calibration_time == FL_UNDEFINED_TIME ? 0 :
+						self->calibration_time + self->utc_offset,
+				self->cal_prec, self->bias[0], self->bias[1], self->bias[2]);
+	}
+	return params;
+}
+
+void calibration_filter_set_calibration(CalibrationFilter* self, const char* params)
+{
+	if (params != NULL) {
+		int version;
+		double cal_time;
+		double cal_prec;
+		double bias0;
+		double bias1;
+		double bias2;
+		if (sscanf(params, "%i,%lf,%lf,%lf,%lf,%lf",
+				&version, &cal_time, &cal_prec, &bias0, &bias1, &bias2) == 6) {
+			if (version == PARAMS_VERSION) {
+				self->calibration_time = cal_time == 0 ? FL_UNDEFINED_TIME : cal_time - self->utc_offset;
+				self->cal_prec = cal_prec;
+				self->bias[0] = bias0;
+				self->bias[1] = bias1;
+				self->bias[2] = bias2;
+			}
+		}
+	}
+}
diff --git a/lbs-server/src/fused/CalibrationFilter.h b/lbs-server/src/fused/CalibrationFilter.h
new file mode 100644
index 0000000..d3f04f4
--- /dev/null
+++ b/lbs-server/src/fused/CalibrationFilter.h
@@ -0,0 +1,50 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#ifndef __CALIBRATIONFILTER_H_
+#define __CALIBRATIONFILTER_H_
+
+#include "Vector.h"
+
+#include <stdbool.h>
+
+
+typedef struct {
+	double utc_offset;		/* Difference between UTC time and monotonic time in [s]. */
+	double calibration_time;
+	Vector_3d bias;
+	Vector_3d output;
+	bool autocalibration;
+	bool calibrating;
+	double cal_prec;
+	long cal_nr;
+	Vector_3d sum;
+	Vector_3d sum2;
+} CalibrationFilter;
+
+extern void calibration_filter_init(CalibrationFilter* self, double utcOffset);
+extern void calibration_filter_exit(CalibrationFilter* self);
+extern void calibration_filter_reset(CalibrationFilter* self);
+
+extern Vector_3d_ref calibration_filter_filter(CalibrationFilter* self, double time, const_Vector_3d_ref data, bool is_stable);
+
+extern void calibration_filter_set_enable_autocalibration(CalibrationFilter* self, bool enable);
+
+extern void calibration_filter_clear_calibration(CalibrationFilter* self);
+extern char* calibration_filter_get_calibration(CalibrationFilter* self);
+extern void calibration_filter_set_calibration(CalibrationFilter* self, const char* params);
+
+#endif /* __CALIBRATIONFILTER_H_ */
diff --git a/lbs-server/src/fused/conversions.c b/lbs-server/src/fused/Conversions.c
index 048177d..affa501 100644
--- a/lbs-server/src/fused/conversions.c
+++ b/lbs-server/src/fused/Conversions.c
@@ -14,34 +14,36 @@
  *  limitations under the License.
  */
 
-#define __FL_CONVERSIONS_C__
+#include "Conversions.h"
 
-#include "conversions.h"
-
-fl_degrees fl_radians_to_positive_degrees(fl_radians radians)
+double conversions_radians_to_positive_degrees(double radians)
 {
-	fl_degrees d;
-
-	d = (180 / M_PI) * radians;
-	/* When the value only ocassionally enters adjacent branches this is more
+	double d = (180 / M_PI) * radians;
+	/* When the value only occasionally enters adjacent branches this is more
 	   efficient a way than division modulo. */
 	if (d < 0)
-		do { d += 360; } while (d < 0);
+		do {
+			d += 360;
+		} while (d < 0);
 	else if (d >= 360)
-		do { d -= 360; } while (d >= 360);
+		do {
+			d -= 360;
+		} while (d >= 360);
 	return d;
 }
 
-fl_degrees fl_radians_to_balanced_degrees(fl_radians radians)
+double conversions_radians_to_balanced_degrees(double radians)
 {
-	fl_degrees d;
-
-	d = (180 / M_PI) * radians;
+	double d = (180 / M_PI) * radians;
 	/* When the value only ocassionally enters adjacent branches this is more
 	   efficient a way than division modulo. */
 	if (d < -180)
-		do { d += 360; } while (d < -180);
+		do {
+			d += 360;
+		} while (d < -180);
 	else if (d >= 180)
-		do { d -= 360; } while (d >= 180);
+		do {
+			d -= 360;
+		} while (d >= 180);
 	return d;
 }
diff --git a/lbs-server/src/fused/Conversions.h b/lbs-server/src/fused/Conversions.h
new file mode 100644
index 0000000..42aaa66
--- /dev/null
+++ b/lbs-server/src/fused/Conversions.h
@@ -0,0 +1,73 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    Conversions.h
+ * @brief   Units conversion functions
+ */
+
+#pragma once
+#ifndef __CONVERSIONS_H__
+#define __CONVERSIONS_H__
+
+#include "math-ext.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define FL_UNDEFINED_TIME -1e+48
+#define KNOWN_TIME(t) (t > FL_UNDEFINED_TIME)
+
+/**
+ * @brief Convert degrees to radians.
+ * @param[in] degrees Value in degrees.
+ * @return Corresponding value in radians.
+ */
+#define conversions_degrees_to_radians(degrees) ((M_PI / 180) * (degrees))
+
+/**
+ * @brief Convert radians to degrees.
+ * @param[in] radians Value in radians.
+ * @return Corresponding value in degrees.
+ */
+#define conversions_radians_to_degrees(radians) ((180 / M_PI) * (radians))
+
+/**
+ * @brief Convert radians to degrees in the [0,360) range
+ * @param[in] radians Arbitrary value in radians.
+ * @return Value in degrees projected onto the [0,360) branch.
+ */
+double conversions_radians_to_positive_degrees(double radians);
+
+/**
+ * @brief Convert radians to degrees in the [-180,180) range.
+ * @param[in] radians Arbitrary value in radians.
+ * @return Value in degrees projected onto the [-180,180) branch.
+ */
+double conversions_radians_to_balanced_degrees(double radians);
+
+/** Convert radians to latitude degrees [-90,90] */
+#define conversions_radians_to_latitude	conversions_radians_to_balanced_degrees
+
+/** Convert radians to longitude degrees [0, 360) */
+#define conversions_radians_to_longitude conversions_radians_to_positive_degrees
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CONVERSIONS_H_ */
diff --git a/lbs-server/src/fused/FrequencyEstimator.c b/lbs-server/src/fused/FrequencyEstimator.c
new file mode 100644
index 0000000..863de85
--- /dev/null
+++ b/lbs-server/src/fused/FrequencyEstimator.c
@@ -0,0 +1,60 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "FrequencyEstimator.h"
+#include "Conversions.h"
+#include "debug_util.h"
+
+#define FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD (1.0 * 1.0)
+
+void frequency_estimator_init(FrequencyEstimator* self, double initial_frequency)
+{
+	self->frequency = initial_frequency;
+	self->frequency_update_time = FL_UNDEFINED_TIME;
+	self->samples = 0;
+
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("frequency_estimator_process() last_t=%.16e, last_f=%.16e"), self->frequency_update_time, self->frequency);
+}
+
+bool frequency_estimator_process(FrequencyEstimator* self, double time)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16e, last_t=%.16e, f=%.16e, c=%i"), time, self->frequency_update_time, self->frequency, self->samples);
+
+	bool frequency_updated = false;
+	if (KNOWN_TIME(self->frequency_update_time)) {
+		++(self->samples);
+		const double dt = time - self->frequency_update_time;
+
+		if (dt > 0) {
+			const double f = self->samples / dt;
+			const double df = fabs(f - self->frequency);
+			if (dt * df >= FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD) {
+				self->samples = 0;
+				self->frequency = f;
+				self->frequency_update_time = time;
+				frequency_updated = true;
+
+				LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("last_t=%.16e, f=%.16e"), time, self->frequency);
+			}
+		}
+
+	} else {
+		self->frequency_update_time = time;
+		LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("last_t=%.16e, f=%.16e"), time, self->frequency);
+	}
+
+	return frequency_updated;
+}
diff --git a/lbs-server/src/fused/FrequencyEstimator.h b/lbs-server/src/fused/FrequencyEstimator.h
new file mode 100644
index 0000000..ea5b809
--- /dev/null
+++ b/lbs-server/src/fused/FrequencyEstimator.h
@@ -0,0 +1,59 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    FrequencyEstimator.h
+ * @brief   Estimates frequency of arbitrary timed events.
+ */
+
+#pragma once
+#ifndef __FREQUENCYESTIMATOR_H_
+#define __FREQUENCYESTIMATOR_H_
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef struct {
+	double   frequency;             /* Current estimated frequency. */
+	double   frequency_update_time; /* Time of last frequency update. */
+	unsigned samples;               /* Number of samples from last frequency update. */
+} FrequencyEstimator;
+
+/**
+ * @brief Initialization of the FrequencyEstimator object (constructor).
+ * @param[in] initial_frequency  Initial frequency.
+ */
+extern void frequency_estimator_init(FrequencyEstimator* self, double initial_frequency);
+
+/** (double) Returns current frequency. */
+#define frequency_estimator_get_frequency(self) ((self)->frequency)
+
+/**
+ * @brief Called when new event happens.
+ * @param[in] time  Time of event.
+ * @return  Is new frequency calculated.
+ */
+extern bool frequency_estimator_process(FrequencyEstimator* self, double time);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __FREQUENCYESTIMATOR_H_ */
diff --git a/lbs-server/src/fused/FusionEngine.c b/lbs-server/src/fused/FusionEngine.c
new file mode 100644
index 0000000..291fea9
--- /dev/null
+++ b/lbs-server/src/fused/FusionEngine.c
@@ -0,0 +1,353 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "GyroscopeFilter.h"
+#include "Conversions.h"
+#include "FusionEngine.h"
+#include "debug_util.h"
+
+#define TIME_FORMAT                     "%.3fs"
+
+/* the bigger the default standard deviations, the swifter response to first measurements */
+#define INITIAL_POSITION_SIGMA2         SQUARE(1024 * M_2PI * EARTH_RADIUS)
+#define INITIAL_VELOCITY_SIGMA2         (1024 * 1024 * SQUARE(FL_DEFAULT_VELOCITY_SIGMA))
+#define INITIAL_ACCELERATION_SIGMA2     DEFAULT_ACCELERATION_SIGMA2
+
+void fusion_engine_init(FusionEngine* self, MotionDetectorStateChangeListener on_motion_state_changed, double utcOffset)
+{
+	LOG_FUSED_DEV(DBG_LOW, INDENT("(motion_cb=%p)"), on_motion_state_changed);
+	orientation_init(&self->orientation, on_motion_state_changed, DEFAULT_ROTATION_SIGMA2);
+	gyroscope_filter_init(&self->gyro_bias);
+	time_offset_init(&self->timestamp_offset);
+	self->engine_started = false;
+	self->enable_gyro_filter = false;
+	peace_detector_init(&self->acc_peace_detector, 0.2, 10);
+	peace_detector_init(&self->gyro_peace_detector, 0.04, 10);
+	calibration_filter_init(&self->calibration_filter, utcOffset);
+	self->enable_static_calibration = true;
+	fusion_engine_reset(self);
+	LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
+}
+
+void fusion_engine_exit(FusionEngine* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, INDENT("()"));
+	time_offset_exit(&self->timestamp_offset);
+	gyroscope_filter_exit(&self->gyro_bias);
+	orientation_exit(&self->orientation);
+	LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
+}
+
+void fusion_engine_start(FusionEngine* self)
+{
+	if (self->engine_started == false) {
+		self->engine_started = true;
+		LOG_FUSED_DEV(DBG_LOW, UNINDENT("(): OK"));
+	} else {
+		LOG_FUSED_DEV(DBG_LOW, UNINDENT("(): Already started"));
+	}
+}
+
+void fusion_engine_stop(FusionEngine* self)
+{
+	if (self->engine_started) {
+		self->engine_started = false;
+		LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(): OK"));
+	} else {
+		LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(): E_NOT_STARTED"));
+	}
+}
+
+void fusion_engine_reset(FusionEngine* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+	self->last_notification_time = FL_UNDEFINED_TIME;
+	self->was_position = false;
+
+	location_filter_init(&self->kalf, 0, 0, EARTH_RADIUS, INITIAL_POSITION_SIGMA2, INITIAL_VELOCITY_SIGMA2, INITIAL_ACCELERATION_SIGMA2);
+	self->fix_status = LOCATION_STATUS_NO_FIX;
+	tangent_frame_init(&self->__te);
+	time_offset_reset(&self->timestamp_offset);
+	orientation_reset(&self->orientation);
+	peace_detector_reset(&self->acc_peace_detector);
+	peace_detector_reset(&self->gyro_peace_detector);
+	calibration_filter_reset(&self->calibration_filter);
+}
+
+static void fusion_engine_spherical_to_tangent2d(FusionEngine* self,
+		double latitude, double longitude, double sigma_of_horizontal_pos, Vector_3d_ref pm, Vector_3d_ref s2pm)
+{
+	self->fix_status = LOCATION_STATUS_2D_FIX;
+	double radius = location_filter_get_position(&self->kalf)[Z];
+	tangent_frame_spherical_to_tangent(&self->__te, latitude, longitude, radius, pm);
+
+	s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(sigma_of_horizontal_pos);
+}
+
+static void fusion_engine_spherical_to_tangent3d(FusionEngine* self,
+		double latitude, double longitude, double altitude, double hor_accuracy, double ver_accuracy,
+		double speed, double sigma_of_speed, double direction, double climb, double sigma_of_climb,
+		Vector_3d_ref pm, Vector_3d_ref vm, Vector_3d_ref s2pm, Vector_3d_ref s2vm)
+{
+	self->fix_status = LOCATION_STATUS_3D_FIX;
+	double radius = EARTH_RADIUS + altitude;
+	tangent_frame_spherical_to_tangent(&self->__te, latitude, longitude, radius, pm);
+
+	double cd = pm[Z] / radius;
+	double cd2 = SQUARE(cd);
+	double sd2 = 1 - cd2;
+	if (sd2 < 0) {
+		cd2 = 1;
+		sd2 = 0;
+	}
+
+	/* Notice:  in principle we should rotate the diagonal sigma matrix here (two matrix multiplications).
+				This approximation assumes the old and new points are close. */
+	if (ver_accuracy > 0) {
+		double s2h = fl_square(hor_accuracy);
+		double s2v = fl_square(ver_accuracy);
+		s2pm[X] = s2pm[Y] = cd2 * s2h + sd2 * s2v;
+		s2pm[Z] = sd2 * s2h + cd2 * s2v;
+	} else {
+		s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(hor_accuracy);
+	}
+
+	tangent_frame_spherical_to_tangent_speed(&self->__te, pm, speed, direction, climb, vm);
+	if (sigma_of_climb > 0) {
+		s2vm[X] = s2vm[Y] = cd2 * fl_square(sigma_of_speed)	+ sd2 * fl_square(sigma_of_climb);
+		s2vm[Z] = sd2 * fl_square(sigma_of_speed) + cd2 * fl_square(sigma_of_climb);
+	} else {
+		s2vm[X] = s2vm[Y] = s2vm[Z] = fl_square(sigma_of_speed);
+	}
+}
+
+/**
+ * @brief Check the notification conditions; if satisfied get the current
+ *        prediction in global coordinate frame and signal it to the user.
+ * @param[in] time  Internal time
+ */
+static bool fusion_engine_signal_updated_location(FusionEngine* self, double time)
+{
+	LOG_FUSED_DEV(DBG_ERR, "t=%g, fixing=%g", time, self->last_notification_time);
+	if (FL_MIN_NOTIFICATION_FILTRATION && self->engine_started && self->was_position) {
+		time += time_offset_get(&self->timestamp_offset);
+		if (!KNOWN_TIME(self->last_notification_time) ||
+				time - self->last_notification_time >= FL_MIN_NOTIFICATION_INTERVAL) {
+			self->last_notification_time = time;
+			return true;
+		}
+	}
+	return false;
+}
+
+/** Filter input: Enter the raw position measurement */
+bool fusion_engine_position_2d_event(FusionEngine* self, double time,
+		double latitude, double longitude, double sigma_of_horizontal_pos)
+{
+	double t = time_offset_correct_time(&self->timestamp_offset, time);
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g, %g), sp=%gm)"),
+			t, latitude, longitude, sigma_of_horizontal_pos);
+	Vector_3d pm; /* p-measurement */
+	Vector_3d s2pm; /* p-measurement variance */
+
+	fusion_engine_spherical_to_tangent2d(self, latitude, longitude, sigma_of_horizontal_pos, pm, s2pm);
+	location_filter_kalman_p(&self->kalf, t, pm, s2pm);
+
+	self->was_position = true;
+	tangent_frame_create(&self->__te, location_filter_get_position(&self->kalf), location_filter_get_velocity(&self->kalf));
+	location_filter_update_state(&self->kalf);
+
+	return fusion_engine_signal_updated_location(self, t);
+}
+
+/** Filter input: Enter the raw position measurement */
+bool fusion_engine_position_3d_event(FusionEngine* self, double time,
+		double latitude, double longitude, double altitude,
+		double hor_accuracy, double ver_accuracy, double speed, double sigma_of_speed,
+		double direction, double climb, double sigma_of_climb)
+{
+	double t = time_offset_correct_time(&self->timestamp_offset, time);
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g, %g, %g), dir=%g, spd=%g, climb=%g, acc=%g, sv=%gm/s)"),
+			time, latitude, longitude, altitude, direction, speed, climb, hor_accuracy, sigma_of_speed);
+
+	Vector_3d pm; /* p-measurement */
+	Vector_3d vm; /* v-measurement */
+	Vector_3d s2pm; /* p-measurement variance */
+	Vector_3d s2vm; /* v-measurement variance */
+
+	fusion_engine_spherical_to_tangent3d(self, latitude, longitude, altitude,
+			hor_accuracy, ver_accuracy, speed, sigma_of_speed,
+			direction, climb, sigma_of_climb, pm, vm, s2pm, s2vm);
+
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+	double vhe = vector_2d_length(location_filter_get_velocity(&self->kalf));
+	double azimuth = conversions_degrees_to_radians(direction);
+	orientation_new_azimuth(&self->orientation, t, azimuth, vhe, speed);
+	location_filter_set_z_rot_speed(&self->kalf, orientation_get_z_rot_speed(&self->orientation));
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("GYROO(), t=%.16e, wze=%.16e"), t, orientation_get_z_rot_speed(&self->orientation));
+#endif
+
+	location_filter_kalman_pv(&self->kalf, t, pm, vm, s2pm, s2vm);
+	self->was_position = true;
+	tangent_frame_create(&self->__te, location_filter_get_position(&self->kalf), location_filter_get_velocity(&self->kalf));
+	location_filter_update_state(&self->kalf);
+
+	return fusion_engine_signal_updated_location(self, t);
+}
+
+void fusion_engine_get_wgs_location(FusionEngine* self, WgsLocation* location)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("WGS I(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
+			self->last_notification_time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
+			self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+
+	if (location == NULL) {
+		LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(), NULL pointer"));
+		return;
+	}
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(), last_notification_time=%gs"), self->last_notification_time);
+
+	const const_Vector_3d_ref pos = location_filter_get_predicted_position(&self->kalf);
+	location->time = self->last_notification_time;
+
+	tangent_frame_tangent_to_spherical(&self->__te, pos, &location->latitude, &location->longitude, &location->altitude);
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("POS (), t=%.16e, p0=%.16e, p1=%.16e, p2=%.16e => lat=%.16e, lon=%.16e, alt=%.16e"),
+			location->time, pos[0], pos[1], pos[2], location->latitude, location->longitude, location->altitude);
+
+	const const_Vector_3d_ref vel = location_filter_get_predicted_velocity(&self->kalf);
+	tangent_frame_tangent_to_spherical_speed(&self->__te, pos, vel, &location->speed, &location->direction, &location->climb);
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPD (), t=%.16e, v0=%.16e, v1=%.16e, v2=%.16e, speed=%.16e, dir=%.16e, climb=%.16e"),
+			location->time, vel[0], vel[1], vel[2], location->speed, location->direction, location->climb);
+
+	const const_fl_kalf_sigma_cube_ref s2x = location_filter_get_predicted_covariance(&self->kalf);
+	location->hor_dev = sqrt((s2x[X][POS][POS] + s2x[Y][POS][POS]) * 0.5);
+	location->ver_dev = s2x[Z][POS][POS];
+	location->speed_dev = sqrt((s2x[X][VEL][VEL] + s2x[Y][VEL][VEL]) * 0.5);
+	location->climb_dev = s2x[Z][VEL][VEL];
+
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("WGS O(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
+			self->last_notification_time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
+			self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+}
+
+bool fusion_engine_speed_event(FusionEngine* self, double time, double speed, double sigma_of_speed)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPEED(), t=%.16e, speed=%.16e, sigma=%.16e"), time, speed, sigma_of_speed);
+	location_filter_update_speed(&self->kalf, time, speed, sigma_of_speed);
+
+	tangent_frame_create(&self->__te, location_filter_get_position(&self->kalf), location_filter_get_velocity(&self->kalf));
+	location_filter_update_state(&self->kalf);
+	return fusion_engine_signal_updated_location(self, time);
+}
+
+bool fusion_engine_acc_event(FusionEngine* self, double time, double x, double y, double z)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC I(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
+			time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
+			self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+	time_offset_reference_time(&self->timestamp_offset, time);
+
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", RAC, a=(%g, %g, %g)[m/s^2])"), time, x, y, z);
+	/* float, math system -> double, geographic system, rotated into local (tangent) frame */
+	Vector_3d acc = { x, y, z };
+	peace_detector_new_data(&self->acc_peace_detector, time, acc);
+	orientation_new_acceleration(&self->orientation, time, acc);
+	/* process linear acceleration */
+	location_filter_clear_acc(&self->kalf, time);
+
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC O(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
+			time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
+			self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+
+	return fusion_engine_signal_updated_location(self, time);
+}
+
+bool fusion_engine_gyro_event(FusionEngine* self, double time, double x, double y, double z)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("GYROI(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
+			time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
+			self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+
+	time_offset_reference_time(&self->timestamp_offset, time);
+	const Vector_3d angular_velocity = { -x, -y, -z };
+	peace_detector_new_data(&self->gyro_peace_detector, time, angular_velocity);
+	const_Vector_3d_ref av = angular_velocity;
+	if (self->enable_static_calibration) {
+		av = calibration_filter_filter(&self->calibration_filter, time, av,
+				peace_detector_is_stable(&self->gyro_peace_detector) &&
+				peace_detector_is_stable(&self->acc_peace_detector));
+	}
+
+	if (self->enable_gyro_filter)
+		av = gyroscope_filter_process(&self->gyro_bias, time, av);
+
+	if (orientation_new_angular_velocity(&self->orientation, time, av, DEFAULT_ROTATION_SIGMA2)) {
+		location_filter_set_z_rot_speed(&self->kalf, orientation_get_z_rot_speed(&self->orientation));
+		location_filter_update_acc(&self->kalf, time);
+	}
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("GYROO(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
+			time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
+			self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+
+	return false;
+}
+
+/**
+ * For ips_devel test
+ */
+void fusion_engine_get_orientation(FusionEngine* self, double* o)
+{
+	orientation_get_orientation(&self->orientation, o);
+}
+
+/**
+ * For ips_devel test
+ */
+void fusion_engine_set_enable_gyro_filter(FusionEngine* self, bool enable)
+{
+	self->enable_gyro_filter = enable;
+}
+
+/**
+ * For ips_devel test
+ */
+void fusion_engine_set_enable_static_calibration(FusionEngine* self, bool enable)
+{
+	self->enable_static_calibration = enable;
+}
+
+/**
+ * For ips_devel test
+ */
+void fusion_engine_set_enable_static_autocalibration(FusionEngine* self, bool enable)
+{
+	calibration_filter_set_enable_autocalibration(&(self->calibration_filter), enable);
+}
+
+void fusion_engine_clear_calibration(FusionEngine* self)
+{
+	calibration_filter_clear_calibration(&(self->calibration_filter));
+}
+
+char* fusion_engine_get_calibration(FusionEngine* self)
+{
+	return calibration_filter_get_calibration(&(self->calibration_filter));
+}
+
+void fusion_engine_set_calibration(FusionEngine* self, const char* params)
+{
+	calibration_filter_set_calibration(&(self->calibration_filter), params);
+}
diff --git a/lbs-server/src/fused/FusionEngine.h b/lbs-server/src/fused/FusionEngine.h
new file mode 100644
index 0000000..5968b8c
--- /dev/null
+++ b/lbs-server/src/fused/FusionEngine.h
@@ -0,0 +1,252 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    FusionEngine.h
+ * @brief   Implementation of a 4D (2D position + 2D velocity) Kalman filter
+ */
+
+#pragma once
+#ifndef __FUSION_ENGINE_H__
+#define __FUSION_ENGINE_H__
+
+#include <stdbool.h>
+#include "parameters.h"
+#include "MotionDetector.h"
+#include "Conversions.h"
+#include "Matrix.h"
+#include "TangentFrame.h"
+#include "LocationFilter.h"
+#include "AccelerometerFilter.h"
+#include "GyroscopeFilter.h"
+#include "TimeOffset.h"
+#include "Orientation.h"
+#include "PeaceDetector.h"
+#include "CalibrationFilter.h"
+
+#include <memory.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define DEFAULT_ACCELERATION_SIGMA2      SQUARE(FL_ACCEL_NOISE_LEVEL)  /* [(m/s^2)^2] */
+#define DEFAULT_ROTATION_SIGMA2          SQUARE(1.0 / 256) /* [(rad/s)^2] */
+
+/** position on the Earth surface in Cartesian coordinates */
+typedef double position_vector[GEO_DIMENSION];
+
+typedef struct {
+	double  time;
+	position_vector  position;
+} fl_prediction;
+
+typedef enum {
+	LOCATION_STATUS_NO_FIX = 0,	/*/< No fix status. */
+	LOCATION_STATUS_2D_FIX,		/*/< 2D fix status (latitude/longitude/speed/direction). */
+	LOCATION_STATUS_3D_FIX,		/*/< 3D fix status (altitude/climb as well). */
+} LocationStatus;
+
+typedef struct {
+	bool engine_started;
+	bool was_position;
+	bool enable_gyro_filter;
+	bool enable_static_calibration;
+	double last_notification_time;
+	TimeOffset timestamp_offset;
+	TangentFrame __te;			/* tangent frame map */
+	Orientation orientation;
+	LocationFilter kalf;		/* Kalman filter */
+	LocationStatus fix_status;	/* pass-through */
+	GyroscopeFilter gyro_bias;
+	PeaceDetector acc_peace_detector;
+	PeaceDetector gyro_peace_detector;
+	CalibrationFilter calibration_filter;
+} FusionEngine;
+
+typedef struct {
+	double time;			/*/< Event time in [s]. */
+	double latitude;		/*/< Latitude data in degrees. */
+	double longitude;		/*/< Longitude data in degrees. */
+	double altitude;		/*/< Altitude data in [m]. */
+	double speed;			/*/< The speed over ground. (m/s) */
+	double direction;		/*/< The course made in degrees relative to true north. The value is always in the range [0.0, 360.0] degree. */
+	double climb;			/*/< The vertical speed. (m/s) */
+	double hor_dev;			/**< std. dev. of horizontal position in [m] */
+	double ver_dev;			/**< std. dev. of vertical   position in [m] */
+	double speed_dev;		/**< std. dev. of horizontal velocity in [m/s] */
+	double climb_dev;		/**< std. dev. of vertical   velocity in [m/s] */
+} WgsLocation;
+
+/**
+ * @brief Initialization of the engine.
+ * @param[in] on_motion_state_changed
+ *            Callback to be invoked when the detected state of motion changes. This argument is optional, and can be NULL.
+ */
+extern void fusion_engine_init(FusionEngine* self,
+		MotionDetectorStateChangeListener on_motion_state_changed, double utcOffset);
+
+/** Cleanup of the singleton unit (static destructor). This corresponds to service unload. */
+extern void fusion_engine_exit(FusionEngine* self);
+
+/**
+ * @brief Start processing the location and sensory inputs, and sending back notifications.
+ *        This corresponds to service start.
+ */
+extern void fusion_engine_start(FusionEngine* self);
+
+/**
+ * @brief Stops processing the inputs, and sending notifications. This corresponds to service stop.
+ */
+extern void fusion_engine_stop(FusionEngine* self);
+
+/**
+ * @brief Reset of the internal state back to the initial one. The handlers and start/stop state are not changed.
+ *        This function is used for initialization, soft restarts, and repetitive testing.
+ */
+extern void fusion_engine_reset(FusionEngine* self);
+
+/*******************************************************************************
+ * Process 2D (horizontal) position without change of the altitude or speed (WPS).
+ * Kalman merging is carried out in tangent frame, and the origin is moved
+ * after each measurement so that the predictions always start at (0, 0, R + h).
+ *  - Clocks synchronization
+ *  - Kalman data blending
+ *  - Repositioning of the tangent frame
+ *
+ * @param[in] time       Internal time.
+ * @param[in] latitude   Latitude in degrees.
+ * @param[in] longitude  Longitude in degrees.
+ * @param[in] sigma      Horizontal standard deviation of provided position in [m].
+ */
+extern bool fusion_engine_position_2d_event(FusionEngine* self, double time, double latitude, double longitude, double sigma);
+
+/*******************************************************************************
+ * Process 3D position along with 3D velocity (GPS).
+ * Kalman merging is carried out in tangent frame, and the origin is moved after each measurement
+ * so that the predictions always start at (0, 0, R + h).
+ *  - Clocks synchronization
+ *  - Kalman data blending
+ *  - Repositioning of the tangent frame
+ * If FL_KALMAN_ANGULAR_VELOCITY is enabled the angular velocity is derived
+ * from consecutive measurements of linear velocities and merged with gyroscope readings.
+ *
+ * @param[in] time           Internal time.
+ * @param[in] latitude       Latitude in degrees.
+ * @param[in] longitude      Longitude in degrees.
+ * @param[in] altitude       Altitude in [m].
+ * @param[in] hor_accuracy   Horizontal standard deviation of provided position in [m].
+ * @param[in] ver_accuracy   Standard deviation of altitude in [m].
+ * @param[in] speed          Horizontal speed in [m/s].
+ * @param[in] sigma_of_speed Standard deviation of horizontal speed in [m/s].
+ * @param[in] direction      Direction (azimuth) of movement in degrees.
+ * @param[in] climb          Vertical speed in [m/s].
+ * @param[in] sigma_of_climb Standard deviation of vertical speed in [m/s].
+ */
+extern bool fusion_engine_position_3d_event(FusionEngine* self, double time,
+		double latitude, double longitude, double altitude,
+		double hor_accuracy, double ver_accuracy,
+		double speed, double sigma_of_speed, double direction, double climb, double sigma_of_climb);
+
+/**
+ * @brief Process speed measurement events.
+ * @param[in] time  Internal time.
+ * @param[in] speed  Horizontal speed in [m/s].
+ * @param[in] sigma_of_speed  Standard deviation of horizontal speed in [m/s].
+ */
+extern bool fusion_engine_speed_event(FusionEngine* self, double time, double speed, double sigma_of_speed);
+
+/**
+ * @brief Process accelerometer events. The time offset is corrected if necessary.
+ *  - Clocks synchronization
+ *  - Device-to-tangent frame orientation
+ *  - Prediction
+ * @param[in] time  Sensory time-stamp
+ * @param[in] x  X coordinate of acceleration.
+ * @param[in] y  Y coordinate of acceleration.
+ * @param[in] z  Z coordinate of acceleration.
+ */
+extern bool fusion_engine_acc_event(FusionEngine* self, double time, double x, double y, double z);
+
+/**
+ * @brief Process gyroscope events. The time offset is corrected if necessary.
+ *  - Clocks synchronization
+ *  - Device-to-tangent frame orientation
+ *  - Prediction
+ * @param[in] time  Sensory time-stamp
+ * @param[in] x  X coordinate of angular rotation velocity vector.
+ * @param[in] y  Y coordinate of angular rotation velocity vector.
+ * @param[in] z  Z coordinate of angular rotation velocity vector.
+ */
+extern bool fusion_engine_gyro_event(FusionEngine* self, double time, double x, double y, double z);
+
+/**
+ * @brief Retrieve the last processed location, typically during the on_location_changed notification.
+ * @param[out] location  Current WGS location
+ */
+extern void fusion_engine_get_wgs_location(FusionEngine* self, WgsLocation* location);
+
+/**
+ * For ips_devel test
+ * @brief Retrieve current orientation.
+ * @param[out] o   Orientation represented as a vector part of rotation quaternion.
+ *                 Must be length of minimum 3.
+ */
+extern void fusion_engine_get_orientation(FusionEngine* self, double* o);
+
+/**
+ * For ips_devel test
+ * @brief Enable gyroscope calibration filter.
+ * @param[in] enable  True to enable calibration filter, false to disable it.
+ */
+extern void fusion_engine_set_enable_gyro_filter(FusionEngine* self, bool enable);
+
+/**
+ * For ips_devel test
+ * @brief Enable gyroscope static calibration.
+ * @param[in] enable  True to enable static calibration, false to disable it.
+ */
+extern void fusion_engine_set_enable_static_calibration(FusionEngine* self, bool enable);
+
+/**
+ * For ips_devel test
+ * @brief Enable gyroscope static autocalibration.
+ * @param[in] enable  True to enable static autocalibration, false to disable it.
+ */
+extern void fusion_engine_set_enable_static_autocalibration(FusionEngine* self, bool enable);
+
+/**
+ * @brief Clears gyroscope calibration parameters.
+ */
+extern void fusion_engine_clear_calibration(FusionEngine* self);
+
+/**
+ * @brief Retrieve current gyroscope calibration parameters.
+ * @return Gyroscope calibration parameters. Must be disposed to prevent memory leak.
+ */
+extern char* fusion_engine_get_calibration(FusionEngine* self);
+
+/**
+ * @brief Sets new gyroscope calibration parameters.
+ * @param[in] params  New gyroscope calibration parameters.
+ */
+extern void fusion_engine_set_calibration(FusionEngine* self, const char* params);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __FUSION_ENGINE_H__ */
diff --git a/lbs-server/src/fused/GravityEstimator.h b/lbs-server/src/fused/GravityEstimator.h
new file mode 100644
index 0000000..101d6f3
--- /dev/null
+++ b/lbs-server/src/fused/GravityEstimator.h
@@ -0,0 +1,73 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    GravityEstimator.h
+ * @brief   Estimator of the gravity value used for accelerometer scale calibration.
+ */
+
+#pragma once
+#ifndef __GRAVITY_ESTIMATOR_H__
+#define __GRAVITY_ESTIMATOR_H__
+
+#include "MovingAverageFilters.h"
+#include "parameters.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef MovingAverage1dFilter GravityEstimator;
+
+#define gravity_estimator_init(self) moving_average_1d_filter_init(self, (1 << FL_GRES_AEMA_PLATEAU_BITS))
+
+#define gravity_estimator_exit(self) moving_average_1d_filter_exit(self)
+
+/**
+ * @brief Bring the internal state back to initial one. This function is used for repetitive testing and module soft restarts.
+ */
+#define gravity_estimator_reset(self) moving_average_1d_filter_reset(self)
+
+/**
+ * @brief Processing of a single data sample.
+ * @param[in] g2   Modulus squared of the measured acceleration projected onto z-axis.
+ * @return[double] Estimated value of the Earth gravity.
+ */
+#define gravity_estimator_process(self, g2) moving_average_1d_filter_process(self, g2)
+
+/**
+ * @brief Bring the filter to a stationary state (EMA mode, input = output). This method is used for testing.
+ * @param[in] g2  Modulus squared value of Earth gravity measured by the accelerometer.
+ */
+#define gravity_estimator_set_estimate(self, g2) moving_average_1d_filter_set_estimate(self, g2)
+
+/**
+ * @brief Fetch the current gravity estimate.
+ * @param[in] self  Object pointer
+ * @return[double]  Output value after last processed sample.
+ */
+
+/** Retrieve current estimate */
+#define gravity_estimator_get_estimate(self) moving_average_1d_filter_get_estimate(self)
+
+#define gravity_estimator_get_update_rate(self) moving_average_get_update_rate(&(self)->base)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* __GRAVITY_ESTIMATOR_H__ */
diff --git a/lbs-server/src/fused/GyroscopeFilter.c b/lbs-server/src/fused/GyroscopeFilter.c
new file mode 100644
index 0000000..376f9d4
--- /dev/null
+++ b/lbs-server/src/fused/GyroscopeFilter.c
@@ -0,0 +1,56 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#if !defined(LOG_THRESHOLD)
+	/* custom logging threshold - keep undefined on the repo */
+	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
+#endif
+
+#include "GyroscopeFilter.h"
+#include "GravityEstimator.h"
+#include "math-ext.h"
+#include "debug_util.h"
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+#define GYRO_PLATEAU_EVIDENCE  (1 << FL_GYRO_AEMA_PLATEAU_BITS)
+#define GYRO_SCALE2_I          (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_I)))
+#define GYRO_SCALE2_F          (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_F)))
+
+void gyroscope_filter_init(GyroscopeFilter* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, INDENT("()"));
+	gyroscope_filter_reset(self);
+	LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
+}
+
+void gyroscope_filter_exit(GyroscopeFilter* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+}
+
+void gyroscope_filter_reset(GyroscopeFilter* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+	waema3d_estimator_reset(self, (SQUARE(FL_GYRO_SPIN_SIGMA_I / FL_GYRO_SPIN_SIGMA_F) - 1) / SQUARE(GYRO_PLATEAU_EVIDENCE), GYRO_PLATEAU_EVIDENCE, GYRO_SCALE2_I);
+}
+
+const_Vector_3d_ref gyroscope_filter_process(GyroscopeFilter* self, double time, const_Vector_3d_ref wm)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs)"), time);
+	return waema3d_estimator_filter(self, wm);
+}
diff --git a/lbs-server/src/fused/GyroscopeFilter.h b/lbs-server/src/fused/GyroscopeFilter.h
new file mode 100644
index 0000000..86fbf2d
--- /dev/null
+++ b/lbs-server/src/fused/GyroscopeFilter.h
@@ -0,0 +1,61 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    GyroscopeFilter.h
+ * @brief   Correction of the gyroscope bias.
+ */
+
+#pragma once
+#ifndef __GYROSCOPE_FILTER_H__
+#define __GYROSCOPE_FILTER_H__
+
+#include "Waema3dEstimator.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef Waema3dEstimator GyroscopeFilter;
+
+
+void gyroscope_filter_init(GyroscopeFilter* self);
+void gyroscope_filter_exit(GyroscopeFilter* self);
+void gyroscope_filter_reset(GyroscopeFilter* self);
+
+/**
+ * @brief [TEST] Bring the filter to a stationary state (EMA mode, input = output).
+ * @param[in] w0  3D vector of angular velocity measured by the gyroscope in inertial coordinate frame.
+ */
+void gyroscope_filter_set(GyroscopeFilter* self, const_Vector_3d_ref w0);
+
+/** Processing of a vector data sample. This method performs:
+ * - estimation of the gyroscope bias with the weighted AEMA filter;
+ *   the individual samples are weighted by the distance from average using Gaussian
+ *   curve with monotonically-decreasing sigma parameter;
+ * - correction of the measurement by the estimated bias.
+ * @param[in] time  Event time in seconds.
+ * @param[in] wm  3D vector of measured angular velocity.
+ * @return  3D vector of bias-corrected angular velocity.
+ */
+const_Vector_3d_ref gyroscope_filter_process(GyroscopeFilter* self, double time, const_Vector_3d_ref wm);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __GYROSCOPE_FILTER_H__ */
diff --git a/lbs-server/src/fused/LocationFilter.c b/lbs-server/src/fused/LocationFilter.c
new file mode 100644
index 0000000..07c3057
--- /dev/null
+++ b/lbs-server/src/fused/LocationFilter.c
@@ -0,0 +1,479 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "LocationFilter.h"
+#include "Matrix.h"
+#include "Conversions.h"
+#include "debug_util.h"
+
+#include <memory.h>
+
+#define TIME_FORMAT "%.9fs"
+
+#define NEAR_ZERO 1.0e-200
+
+static bool location_filter_update_heading(_fl_kalf_state_m* self)
+{
+	const double vx = self->v[X];
+	const double vy = self->v[Y];
+	if (fabs(vx) > 0.001 || fabs(vy) > 0.001) {
+		self->heading = atan2(vy, vx);
+		return true;
+	}
+	return false;
+}
+
+void location_filter_init(LocationFilter* self, double x, double y, double z, double var_p, double var_v, double var_a)
+{
+	self->default_var_a = var_a;
+	self->last_correction_time = FL_UNDEFINED_TIME;
+	memset(&self->prior, 0, sizeof(self->prior));
+	memset(&self->posterior, 0, sizeof(self->posterior));
+	self->z_rot_speed = 0;
+	self->prior.x.p[X] = self->posterior.x.p[X] = x;
+	self->prior.x.p[Y] = self->posterior.x.p[Y] = y;
+	self->prior.x.p[Z] = self->posterior.x.p[Z] = z;
+	unsigned i;
+	for (i = GEO_DIMENSION; i;) {
+		--i;
+		/* diagonal terms */
+		self->prior.s2x[i][POS][POS] = self->posterior.s2x[i][POS][POS] = var_p;
+		self->prior.s2x[i][VEL][VEL] = self->posterior.s2x[i][VEL][VEL] = var_v;
+		self->prior.s2x[i][ACC][ACC] = self->posterior.s2x[i][ACC][ACC] = var_a;
+	}
+	location_filter_update_heading(&self->posterior.x);
+	self->prior.x.heading = self->posterior.x.heading;
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+}
+
+/**
+ * @brief Check for and correct the numerical errors which cause the covariance matrix
+ *        to cease being positive-definite. This operation has to be performed regularly,
+ *        or otherwise magic ensues, and every strange result becomes possible.
+ * @param[in, out] src  The state object to be rectified.
+ *                      This is either the predicted (prior), or estimated (posterior) one.
+ */
+static void __rectify_state(_fl_kalf_sigma_cube s2x)
+{
+	unsigned i;
+	for (i = GEO_DIMENSION; i--;) {
+		Matrix_3d_ref s2xi = s2x[i];
+		/* diagonal elements */
+#define CHECK(u) \
+		if (s2xi[u][u] < 0) { \
+			/*LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): src.s2x[%u][" #u "][" #u "] = %g < 0"), src, i, s2xi[u][u]);*/ \
+			s2xi[u][u] = 0; \
+		}
+		CHECK(POS);
+		CHECK(VEL);
+		CHECK(ACC);
+#undef CHECK
+
+		/* partial determinants */
+		double uuvv, uvvu;
+#define CHECK(u, v) \
+		uuvv = s2xi[u][u] * s2xi[v][v]; \
+		uvvu = s2xi[u][v] * s2xi[v][u]; \
+		if (uuvv < uvvu) { \
+			/*LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): det(src.s2x[%u], " #u ", " #v ") = %g < 0"), src, i, uuvv - uvvu);*/ \
+			s2xi[u][v] = \
+			s2xi[v][u] *= sqrt(uuvv / uvvu); \
+		}
+		CHECK(POS, VEL);
+		CHECK(POS, ACC);
+		CHECK(ACC, VEL);
+#undef CHECK
+	}
+}
+
+/**
+ * @brief Copy of the state (3x3D position & covariance) object.
+ * @param[out] dst      Destination state object
+ * @param[out] src      Source state object
+ */
+static void __copy_state(_fl_kalf_state *dst, _fl_kalf_state *src)
+{
+	__rectify_state(src->s2x);
+	memcpy(dst, src, sizeof(*dst));
+}
+
+void location_filter_update_state(LocationFilter* self)
+{
+	memcpy(&self->prior.x, &self->posterior.x, sizeof(self->prior.x));
+}
+
+/**
+ * @brief Calculates prior from posterior, i.e. extrapolates position,
+ *        velocity, and the covariance-matrices up to the given time.
+ *        We assume rotation is at constant angular rate (z_rot_speed) and speed (norm of velocity) is also constant.
+ * Therefore motion equations are:
+ * Vx = vx cos(wt) - vy sin(wt)
+ * Vy = vy cos(wt) + vx sin(wt)
+ * Vz = vz
+ * X = x + vx sin(wt)/w + vy(cos(wt) - 1)/w (by integrating)
+ * Y = y + vy sin(wt)/w - vx(cos(wt) - 1)/w
+ * Z = z + vz t
+ * Ax = -vy w (differentiating at t=0)
+ * Ay = vx w
+ * Az = 0
+ * And transition matrix is:
+ * |	1	0	0	sin(wt)/w			(cos(wt) - 1)/w	0	0	0	0|
+ * |	0	1	0	-(cos(wt) - 1)/w	sin(wt)/w		0	0	0	0|
+ * |	0	0	1	0					0				t	0	0	0|
+ * |	0	0	0	cos(wt)				-sin(wt)		0	0	0	0|
+ * |	0	0	0	sin(wt)				-cos(wt)		0	0	0	0|
+ * |	0	0	0	0					0				1	0	0	0|
+ * |	0	0	0	0					-w				0	0	0	0|
+ * |	0	0	0	w					0				0	0	0	0|
+ * |	0	0	0	0					0				0	0	0	0|
+ * so for small wt:
+ * |	1	0	0	t				wt^2/2			0	0	0	0|
+ * |	0	1	0	-wt^2/2			t				0	0	0	0|
+ * |	0	0	1	0				0				t	0	0	0|
+ * |	0	0	0	1 - w^2t^2/2	-t				0	0	0	0|
+ * |	0	0	0	t				-1 + w^2t^2/2	0	0	0	0|
+ * |	0	0	0	0				0				1	0	0	0|
+ * |	0	0	0	0				-w				0	0	0	0|
+ * |	0	0	0	w				0				0	0	0	0|
+ * |	0	0	0	0				0				0	0	0	0|
+ * We assume covariance matrix can be represented as
+ * 3 independent covariance matrixes, one for each coordinate,
+ * and assume transition matrix used for covariance update
+ * for each coordinate is:
+ * |	1	t	t^2/2	|
+ * |	0	1	t		|
+ * |	0	0	1		|
+ * Process noise matrix ???
+ *
+ * @param[in] time  Internal time
+ */
+static void location_filter_predict(LocationFilter* self, double time)
+{
+	if (!KNOWN_TIME(self->last_correction_time)) {
+		/* no prior data */
+		return;
+	}
+	double dt = time - self->last_correction_time;
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT "), dt=" TIME_FORMAT), time, dt);
+	if (dt <= 0) {
+		return;
+	}
+	const _fl_kalf_state_m* post = &self->posterior.x;
+	_fl_kalf_state_m* prior = &self->prior.x;
+	double z_rot_speed = self->z_rot_speed;
+	double rot = z_rot_speed * dt;
+	if (fabs(rot) > NEAR_ZERO) {
+		double cos_rot = cos(rot);
+		double sin_rot = sin(rot);
+		double inv_z_rot_speed = 1.0 / z_rot_speed;
+		double cos_rot_inv = (cos_rot - 1) * inv_z_rot_speed;
+		double sin_rot_inv = sin_rot * inv_z_rot_speed;
+
+		prior->p[X] = post->p[X] + post->v[X] * sin_rot_inv + post->v[Y] * cos_rot_inv;
+		prior->p[Y] = post->p[Y] + post->v[Y] * sin_rot_inv - post->v[X] * cos_rot_inv;
+		prior->v[X] = post->v[X] * cos_rot - post->v[Y] * sin_rot;
+		prior->v[Y] = post->v[Y] * cos_rot + post->v[X] * sin_rot;
+		if (!location_filter_update_heading(prior)) {
+			prior->heading = post->heading + rot;
+		}
+	} else {
+		prior->p[X] = post->p[X] + dt * post->v[X];
+		prior->p[Y] = post->p[Y] + dt * post->v[Y];
+		prior->v[X] = post->v[X];
+		prior->v[Y] = post->v[Y];
+		prior->heading = post->heading;
+	}
+	prior->p[Z] = post->p[Z] + dt * post->v[Z];
+	prior->v[Z] = post->v[Z];
+
+	prior->a[X] = -prior->v[Y] * z_rot_speed;
+	prior->a[Y] = prior->v[X] * z_rot_speed;
+	prior->a[Z] = 0;
+
+	const_fl_kalf_sigma_cube_ref post_s = self->posterior.s2x;
+	_fl_kalf_sigma_cube_ref prior_s = self->prior.s2x;
+	unsigned i;
+	for (i = GEO_DIMENSION; i;) {
+		--i;
+
+		const_Matrix_3d_ref post_si = post_s[i];
+		Matrix_3d_ref prior_si = prior_s[i];
+
+		prior_si[ACC][ACC] = post_si[ACC][ACC];
+		prior_si[ACC][VEL] = post_si[ACC][VEL] + post_si[ACC][ACC] * dt;
+		prior_si[ACC][POS] = post_si[ACC][POS]
+				+ (post_si[ACC][VEL] + post_si[ACC][ACC] * dt / 2) * dt;
+
+		prior_si[VEL][ACC] = prior_si[ACC][VEL]; /* by symmetry */
+		prior_si[VEL][VEL] = post_si[VEL][VEL]
+				+ (post_si[VEL][ACC] * 2 + post_si[ACC][ACC] * dt) * dt;
+		prior_si[VEL][POS] = post_si[VEL][POS]
+				+ (post_si[VEL][VEL] + post_si[POS][ACC]
+						+ (post_si[VEL][ACC] * 3
+								+ post_si[ACC][ACC] * dt) / 2 * dt) * dt;
+
+		prior_si[POS][ACC] = prior_si[ACC][POS]; /* by symmetry */
+		prior_si[POS][VEL] = prior_si[VEL][POS]; /* by symmetry */
+		prior_si[POS][POS] = post_si[POS][POS]
+				+ (post_si[POS][VEL] * 2
+						+ (post_si[VEL][VEL] + post_si[ACC][POS]
+								+ (post_si[ACC][VEL]
+										+ post_si[ACC][ACC] * dt / 4) * dt)
+								* dt) * dt;
+	}
+	/* process noise */
+	prior_s[X][ACC][ACC] += fl_square(prior->a[X] - post->a[X]);
+	prior_s[Y][ACC][ACC] += fl_square(prior->a[Y] - post->a[Y]);
+	__rectify_state(prior_s);
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+}
+
+/**
+ * @brief  Update of the 2D position
+ * Observation matrix for each coordinate is:
+ * |	1	0	0|
+ *
+ * @param[in] time      Internal time
+ * @param[in] pos       Measured position
+ * @param[in] pos_ac    Measured position deviation (squared standard deviation)
+ */
+void location_filter_kalman_p(LocationFilter* self, double time, const_Vector_3d_ref pos, const_Vector_3d_ref pos_ac)
+{
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=%g, x0=%g, x1=%g, x2=%g, s0=%g, s1=%g, s2=%g)"),
+			time, pos[0], pos[1], pos[2], pos_ac[0], pos_ac[1], pos_ac[2]);
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("input (t=%g, p0=%g, p1=%g, p2=%g, v0=%g, v1=%g, v2=%g)"),
+			time, self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2], self->prior.x.v[0], self->prior.x.v[1], self->prior.x.v[2]);
+
+	location_filter_predict(self, time);
+	unsigned i;
+	/* @note WPS-specific: restrict the update to horizontal position */
+	for (i = PLANAR_DIMENSION; i;) {
+		--i;
+		const double s2mi = pos_ac[i];
+		Matrix_3d_ref post_si = self->posterior.s2x[i];
+		const_Matrix_3d_ref prior_si = self->prior.s2x[i];
+		const_Vector_3d_ref s2pis = prior_si[POS];
+		const double s2piss = s2pis[POS];
+		const double innovation_covariance = s2mi + s2piss;
+		if (innovation_covariance > 0) {
+			const double inv_inn_cov = 1.0 / innovation_covariance;
+			const double innovation = pos[i] - self->prior.x.p[i];
+
+			/* reduced Joseph form in special 3x1 case: subtract scaled projection */
+#define SET_FIELD(a, b) post_si[a][b] = inv_inn_cov * ((prior_si[a][b] * s2piss - s2pis[a] * s2pis[b]) + prior_si[a][b] * s2mi)
+#define CPY_FIELD(a, b) post_si[a][b] = post_si[b][a]
+			SET_FIELD(POS, POS);
+			SET_FIELD(POS, VEL);
+			SET_FIELD(POS, ACC);
+			CPY_FIELD(VEL, POS);
+			SET_FIELD(VEL, VEL);
+			SET_FIELD(VEL, ACC);
+			CPY_FIELD(ACC, POS);
+			CPY_FIELD(ACC, VEL);
+			SET_FIELD(ACC, ACC);
+#undef CPY_FIELD
+#undef SET_FIELD
+
+			self->posterior.x.p[i] = self->prior.x.p[i] + innovation * inv_inn_cov * s2pis[POS];
+			self->posterior.x.v[i] = self->prior.x.v[i] + innovation * inv_inn_cov * s2pis[VEL];
+			self->posterior.x.a[i] = self->prior.x.a[i] + innovation * inv_inn_cov * s2pis[ACC];
+			LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(i=%i, t=%g, prev=%g, dx=%g, inv_s2k=%g, s2pis[POS]=%g, cur=%g)"),
+						i, time, self->prior.x.p[i], innovation, inv_inn_cov, s2piss, self->posterior.x.p[i]);
+		}
+	}
+	/* the third coordinate is of `special care': we want to preserve it in the tangent frame, along with its sigma */
+	self->posterior.x.p[Z] = pos[Z];
+	location_filter_update_heading(&self->posterior.x);
+	__copy_state(&self->prior, &self->posterior);
+	self->last_correction_time = time;
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+}
+
+/**
+ * @brief  Update of the 3D position & velocity
+ * Observation matrix for each coordinate is:
+ * |	1	0	0|
+ * |	0	1	0|
+ *
+ * @param[in] time      Internal time
+ * @param[in] pm        Measured position
+ * @param[in] vm        Measured velocity
+ * @param[in] s2pm      Measured position deviation (squared standard deviation)
+ * @param[in] s2vm      Measured velocity deviation (squared standard deviation)
+ */
+void location_filter_kalman_pv(LocationFilter* self, double time,
+		const_Vector_3d_ref pm, const_Vector_3d_ref vm, const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm)
+{
+	location_filter_predict(self, time);
+	unsigned i;
+	for (i = GEO_DIMENSION; i;) {
+		--i;
+
+		/*  shortcuts */
+		const_Matrix_3d_ref prior_cov = self->prior.s2x[i];
+
+		/* Sk = Sp + Sm */
+		Matrix_2d innovation_covariance;
+		innovation_covariance[POS][POS] = prior_cov[POS][POS] + s2pm[i];
+		innovation_covariance[POS][VEL] = /* == prior_cov[POS][VEL] */
+		innovation_covariance[VEL][POS] = prior_cov[VEL][POS];
+		innovation_covariance[VEL][VEL] = prior_cov[VEL][VEL] + s2vm[i];
+
+		Matrix_2d inv_inn_cov;
+		if (matrix_2d_invert(innovation_covariance, inv_inn_cov)) {
+			Matrix_2d k0;
+			matrix_3d_multiply_matrix_2d(prior_cov, inv_inn_cov, k0); /* K = Sp.Si */
+			/* 1 - K = Sm.Si, use positive form */
+			Matrix_2d k1;
+			k1[POS][POS] = s2pm[i] * inv_inn_cov[POS][POS];
+			k1[POS][VEL] = s2pm[i] * inv_inn_cov[POS][VEL];
+			k1[VEL][POS] = s2vm[i] * inv_inn_cov[VEL][POS];
+			k1[VEL][VEL] = s2vm[i] * inv_inn_cov[VEL][VEL];
+			/* state update */
+			self->posterior.x.p[i] = k0[POS][POS] * pm[i]
+					+ k0[POS][VEL] * vm[i]
+					+ k1[POS][POS] * self->prior.x.p[i]
+					+ k1[POS][VEL] * self->prior.x.v[i];
+
+			self->posterior.x.v[i] = k0[VEL][POS] * pm[i]
+					+ k0[VEL][VEL] * vm[i]
+					+ k1[VEL][POS] * self->prior.x.p[i]
+					+ k1[VEL][VEL] * self->prior.x.v[i];
+
+			/* covariance update Se = K1,K1.Sp + K,K.Sm */
+			Matrix_3d_ref s2ei = self->posterior.s2x[i];
+
+#define SET_S2E(j, k)	s2ei[j][k] = \
+				  k1[j][POS] * k1[k][POS] * prior_cov[POS][POS] + k0[j][POS] * k0[k][POS] * s2pm[i] \
+				+ k1[j][POS] * k1[k][VEL] * prior_cov[POS][VEL]                                     \
+				+ k1[j][VEL] * k1[k][POS] * prior_cov[VEL][POS]                                     \
+				+ k1[j][VEL] * k1[k][VEL] * prior_cov[VEL][VEL] + k0[j][VEL] * k0[k][VEL] * s2vm[i]
+
+			SET_S2E(POS, POS);
+			SET_S2E(POS, VEL);
+			SET_S2E(VEL, POS);
+			SET_S2E(VEL, VEL);
+#undef SET_S2E
+		}
+		/* upon arrival of new position-velocity pair (a maximally uncertain) the pair (p, v) decorrelates from a: */
+		self->posterior.s2x[i][POS][ACC] = 0;
+		self->posterior.s2x[i][VEL][ACC] = 0;
+		self->posterior.s2x[i][ACC][POS] = 0;
+		self->posterior.s2x[i][ACC][VEL] = 0;
+		self->posterior.s2x[i][ACC][ACC] = self->prior.s2x[i][ACC][ACC];
+	}
+	location_filter_update_heading(&self->posterior.x);
+	__copy_state(&self->prior, &self->posterior);
+	self->last_correction_time = time;
+
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+}
+
+/**
+ * @brief  Update of the 2D position using speed measurement
+ * Observation matrix is:
+ * |	0	vx/v	0	0	vy/v	0	0	0	0|
+ * Where v = sqrt(vx^2 + vy^2)
+ *
+ * @param[in] time                Internal time
+ * @param[in] speed               Measured horizontal speed
+ * @param[in] sigma_of_speed      Measured horizontal speed deviation (squared standard deviation)
+ */
+void location_filter_update_speed(LocationFilter* self, double time, double speed, double sigma_of_speed)
+{
+	location_filter_predict(self, time);
+	const double vx = self->prior.x.v[X];
+	const double vy = self->prior.x.v[Y];
+	const double v = sqrt(vx * vx + vy * vy);
+	double ch;
+	double sh;
+	if (v != 0) {
+		ch = vx/v;
+		sh = vy/v;
+	} else {
+		const double heading = self->prior.x.heading;
+		ch = cos(heading);
+		sh = sin(heading);
+	}
+	const double ch2 = ch * ch;
+	const double sh2 = sh * sh;
+	const double innovation_covariance =
+			ch2 * self->prior.s2x[X][VEL][VEL] + sh2 * self->prior.s2x[Y][VEL][VEL] + SQUARE(sigma_of_speed);
+	const double innovation = speed - v;
+	/* Update state estimate */
+	const double vxk = ch * innovation / innovation_covariance;
+	self->posterior.x.p[X] = self->prior.x.p[X] + self->prior.s2x[X][POS][VEL] * vxk;
+	self->posterior.x.v[X] = self->prior.x.v[X] + self->prior.s2x[X][VEL][VEL] * vxk;
+	self->posterior.x.a[X] = self->prior.x.a[X] + self->prior.s2x[X][ACC][VEL] * vxk;
+
+	const double vyk = sh * innovation / innovation_covariance;
+	self->posterior.x.p[Y] = self->prior.x.p[Y] + self->prior.s2x[Y][POS][VEL] * vyk;
+	self->posterior.x.v[Y] = self->prior.x.v[Y] + self->prior.s2x[Y][VEL][VEL] * vyk;
+	self->posterior.x.a[Y] = self->prior.x.a[Y] + self->prior.s2x[Y][ACC][VEL] * vyk;
+
+	self->posterior.x.p[Z] = self->prior.x.p[Z];
+	self->posterior.x.v[Z] = self->prior.x.v[Z];
+	self->posterior.x.a[Z] = self->prior.x.a[Z];
+
+	/* Update covariance estimate */
+	Vector_2d vv;
+	vv[0] = ch2 / innovation_covariance;
+	vv[1] = sh2 / innovation_covariance;
+	unsigned i;
+	for (i = PLANAR_DIMENSION; i;) {
+		--i;
+		const_Matrix_3d_ref prior_cov = self->prior.s2x[i];
+		Matrix_3d_ref post_cov = self->posterior.s2x[i];
+		unsigned s1;
+		for (s1 = STATE_DIMENSION; s1;) {
+			s1--;
+			unsigned s2;
+			for (s2 = s1 + 1; s2;) {
+				s2--;
+				post_cov[s2][s1] = post_cov[s1][s2] =
+						prior_cov[s1][s2] - prior_cov[s1][VEL] * prior_cov[VEL][s2] * vv[i];
+			}
+		}
+	}
+	matrix_3d_copy(self->posterior.s2x[Z], self->prior.s2x[Z]);
+	location_filter_update_heading(&self->posterior.x);
+	__copy_state(&self->prior, &self->posterior);
+	self->last_correction_time = time;
+}
+
+void location_filter_clear_acc(LocationFilter* self, double time)
+{
+	unsigned i;
+	for (i = GEO_DIMENSION; i--;) {
+		self->posterior.x.a[i] = 0;
+		self->posterior.s2x[i][ACC][ACC] = self->default_var_a;
+	}
+
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+
+	location_filter_predict(self, time);
+}
+
+void location_filter_update_acc(LocationFilter* self, double time)
+{
+	location_filter_predict(self, time);
+	self->posterior.s2x[X][ACC][ACC] = fl_square(self->prior.x.a[X] - self->posterior.x.a[X]);
+	self->posterior.s2x[Y][ACC][ACC] = fl_square(self->prior.x.a[Y] - self->posterior.x.a[Y]);
+	__copy_state(&self->posterior, &self->prior);
+	self->last_correction_time = time;
+
+	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+}
diff --git a/lbs-server/src/fused/LocationFilter.h b/lbs-server/src/fused/LocationFilter.h
new file mode 100644
index 0000000..1c82a1d
--- /dev/null
+++ b/lbs-server/src/fused/LocationFilter.h
@@ -0,0 +1,135 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    LocationFilter.h
+ * @brief   Implementation of a 4D (2D position + 2D velocity) Kalman filter
+ */
+
+#pragma once
+#ifndef __LOCATION_FILTER_H_
+#define __LOCATION_FILTER_H_
+
+#include "parameters.h"
+#include "Vector.h"
+#include "Matrix.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+	POS,			/* position     [m]     */
+	VEL,			/* velocity     [m/s]   */
+	ACC,			/* acceleration [m/s^2] */
+	STATE_DIMENSION
+} _fl_kalf_state_component;
+
+typedef struct {
+	Vector_3d p;    /* Position */
+	Vector_3d v;    /* Velocity */
+	Vector_3d a;    /* Acceleration */
+	double heading;	/* Heading */
+} _fl_kalf_state_m;
+
+typedef            Matrix_3d  _fl_kalf_sigma_cube[GEO_DIMENSION];
+typedef            Matrix_3d(*_fl_kalf_sigma_cube_ref);
+typedef const Matrix_3d* const_fl_kalf_sigma_cube_ref;
+
+typedef struct {
+	_fl_kalf_state_m x;			/* state vector */
+	_fl_kalf_sigma_cube s2x;	/* covariance matrix */
+} _fl_kalf_state;
+
+typedef struct {
+	double default_var_a;
+    _fl_kalf_state prior;			/* predicted (a priori) state */
+    _fl_kalf_state posterior;		/* corrected (a posteriori) state */
+    double last_correction_time;	/* update of posterior */
+    double z_rot_speed;			/* reference angular z-velocity (horizontal plane) */
+} LocationFilter;
+
+/**
+ * @brief Initialization of the filter (constructor).
+ * @param[in] x  X coordinate of the initial position.
+ * @param[in] y  Y coordinate of the initial position.
+ * @param[in] z  Z coordinate of the initial position.
+ * @param[in] var_p  Initial variance of position.
+ * @param[in] var_v  Initial variance of speed.
+ * @param[in] var_a  Initial variance of acceleration.
+ */
+extern void location_filter_init(LocationFilter* self,
+		double x, double y, double z, double var_p, double var_v, double var_a);
+
+/**
+ * @brief Sets z rotation speed.
+ * @param[in] z_rot_speed  Z rotation speed in [rad/sec].
+ */
+#define location_filter_set_z_rot_speed(self, _z_rot_speed) ((self)->z_rot_speed = (_z_rot_speed))
+
+extern void location_filter_update_state(LocationFilter* self);
+
+/**
+ * @brief Planar (2D) Kalman blending of position.
+ * @param[in] time      Internal time.
+ * @param[in] pm        Measured position.
+ * @param[in] s2pm      Position variances.
+ */
+extern void location_filter_kalman_p(LocationFilter* self,
+		double time, const_Vector_3d_ref pm, const_Vector_3d_ref s2pm);
+
+/**
+ * @brief Spatial (3D) Kalman blending of position and velocity.
+ * @param[in] time      Internal time.
+ * @param[in] pm        Measured position.
+ * @param[in] vm        Measured velocity.
+ * @param[in] s2pm      Position variances.
+ * @param[in] s2vm      Velocity variances.
+ */
+extern void location_filter_kalman_pv(LocationFilter* self, double time,
+		const_Vector_3d_ref pm, const_Vector_3d_ref vm, const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm);
+
+/**
+ * @brief Kalman blending of speed.
+ * @param[in] time                Internal time.
+ * @param[in] speed               Measured speed.
+ * @param[in] sigma_of_speed      Variance of speed.
+ */
+extern void location_filter_update_speed(LocationFilter* self, double time, double speed, double sigma_of_speed);
+
+extern void location_filter_clear_acc(LocationFilter* self, double time);
+extern void location_filter_update_acc(LocationFilter* self, double time);
+
+/** (Vector_3d_ref) Returns current position estimate */
+#define location_filter_get_position(self) ((self)->posterior.x.p)
+
+/** (Vector_3d_ref) Returns current velocity estimate */
+#define location_filter_get_velocity(self) ((self)->posterior.x.v)
+
+/** (const_fl_kalf_sigma_cube_ref) */
+#define location_filter_get_predicted_covariance(self) ((self)->prior.s2x)
+
+/** (const_Vector_3d_ref) Returns predicted position estimate */
+#define location_filter_get_predicted_position(self) ((self)->prior.x.p)
+
+/** (const_Vector_3d_ref) Returns predicted velocity estimate */
+#define location_filter_get_predicted_velocity(self) ((self)->prior.x.v)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __LOCATION_FILTER_H_ */
diff --git a/lbs-server/src/fused/Matrix.c b/lbs-server/src/fused/Matrix.c
new file mode 100644
index 0000000..d7f5be2
--- /dev/null
+++ b/lbs-server/src/fused/Matrix.c
@@ -0,0 +1,156 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "Matrix.h"
+#include "math-ext.h"
+
+void matrix_3d_transpose(Matrix_3d_ref self)
+{
+	int i, j;
+
+	for (i = GEO_DIMENSION - 1; i >= 0; i--) {
+		for (j = i - 1; j >= 0; j--) {
+			double v = self[i][j];
+			self[i][j] = self[j][i];
+			self[j][i] = v;
+		}
+	}
+}
+
+void matrix_3d_multiply_matrix_3d(const_Matrix_3d_ref m1, const_Matrix_3d_ref m2, Matrix_3d_ref out)
+{
+	int i, j;
+
+	for (i = GEO_DIMENSION - 1; i >= 0; i--) {
+		for (j = GEO_DIMENSION - 1; j >= 0; j--) {
+			out[i][j] = m1[i][X] * m2[X][j] + m1[i][Y] * m2[Y][j] + m1[i][Z] * m2[Z][j];
+		}
+	}
+}
+
+void matrix_3d_multiply_vector_3d(const_Matrix_3d_ref m, const_Vector_3d_ref v, Vector_3d_ref out)
+{
+	out[X] = vector_3d_dot_product(m[X], v);
+	out[Y] = vector_3d_dot_product(m[Y], v);
+	out[Z] = vector_3d_dot_product(m[Z], v);
+}
+
+void vector_3d_multiply_matrix_3d(const_Vector_3d_ref v, const_Matrix_3d_ref m, Vector_3d_ref out)
+{
+	out[X] = m[X][X] * v[X] + m[Y][X] * v[Y] + m[Z][X] * v[Z];
+	out[Y] = m[X][Y] * v[X] + m[Y][Y] * v[Y] + m[Z][Y] * v[Z];
+	out[Z] = m[X][Z] * v[X] + m[Y][Z] * v[Y] + m[Z][Z] * v[Z];
+}
+
+void matrix_3d_from_vector_3d(const_Vector_3d_ref p, Matrix_3d_ref out,
+		double* lat, double* lon, double* sin_lat, double* cos_lat)
+{
+	double cos_rx, sin_rx;
+	double cos_ry, sin_ry;
+	double yz2 = fl_square(p[Y]) + fl_square(p[Z]);
+	double r2s = sqrt(yz2 + fl_square(p[X]));
+	if (r2s > 0) {
+		double _r = 1.0 / r2s;
+		double yz = sqrt(yz2);
+
+		*lat = atan2(p[X], yz);
+		*sin_lat = sin_rx = _r * p[X];
+		*cos_lat = cos_rx = _r * yz;
+		if (yz > 0) {
+			double _yz = 1.0 / yz;
+
+			*lon = atan2(p[Y], p[Z]);
+			sin_ry = _yz * p[Y];
+			cos_ry = _yz * p[Z];
+		} else {
+			*lon = 0;
+			sin_ry = 0;
+			cos_ry = 1;
+		}
+	} else {
+		*lat = 0;
+		*lon = 0;
+		*sin_lat = sin_rx = 0;
+		*cos_lat = cos_rx = 1;
+		sin_ry = 0;
+		cos_ry = 1;
+	}
+	out[X][X] = cos_rx;
+	out[Y][X] = 0;
+	out[Z][X] = sin_rx;
+
+	out[X][Y] = -sin_ry * sin_rx;
+	out[Y][Y] = cos_ry;
+	out[Z][Y] = sin_ry * cos_rx;
+
+	out[X][Z] = -cos_ry * sin_rx;
+	out[Y][Z] = -sin_ry;
+	out[Z][Z] = cos_ry * cos_rx;
+}
+
+void matrix_2d_multiply_matrix_2d(const_Matrix_2d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out)
+{
+	int i, j;
+
+	for (i = PLANAR_DIMENSION - 1; i >= 0; i--) {
+		for (j = PLANAR_DIMENSION - 1; j >= 0; j--) {
+			out[i][j] = m1[i][PLANAR_X] * m2[PLANAR_X][j] + m1[i][PLANAR_Y] * m2[PLANAR_Y][j];
+		}
+	}
+}
+
+void matrix_3d_multiply_matrix_2d(const_Matrix_3d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out)
+{
+	int i, j;
+
+	for (i = PLANAR_DIMENSION - 1; i >= 0; i--) {
+		for (j = PLANAR_DIMENSION - 1; j >= 0; j--) {
+			out[i][j] = m1[i][PLANAR_X] * m2[PLANAR_X][j] + m1[i][PLANAR_Y] * m2[PLANAR_Y][j];
+		}
+	}
+}
+
+void matrix_2d_multiply_vector_2d(const_Matrix_2d_ref m, const_Vector_2d_ref v, Vector_2d_ref out)
+{
+	out[PLANAR_X] = vector_2d_dot_product(m[PLANAR_X], v);
+	out[PLANAR_Y] = vector_2d_dot_product(m[PLANAR_Y], v);
+}
+
+void vector_2d_multiply_matrix_2d(const_Vector_2d_ref v, const_Matrix_2d_ref m, Vector_2d_ref out)
+{
+	out[PLANAR_X] = m[PLANAR_X][PLANAR_X] * v[PLANAR_X] + m[PLANAR_Y][PLANAR_X] * v[PLANAR_Y];
+	out[PLANAR_Y] = m[PLANAR_X][PLANAR_Y] * v[PLANAR_X] + m[PLANAR_Y][PLANAR_Y] * v[PLANAR_Y];
+}
+
+double matrix_2d_det(const_Matrix_2d_ref self)
+{
+	return self[PLANAR_X][PLANAR_X] * self[PLANAR_Y][PLANAR_Y] - self[PLANAR_Y][PLANAR_X] * self[PLANAR_X][PLANAR_Y];
+}
+
+bool matrix_2d_invert(const_Matrix_2d_ref self, Matrix_2d_ref out)
+{
+	const double det = matrix_2d_det(self);
+	if (det == 0) {
+		return false;
+	}
+
+	const double denom = 1.0 / det;
+	out[PLANAR_X][PLANAR_X] = self[PLANAR_Y][PLANAR_Y] * denom;
+	out[PLANAR_X][PLANAR_Y] = -self[PLANAR_Y][PLANAR_X] * denom;
+	out[PLANAR_Y][PLANAR_X] = -self[PLANAR_X][PLANAR_Y] * denom;
+	out[PLANAR_Y][PLANAR_Y] = self[PLANAR_X][PLANAR_X] * denom;
+	return true;
+}
diff --git a/lbs-server/src/fused/Matrix.h b/lbs-server/src/fused/Matrix.h
new file mode 100644
index 0000000..b86d911
--- /dev/null
+++ b/lbs-server/src/fused/Matrix.h
@@ -0,0 +1,169 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    Matrix.h
+ * @brief   matrix
+ */
+
+#pragma once
+#ifndef __MATRIX_H_
+#define __MATRIX_H_
+
+#include "Vector.h"
+#include <stdbool.h>
+#include <memory.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef             double  Matrix_3d[GEO_DIMENSION][GEO_DIMENSION];
+typedef             double(*Matrix_3d_ref)[GEO_DIMENSION];
+typedef const double(*const_Matrix_3d_ref)[GEO_DIMENSION];
+
+typedef             double  Matrix_2d[PLANAR_DIMENSION][PLANAR_DIMENSION];
+typedef             double(*Matrix_2d_ref)[PLANAR_DIMENSION];
+typedef const double(*const_Matrix_2d_ref)[PLANAR_DIMENSION];
+
+/**
+ * @brief Clears content of provided 3D matrix.
+ * @param[in] self  3D matrix to clear.
+ */
+#define matrix_3d_clear(self) memset(self, 0, sizeof(Matrix_3d))
+
+/**
+ * @brief Copy content of one 3D matrix to another one.
+ * @param[in] dst Destination 3D matrix.
+ * @param[in] src Source 3D matrix.
+ */
+#define matrix_3d_copy(dst, src) memcpy(dst, src, sizeof(Matrix_3d))
+
+/**
+ * @brief Transposition of provided 3D matrix.
+ * @param[in] self 3D matrix to transpose.
+ */
+extern void matrix_3d_transpose(Matrix_3d_ref self);
+
+/**
+ * @brief Multiplies two 3D matrixes.
+ * @param[in] m1 First 3D matrix.
+ * @param[in] m2 Second 3D matrix.
+ * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2
+ */
+extern void matrix_3d_multiply_matrix_3d(const_Matrix_3d_ref m1, const_Matrix_3d_ref m2, Matrix_3d_ref out);
+
+/**
+ * @brief Multiplies 3D matrix with 3D vector.
+ * @param[in] m 3D matrix.
+ * @param[in] v 3D vector.
+ * @param[out] out Multiplication of provided matrix and vector, i.e. out = m * v
+ */
+extern void matrix_3d_multiply_vector_3d(const_Matrix_3d_ref m, const_Vector_3d_ref v, Vector_3d_ref out);
+
+/**
+ * @brief Multiplies 3D vector with 3D matrix.
+ * @param[in] v 3D vector.
+ * @param[in] m 3D matrix.
+ * @param[out] out Multiplication of provided vector and matrix, i.e. out = v * m
+ */
+extern void vector_3d_multiply_matrix_3d(const_Vector_3d_ref v, const_Matrix_3d_ref m, Vector_3d_ref out);
+
+extern void matrix_3d_from_vector_3d(const_Vector_3d_ref p, Matrix_3d_ref out,
+		double *lat, double *lon, double* sin_lat, double* cos_lat);
+
+/**
+ * @brief Inlined left-action of a linear operator on a 3D covector.
+ * @param[in] m 3x3D matrix.
+ * @param[in] v 3D vector.
+ * @param[out] out Left-action of the linear operator @a m on the vector @a v, i.e. out = m * v
+ */
+#define MATRIX_DOT_VECTOR_3D(m, v, out)\
+	do {\
+		(out)[X] = (m)[X][X] * (v)[X] + (m)[X][Y] * (v)[Y] + (m)[X][Z] * (v)[Z];\
+		(out)[Y] = (m)[Y][X] * (v)[X] + (m)[Y][Y] * (v)[Y] + (m)[Y][Z] * (v)[Z];\
+		(out)[Z] = (m)[Z][X] * (v)[X] + (m)[Z][Y] * (v)[Y] + (m)[Z][Z] * (v)[Z];\
+	} while (0)
+
+/**
+ * @brief Inlined right-action of a linear operator on a 3D.
+ *        In Euclidean metric this is the same as left action of the transposed operator on the vector.
+ * @param[in] v 3D vector.
+ * @param[in] m 3x3D matrix.
+ * @param[out] out Right-action of the linear operator @a m on the covector @a v, i.e. out = v * m
+ */
+#define VECTOR_DOT_MATRIX_3D(v, m, out)\
+	do {\
+		(out)[X] = (v)[X] * (m)[X][X] + (v)[Y] * (m)[Y][X] + (v)[Z] * (m)[Z][X];\
+		(out)[Y] = (v)[X] * (m)[X][Y] + (v)[Y] * (m)[Y][Y] + (v)[Z] * (m)[Z][Y];\
+		(out)[Z] = (v)[X] * (m)[X][Z] + (v)[Y] * (m)[Y][Z] + (v)[Z] * (m)[Z][Z];\
+	} while (0)
+
+/**
+ * @brief Multiplies two 2D matrixes.
+ * @param[in] m1 First 2D matrix.
+ * @param[in] m2 Second 2D matrix.
+ * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2
+ */
+extern void matrix_2d_multiply_matrix_2d(const_Matrix_2d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out);
+
+/**
+ * @brief Multiplies submatrix of 3D matrix with 2D matrixes.
+ * @param[in] m1 3D matrix. For multiplication 2D matrix consisting of first 2 rows and first 2 columns is taken.
+ * @param[in] m2 2D matrix.
+ * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2
+ */
+extern void matrix_3d_multiply_matrix_2d(const_Matrix_3d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out);
+
+/**
+ * @brief Multiplies 2D matrix with 2D vector.
+ * @param[in] m 2D matrix.
+ * @param[in] v 2D vector.
+ * @param[out] out Multiplication of provided matrix and vector, i.e. out = m * v
+ */
+extern void matrix_2d_multiply_vector_2d(const_Matrix_2d_ref m, const_Vector_2d_ref v, Vector_2d_ref out);
+
+/**
+ * @brief Multiplies 2D vector with 2D matrix.
+ * @param[in] v 2D vector.
+ * @param[in] m 2D matrix.
+ * @param[out] out Multiplication of provided vector and matrix, i.e. out = v * m
+ */
+extern void vector_2d_multiply_matrix_2d(const_Vector_2d_ref v, const_Matrix_2d_ref m, Vector_2d_ref out);
+
+/**
+ * @brief Determinant of 2D matrix.
+ * @param[in] m 2D matrix.
+ * @return Determinant of the matrix.
+ */
+extern double matrix_2d_det(const_Matrix_2d_ref self);
+
+/**
+ * @brief Inverts 2D matrix.
+ * @param[in] m 2D matrix.
+ * @param[out] out Inverted matrix. Not modified if result of a function is false (determinant is 0).
+ * @return
+ *  - true - If inverted matrix was calculated.
+ *  - false - If matrix can not be inverted (determinant is 0).
+ */
+extern bool matrix_2d_invert(const_Matrix_2d_ref m, Matrix_2d_ref out);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MATRIX_H_ */
diff --git a/lbs-server/src/fused/MotionDetector.c b/lbs-server/src/fused/MotionDetector.c
new file mode 100644
index 0000000..d018ab4
--- /dev/null
+++ b/lbs-server/src/fused/MotionDetector.c
@@ -0,0 +1,108 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#if !defined(LOG_THRESHOLD)
+	/* custom logging threshold - keep undefined on the repo */
+	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
+#endif
+
+#include "MotionDetector.h"
+#include "math-ext.h"
+#include "debug_util.h"
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+#if (FL_MOTION_DETECTOR)
+
+void motion_detector_init(MotionDetector* self, MotionDetectorStateChangeListener on_motion_state_changed)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(motion_cb=%p)"), on_motion_state_changed);
+	Aema1dEstimator_init(&self->moti_al2, 1 << FL_MOTI_AEMA_PLATEAU_BITS);
+	self->moti_immobile_time           = 0;
+	self->moti_state                   = MOTI_UNDECIDED;
+	self->moti_last_notification       = MOTI_UNDECIDED;
+	self->moti_on_motion_state_changed = on_motion_state_changed;
+}
+
+void motion_detector_exit(MotionDetector* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+	self->moti_on_motion_state_changed = NULL;
+	self->moti_last_notification       = MOTI_UNDECIDED;
+	self->moti_state                   = MOTI_UNDECIDED;
+	self->moti_immobile_time           = 0;
+	Aema1dEstimator_exit(&self->moti_al2);
+}
+
+void motion_detector_reset(MotionDetector* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
+	self->moti_immobile_time     = 0;
+	self->moti_state             = MOTI_UNDECIDED;
+	self->moti_last_notification = MOTI_UNDECIDED;
+	Aema1dEstimator_reset(&self->moti_al2);
+}
+
+/**
+ * @brief Change the state indicator and notify the user.
+ * @param[in] state New state value (does not have to be different than present one).
+ */
+static void motion_detector_set_state(MotionDetector* self, MotionDetectorState state)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%u->%u)"), self->moti_state, state);
+	self->moti_state = state;
+	if (self->moti_on_motion_state_changed && self->moti_last_notification != state) {
+		self->moti_on_motion_state_changed(state);
+		self->moti_last_notification = state;
+	}
+}
+
+void motion_detector_process(MotionDetector* self, double t, const_Vector_3d_ref al)
+{
+	double al2 = Aema1dEstimator_process(&self->moti_al2, vector_3d_length2(al));
+	if (al2 > SQUARE(FL_MOTI_MOTION_LEVEL)) {
+		switch (self->moti_state) {
+		case MOTI_UNDECIDED:
+		case MOTI_IMMOBILE:
+		case MOTI_SLEEP:
+			MotionDetector_set_state(self, MOTI_MOVING);
+			break;
+
+		default: /* MOTI_MOVING */
+			break;
+		}
+	} else if (al2 < SQUARE(FL_MOTI_NOISE_LEVEL)) {
+		switch (self->moti_state) {
+		case MOTI_UNDECIDED:
+		case MOTI_MOVING:
+			MotionDetector_set_state(self, MOTI_IMMOBILE);
+			self->moti_immobile_time = t;
+			break;
+
+		case MOTI_IMMOBILE:
+			if (t - self->moti_immobile_time > FL_MOTI_IMMOBILITY_INTERVAL)
+				MotionDetector_set_state(self, MOTI_SLEEP);
+			break;
+
+		default:
+		/* MOTI_SLEEP: */
+			break;
+		}
+	}
+}
+
+#endif
diff --git a/lbs-server/src/fused/MotionDetector.h b/lbs-server/src/fused/MotionDetector.h
new file mode 100644
index 0000000..a6990ef
--- /dev/null
+++ b/lbs-server/src/fused/MotionDetector.h
@@ -0,0 +1,98 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    MotionDetector.h
+ * @brief   Accelerometer-based detector of significant motion.
+ */
+
+#pragma once
+#ifndef __MOTION_DETECTOR_H__
+#define __MOTION_DETECTOR_H__
+
+#include "MovingAverageFilters.h"
+#include "Vector.h"
+#include "parameters.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+	MOTI_UNDECIDED, /* initial */
+	MOTI_MOVING,    /* acceleration above MOTION_LEVEL  */
+	MOTI_IMMOBILE,  /* acceleration below NOISE_LEVEL   */
+	MOTI_SLEEP,     /* immobile for IMMOBILITY_INTERVAL */
+} MotionDetectorState;
+
+/**
+ * @brief Callback invoked on every change in motion state.
+ * @param[in] state   New motion state.
+ * @param[in] handler Arbitrary pointer to user data specified during the init() call.
+ */
+typedef void (*MotionDetectorStateChangeListener)(MotionDetectorState state);
+
+typedef struct {
+	MovingAverage1dFilter moti_al2;				/**< Linear acceleration low-pass filter */
+	double moti_immobile_time;					/**< Start time of immobility */
+	MotionDetectorState moti_state;				/**< Current state */
+	MotionDetectorState moti_last_notification;	/**< Last notified state */
+	MotionDetectorStateChangeListener moti_on_motion_state_changed;		/**< User callback */
+} MotionDetector;
+
+#if (FL_MOTION_DETECTOR)
+
+/**
+ * @brief Initialization of the singleton unit (static constructor).
+ * @param[in] on_motion_state_changed  Callback to be invoked when the detected state of motion changes.
+ * @param[in] handler  Arbitrary user parameter passed to the callback.
+ */
+void motion_detector_init(MotionDetector* self, MotionDetectorStateChangeListener on_motion_state_changed);
+
+/** Cleanup of the singleton unit (static destructor) */
+void motion_detector_exit(MotionDetector* self);
+
+/**
+ * @brief Reset of the internal state back to initial one. This function is
+ *        used for repetitive testing and module soft restarts.
+ */
+void motion_detector_reset(MotionDetector* self);
+
+/**
+ * @biref Processing of a single sample from the accelerometer. This function
+ * implments a hysteresis in the 2D space of linear acceleration modulus |al|^2
+ * vs motion state:
+ *  - Crossing the FL_MOTI_MOTION_LEVEL from below changes state to MOVING;
+ *  - Crossing the FL_MOTI_NOISE_LEVEL from above changes state to IMMOBILE;
+ *  - Staying IMMOBILE for longer than FL_MOTI_IMMOBILITY_INTERVAL changes state to SLEEP.
+ *
+ * @param[in] time  Time of the event in seconds.
+ * @param[in] al  3D vector of linear acceleration.
+ */
+void motion_detector_process(MotionDetector* self, double time, const_Vector_3d_ref al);
+
+#else
+	#define motion_detector_init(self, on_motion_state_changed)
+	#define motion_detector_exit(self)
+	#define motion_detector_reset(self)
+	#define motion_detector_process(self, t, al)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MOTION_DETECTOR_H__ */
diff --git a/lbs-server/src/fused/MovingAverage.c b/lbs-server/src/fused/MovingAverage.c
new file mode 100644
index 0000000..cbdb8ed
--- /dev/null
+++ b/lbs-server/src/fused/MovingAverage.c
@@ -0,0 +1,56 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "MovingAverage.h"
+#include "debug_util.h"
+
+#include <math.h>
+
+void moving_average_init(MovingAverage* self, unsigned plateau_samples)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](bits=%u)"), self, plateau_samples);
+	self->plateau_samples = plateau_samples;
+	moving_average_reset(self);
+}
+
+void moving_average_exit(MovingAverage *self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self);
+}
+
+void moving_average_reset(MovingAverage* self)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self);
+	self->samples     = 0;
+	self->update_rate = 1;
+	self->decay_rate  = 0;
+}
+
+void moving_average_process(MovingAverage *self)
+{
+	if (self->samples < self->plateau_samples) {
+		self->samples++;
+		self->update_rate = 1.0 / self->samples;
+		self->decay_rate  = 1.0 - self->update_rate;
+	}
+}
+
+void moving_average_set_estimate(MovingAverage* self)
+{
+	self->samples     = self->plateau_samples;
+	self->update_rate = 1.0 / self->plateau_samples;
+	self->decay_rate  = 1.0 - self->update_rate;
+}
diff --git a/lbs-server/src/fused/MovingAverage.h b/lbs-server/src/fused/MovingAverage.h
new file mode 100644
index 0000000..ab52fc4
--- /dev/null
+++ b/lbs-server/src/fused/MovingAverage.h
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    MovingAverage.h
+ * @brief   Adaptive Exponential Moving Average (1st order) filter
+ */
+
+#pragma once
+#ifndef __MOVING_AVERAGE_H__
+#define __MOVING_AVERAGE_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+	unsigned plateau_samples; /* number of samples to collect before EMA kicks-in */
+	unsigned samples;         /* min(number of samples collected, plateau_samples) */
+	double   update_rate;     /* (1 / samples) */
+	double   decay_rate;      /* (1 - update_rate) */
+} MovingAverage;
+
+/**
+ * @brief Initialization of the object (class constructor).
+ * @param[in] self                 Object pointer
+ * @param[in] plateau_samples      Number of samples to collect before switching to standard EMA mode.
+ */
+extern void moving_average_init(MovingAverage *self, unsigned plateau_samples);
+
+/** Cleanup of the base class (destructor) */
+extern void moving_average_exit(MovingAverage *self);
+
+/**
+ * @brief Reset the AEMA object back to initial state.
+ *        This function is used for repetitive testing and module soft restarts.
+ */
+extern void moving_average_reset(MovingAverage *self);
+
+/**
+ * @brief Processing of a data sample. This is the common part of the
+ * derived processing methods, which implements the AEMA update/decay rate.
+ */
+extern void moving_average_process(MovingAverage *self);
+
+/** Bring the base filter to a stationary state (EMA mode). This method is used for testing */
+extern void moving_average_set_estimate(MovingAverage* self);
+
+/** (double) */
+#define moving_average_get_update_rate(self) ((self)->update_rate)
+
+/** (double) */
+#define moving_average_get_decay_rate(self) ((self)->decay_rate)
+
+/** void moving_average_filter(MovingAverage* self, double* f, double v) */
+#define moving_average_filter(self, f, v) ((f) = (self)->decay_rate * (f) + (self)->update_rate * (v))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MOVING_AVERAGE_H__ */
diff --git a/lbs-server/src/fused/MovingAverageFilters.c b/lbs-server/src/fused/MovingAverageFilters.c
new file mode 100644
index 0000000..a5d9df2
--- /dev/null
+++ b/lbs-server/src/fused/MovingAverageFilters.c
@@ -0,0 +1,127 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "MovingAverageFilters.h"
+#include "debug_util.h"
+
+/* Labels for an axial (or cylindrical without phase) 2D space. */
+typedef enum {
+	AXIAL_H,
+	AXIAL_V,
+	AXIAL_DIMENSION
+} fl_axial;
+
+void moving_average_1d_filter_init(MovingAverage1dFilter* self, unsigned plateau_samples)
+{
+	moving_average_init(&self->base, plateau_samples);
+	self->value = 0;
+}
+
+void moving_average_1d_filter_exit(MovingAverage1dFilter *self)
+{
+	moving_average_exit(&self->base);
+}
+
+void moving_average_1d_filter_reset(MovingAverage1dFilter* self)
+{
+	moving_average_reset(&self->base);
+	self->value = 0;
+}
+
+double moving_average_1d_filter_process(MovingAverage1dFilter* self, double value)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value);
+	moving_average_process(&self->base);
+	moving_average_filter(&self->base, self->value, value);
+	return self->value;
+}
+
+void moving_average_1d_filter_set_estimate(MovingAverage1dFilter* self, double value)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value);
+	moving_average_set_estimate(&self->base);
+	self->value = value;
+}
+
+void moving_average_2d_filter_init(MovingAverage2dFilter* self, unsigned plateau_samples)
+{
+	moving_average_init(&self->base, plateau_samples);
+	self->value[AXIAL_H] = 0;
+	self->value[AXIAL_V] = 0;
+}
+
+void moving_average_2d_filter_exit(MovingAverage2dFilter *self)
+{
+	moving_average_exit(&self->base);
+}
+
+void moving_average_2d_filter_reset(MovingAverage2dFilter* self)
+{
+	moving_average_reset(&self->base);
+	self->value[PLANAR_X] = 0;
+	self->value[PLANAR_Y] = 0;
+}
+
+const_Vector_2d_ref moving_average_2d_filter_process(MovingAverage2dFilter* self, double value_H, double value_V)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V);
+	moving_average_process(&self->base);
+	moving_average_filter(&self->base, self->value[AXIAL_H], value_H);
+	moving_average_filter(&self->base, self->value[AXIAL_V], value_V);
+	return self->value;
+}
+
+void moving_average_2d_filter_set_estimate(MovingAverage2dFilter* self, double value_H, double value_V)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V);
+	moving_average_set_estimate(&self->base);
+	self->value[AXIAL_H] = value_H;
+	self->value[AXIAL_V] = value_V;
+}
+
+void moving_average_3d_filter_init(MovingAverage3dFilter* self, unsigned plateau_samples)
+{
+	moving_average_init(&self->base, plateau_samples);
+	vector_3d_clear(self->value);
+}
+
+void moving_average_3d_filter_exit(MovingAverage3dFilter *self)
+{
+	moving_average_exit(&self->base);
+}
+
+void moving_average_3d_filter_reset(MovingAverage3dFilter* self)
+{
+	moving_average_reset(&self->base);
+	vector_3d_clear(self->value);
+}
+
+const_Vector_3d_ref moving_average_3d_filter_process(MovingAverage3dFilter* self, const_Vector_3d_ref value)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[X], value[Y], value[Z]);
+	moving_average_process(&self->base);
+	moving_average_filter(&self->base, self->value[X], value[X]);
+	moving_average_filter(&self->base, self->value[Y], value[Y]);
+	moving_average_filter(&self->base, self->value[Z], value[Z]);
+	return self->value;
+}
+
+void moving_average_3d_filter_set_estimate(MovingAverage3dFilter* self, const_Vector_3d_ref value)
+{
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[X], value[Y], value[Z]);
+	moving_average_set_estimate(&self->base);
+	vector_3d_copy(self->value, value);
+}
diff --git a/lbs-server/src/fused/MovingAverageFilters.h b/lbs-server/src/fused/MovingAverageFilters.h
new file mode 100644
index 0000000..7794fe0
--- /dev/null
+++ b/lbs-server/src/fused/MovingAverageFilters.h
@@ -0,0 +1,156 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    MovingAverageFilters.h
+ * @brief   Moving Average (1st order) filters
+ */
+
+#pragma once
+#ifndef __MOVING_AVERAGE_1D_FILTER_H__
+#define __MOVING_AVERAGE_1D_FILTER_H__
+
+#include "MovingAverage.h"
+#include "Vector.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/** Moving Average (1st order) 1D filter */
+typedef struct {
+	MovingAverage base;   /* base object */
+	double        value;  /* filter output (estimated value) */
+} MovingAverage1dFilter;
+
+/**
+ * @brief Initialization of the 1D AEMA object (class constructor).
+ * @param[in] self  Object pointer
+ * @param[in] plateau_samples  Number of samples to collect before switching to standard EMA mode.
+ */
+void moving_average_1d_filter_init(MovingAverage1dFilter *self, unsigned plateau_samples);
+void moving_average_1d_filter_exit(MovingAverage1dFilter *self);
+void moving_average_1d_filter_reset(MovingAverage1dFilter *self);
+
+/**
+ * @brief Processing of a single data sample.
+ * @param[in] self  Object pointer
+ * @param[in] value  Input value to the filter.
+ * @return Output value of the filter after processing.
+ *         This is the same as returned by fl_aema1d_get_estimate(self).
+ */
+double moving_average_1d_filter_process(MovingAverage1dFilter *self, double value);
+
+/**
+ * @brief [TEST] Bring the filter to a stationary state (EMA mode, input = output).
+ * @param[in] self  Object pointer
+ * @param[in] value  I/o value of the filter.
+ */
+void moving_average_1d_filter_set_estimate(MovingAverage1dFilter *self, double value);
+
+/**
+ * @brief Fetch the current filter output.
+ * @param[in] self  Object pointer
+ * @return  Output value after last processed sample.
+ */
+#define moving_average_1d_filter_get_estimate(self) ((self)->value)
+
+
+/** Moving Average (1st order) 2D filter */
+typedef struct {
+	MovingAverage base;   /* base object */
+	Vector_2d     value;  /* filter output (estimated value) */
+} MovingAverage2dFilter;
+
+/**
+ * @brief Initialization of the 2D AEMA object (class constructor).
+ * @param[in] self  Object pointer
+ * @param[in] plateau_samples  Number of samples to collect before switching to standard EMA mode.
+ */
+void moving_average_2d_filter_init(MovingAverage2dFilter *self, unsigned plateau_samples);
+void moving_average_2d_filter_exit(MovingAverage2dFilter *self);
+void moving_average_2d_filter_reset(MovingAverage2dFilter *self);
+
+/**
+ * @brief Processing of a double data sample.
+ * @param[in] self  Object pointer
+ * @param[in] value_H  First (horizontal) input value to the filter.
+ * @param[in] value_V  Second (vertical) input value to the filter.
+ * @return Output value of the filter after processing.
+ *         This is the same as returned by fl_aema2d_get_estimate(self).
+ */
+const_Vector_2d_ref moving_average_2d_filter_process(MovingAverage2dFilter *self, double value_X, double value_Y);
+
+/**
+ * @brief [TEST] Bring the filter to a stationary state (EMA mode, input = output).
+ * @param[in] self  Object pointer
+ * @param[in] value_H  First (horizontal) i/o value of the filter.
+ * @param[in] value_V  Second (vertical) i/o value of the filter.
+ */
+void moving_average_2d_filter_set_estimate(MovingAverage2dFilter *self, double value_X, double value_Y);
+
+/**
+ * @brief (const_Vector_2d_ref) Fetch the current filter output
+ * @param[in] self  Object pointer
+ * @return  Output 2D vector after last processed sample.
+ */
+#define moving_average_2d_filter_get_estimate(self) ((self)->value)
+
+
+/** Moving Average (1st order) 3D filter */
+typedef struct {
+	MovingAverage base;   /* base object */
+	Vector_3d     value;  /* filter output (estimated value) */
+} MovingAverage3dFilter;
+
+/**
+ * @brief Initialization of the 3D AEMA object (class constructor).
+ * @param[in] self  Object pointer
+ * @param[in] plateau_samples  Number of samples to collect before switching to standard EMA mode.
+ */
+void moving_average_3d_filter_init(MovingAverage3dFilter *self, unsigned plateau_samples);
+void moving_average_3d_filter_exit(MovingAverage3dFilter *self);
+void moving_average_3d_filter_reset(MovingAverage3dFilter *self);
+
+/**
+ * @brief Processing of a triple data sample.
+ * @param[in] self  Object pointer
+ * @param[in] value  Input 3D vector value to the filter.
+ * @return Output value of the filter after processing.
+ *         This is the same as returned by fl_aema3d_get_estimate(self).
+ */
+const_Vector_3d_ref moving_average_3d_filter_process(MovingAverage3dFilter *self, const_Vector_3d_ref value);
+
+/**
+ * @brief [TEST]Bring the filter to a stationary state (EMA mode, input = output).
+ * @param[in] self  Object pointer
+ * @param[in] value  I/o 3D vector value of the filter.
+ */
+void moving_average_3d_filter_set_estimate(MovingAverage3dFilter *self, const_Vector_3d_ref value);
+
+/**
+ * (const_Vector_3d_ref) Fetch the current filter output.
+ * @param[in] self  Object pointer
+ * @return  Output 3D vector after last processed sample.
+ */
+#define moving_average_3d_filter_get_estimate(self) ((self)->value)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MOVING_AVERAGE_1D_FILTER_H__ */
diff --git a/lbs-server/src/fused/Orientation.c b/lbs-server/src/fused/Orientation.c
new file mode 100644
index 0000000..44a80c4
--- /dev/null
+++ b/lbs-server/src/fused/Orientation.c
@@ -0,0 +1,388 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "Orientation.h"
+#include "Conversions.h"
+#include "debug_util.h"
+#include "parameters.h"
+
+#define DEFAULT_ROTATION_SIGMA2          SQUARE(1.0 / 256) /* [(rad/s)^2] */
+#define TIME_FORMAT                     "%.3fs"
+
+#if (FL_ENABLE_DEVEL_LOG)
+static const Vector_3d gravity = {0, 0, 1};
+#endif
+
+void orientation_init(Orientation* self,
+		MotionDetectorStateChangeListener on_motion_state_changed, double rot_var)
+{
+	accelerometer_filter_init(&self->acc_filter, on_motion_state_changed);
+	rotation_filter_init(&self->rotation_filter, rot_var);
+	orientation_reset(self);
+}
+
+void orientation_exit(Orientation* self)
+{
+	accelerometer_filter_exit(&self->acc_filter);
+}
+
+void orientation_reset(Orientation* self)
+{
+	matrix_3d_clear(&self->rotation);
+	unsigned i;
+	for (i = GEO_DIMENSION; i;) {
+		--i;
+		self->rotation[i][i] = 1;
+	}
+	self->last_angular_velocity_time = FL_UNDEFINED_TIME;
+	self->weight2 = M_1_PI / 3;
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+	rotation_filter_reset(&self->rotation_filter);
+#else
+	self->z_rot_speed = 0;
+#endif
+}
+
+bool orientation_new_angular_velocity(Orientation* self, double time, const_Vector_3d_ref angular_velocity, double s2wm)
+{
+	bool result = false;
+	if (KNOWN_TIME(self->last_angular_velocity_time)) {
+		const double dt = time - self->last_angular_velocity_time;
+
+		if (dt > 0) {
+			Vector_3d mean_av;
+			vector_3d_linear_combination(0.5, self->last_angular_velocity, 0.5, angular_velocity, mean_av);
+			const double z_rot_speed = vector_3d_dot_product(self->rotation[Z], mean_av);
+
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+			rotation_filter_new_rot_speed(&self->rotation_filter, time, z_rot_speed, SQUARE(FL_DEFAULT_SPIN_SIGMA));
+#else
+			self->z_rot_speed = z_rot_speed;
+#endif
+			result = true;
+			LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, dt=%.16e, wz=%.16e, wze=%.16e, m0=%.16e, m1=%.16e, m2=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"),
+					time, dt, z_rot_speed, orientation_get_z_rot_speed(self),
+					mean_av[0], mean_av[1], mean_av[2], self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]);
+
+			if (vector_3d_length(mean_av) > 0) {
+				/* Integrate over dt: We use the fact that for any scalar t the quaternions commute.
+				 *   [exp(w), exp(tw)] = 0
+				 */
+				Quaternion qwt;
+				quaternion_from_rotation_vector(qwt, mean_av, dt);
+				LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e"),
+						time, qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z]);
+
+				Vector_3d v;
+				vector_3d_cross_vector_3d(self->last_angular_velocity, angular_velocity, v);
+				const double dt2 = dt * dt / 24;
+				qwt[QTR_X] += dt2 * v[X];
+				qwt[QTR_Y] += dt2 * v[Y];
+				qwt[QTR_Z] += dt2 * v[Z];
+				LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, v0=%.16e, v1=%.16e, v2=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e, ql=%.16e"),
+						time, v[0], v[1], v[2], qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z], quaternion_norm(qwt));
+
+				quaternion_normalize(qwt);
+				LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e"),
+						time, qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z]);
+
+				Matrix_3d dsr;
+				quaternion_to_rotation_matrix(qwt, dsr);
+				LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, r00=%.16e, r01=%.16e, r02=%.16e, r10=%.16e, r11=%.16e, r12=%.16e, r20=%.16e, r21=%.16e, r22=%.16e"),
+						time, dsr[0][0], dsr[0][1], dsr[0][2], dsr[1][0], dsr[1][1], dsr[1][2], dsr[2][0], dsr[2][1], dsr[2][2]);
+
+				Matrix_3d new_rot;
+				matrix_3d_multiply_matrix_3d(self->rotation, dsr, new_rot);
+				matrix_3d_copy(self->rotation, new_rot);
+				LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"), time, self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]);
+			}
+		}
+	}
+	vector_3d_copy(self->last_angular_velocity, angular_velocity);
+	self->last_angular_velocity_time = time;
+	return  result;
+}
+
+/**
+ * @brief Extrapolate the system rotation (__sp) to the given time assuming constant angular velocity.
+ * @param[in] t  Internal time
+ */
+static void orientation_predict_rw(Orientation* self, double time)
+{
+	if (KNOWN_TIME(self->last_angular_velocity_time)) {
+		const double dt = time - self->last_angular_velocity_time;
+
+		if (dt > 0) {
+			const double w = vector_3d_length(self->last_angular_velocity);
+
+			if (w > 0) {
+				Quaternion qwt;
+				quaternion_from_rotation_vector(qwt, self->last_angular_velocity, dt);
+				Matrix_3d dsr;
+				quaternion_to_rotation_matrix(qwt, dsr);
+				Matrix_3d new_rot;
+				matrix_3d_multiply_matrix_3d(self->rotation, dsr, new_rot);
+				matrix_3d_copy(self->rotation, new_rot);
+			}
+		}
+	}
+}
+
+void orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acc)
+{
+	orientation_predict_rw(self, time);
+	Vector_3d gacc;		/* Acceleration in global (tangent) frame */
+	matrix_3d_multiply_vector_3d(self->rotation, acc, gacc);
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.16e, acc0=%.16e, acc1=%.16e, acc2=%.16e, gacc0=%.16e, gacc1=%.16e, gacc2=%.16e, angle=%.16e"),
+					time, acc[0], acc[1], acc[2], gacc[0], gacc[1], gacc[2], vector_3d_angle(gacc, gravity));
+	Vector_3d gu;
+	vector_3d_copy(gu, gacc);
+
+#if (FL_ENABLE_DEVEL_LOG)
+	Vector_3d gn; /* Gravity in device frame */
+	/* rotate back to device frame */
+	VECTOR_DOT_MATRIX_3D(gu, self->rotation, gn);
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.16e, gn0=%.16e, gn1=%.16e, gn2=%.16e"),
+			time, gn[0], gn[1], gn[2]);
+#endif
+	/* update rotation matrix */
+	Matrix_3d new_rot;
+	double accLen = vector_3d_length(gu);
+	if (accLen > 0) {
+		Quaternion q;
+		q[QTR_0] = accLen + gu[Z];
+		q[QTR_X] = gu[Y];
+		q[QTR_Y] = -gu[X];
+		q[QTR_Z] = 0;
+		quaternion_normalize(q);
+		Vector_3d rot_vect;
+		quaternion_get_rotation_vector(q, rot_vect);
+		quaternion_from_rotation_vector(q, rot_vect, 0.01);
+		Matrix_3d m;
+		quaternion_to_rotation_matrix(q, m);
+		matrix_3d_transpose(m);
+		matrix_3d_multiply_matrix_3d(m, self->rotation, new_rot);
+		matrix_3d_copy(self->rotation, new_rot);
+	}
+
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), time=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"),
+			time, self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]);
+}
+
+void orientation_get_orientation(Orientation* self, double* o)
+{
+	Quaternion q;
+	quaternion_from_rotation_matrix(q, self->rotation);
+	if (q[QTR_0] < 0) {
+		o[0] = q[QTR_X];
+		o[1] = q[QTR_Y];
+		o[2] = q[QTR_Z];
+	} else {
+		o[0] = -q[QTR_X];
+		o[1] = -q[QTR_Y];
+		o[2] = -q[QTR_Z];
+	}
+}
+
+void quaternion_normalize(Quaternion_ref self)
+{
+	double qwtn = quaternion_norm(self);
+	if (qwtn > 0) {
+		double norm_qwt = 1.0 / qwtn;
+
+		self[QTR_0] *= norm_qwt;
+		self[QTR_X] *= norm_qwt;
+		self[QTR_Y] *= norm_qwt;
+		self[QTR_Z] *= norm_qwt;
+	}
+}
+
+double quaternion_get_rotation_angle(const_Quaternion_ref self)
+{
+	return 2 * acos(fabs(self[QTR_0]));
+}
+
+void quaternion_from_rotation_vector(Quaternion_ref self, const_Vector_3d_ref rot_vector, double scale)
+{
+	double w = vector_3d_length(rot_vector);
+	if (w > 0) {
+		double wt = w * scale * 0.5;
+		double swt = sin(wt) / w;
+		self[QTR_0] = cos(wt);
+		self[QTR_X] = swt * rot_vector[X];
+		self[QTR_Y] = swt * rot_vector[Y];
+		self[QTR_Z] = swt * rot_vector[Z];
+	} else {
+		self[QTR_0] = 1;
+		self[QTR_X] = 0;
+		self[QTR_Y] = 0;
+		self[QTR_Z] = 0;
+	}
+}
+
+void quaternion_get_rotation_vector(const_Quaternion_ref self, Vector_3d_ref rot)
+{
+	double length = sqrt(SQUARE(self[QTR_X]) + SQUARE(self[QTR_Y]) + SQUARE(self[QTR_Z]));
+
+	if (length == 0) {
+		vector_3d_clear(rot);
+	} else {
+		double l = self[QTR_0] >= 0
+				? quaternion_get_rotation_angle(self) / length
+				: -quaternion_get_rotation_angle(self) / length;
+
+		vector_3d_set(rot, self[QTR_X] * l, self[QTR_Y] * l, self[QTR_Z] * l);
+	}
+}
+
+void quaternion_to_rotation_matrix(const_Quaternion_ref self, Matrix_3d_ref rot_matrix)
+{
+	double xx = 2 * self[QTR_X] * self[QTR_X];
+	double xy = 2 * self[QTR_X] * self[QTR_Y];
+	double xz = 2 * self[QTR_X] * self[QTR_Z];
+	double xw = 2 * self[QTR_X] * self[QTR_0];
+	double yy = 2 * self[QTR_Y] * self[QTR_Y];
+	double yz = 2 * self[QTR_Y] * self[QTR_Z];
+	double yw = 2 * self[QTR_Y] * self[QTR_0];
+	double zz = 2 * self[QTR_Z] * self[QTR_Z];
+	double zw = 2 * self[QTR_Z] * self[QTR_0];
+	rot_matrix[X][X] = 1 - yy - zz;
+	rot_matrix[X][Y] = xy + zw;
+	rot_matrix[X][Z] = xz - yw;
+
+	rot_matrix[Y][X] = xy - zw;
+	rot_matrix[Y][Y] = 1 - xx - zz;
+	rot_matrix[Y][Z] = yz + xw;
+
+	rot_matrix[Z][X] = xz + yw;
+	rot_matrix[Z][Y] = yz - xw;
+	rot_matrix[Z][Z] = 1 - xx - yy;
+}
+
+void quaternion_from_rotation_matrix(Quaternion_ref self, const_Matrix_3d_ref m)
+{
+	double qw;
+	double qx;
+	double qy;
+	double qz;
+	double tr = m[0][0] + m[1][1] + m[2][2];
+
+	if (tr > 0) {
+		double s = sqrt(tr + 1.0) * 2; /* s = 4 * qw */
+		qw = 0.25 * s;
+		qx = (m[2][1] - m[1][2]) / s;
+		qy = (m[0][2] - m[2][0]) / s;
+		qz = (m[1][0] - m[0][1]) / s;
+	} else if ((m[0][0] > m[1][1]) & (m[0][0] > m[2][2])) {
+		double s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2; /* s = 4 * qx */
+		qw = (m[2][1] - m[1][2]) / s;
+		qx = 0.25 * s;
+		qy = (m[0][1] + m[1][0]) / s;
+		qz = (m[0][2] + m[2][0]) / s;
+	} else if (m[1][1] > m[2][2]) {
+		double s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2; /* s = 4 * qy */
+		qw = (m[0][2] - m[2][0]) / s;
+		qx = (m[0][1] + m[1][0]) / s;
+		qy = 0.25 * s;
+		qz = (m[1][2] + m[2][1]) / s;
+	} else {
+		double s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2; /* s = 4 * qz */
+		qw = (m[1][0] - m[0][1]) / s;
+		qx = (m[0][2] + m[2][0]) / s;
+		qy = (m[1][2] + m[2][1]) / s;
+		qz = 0.25 * s;
+	}
+
+	double n = sqrt(qw * qw + qx * qx + qy * qy + qz * qz);
+	if (qw >= 0) {
+		self[QTR_0] = qw / n;
+		self[QTR_X] = -qx / n;
+		self[QTR_Y] = -qy / n;
+		self[QTR_Z] = -qz / n;
+	} else {
+		self[QTR_0] = -qw / n;
+		self[QTR_X] = qx / n;
+		self[QTR_Y] = qy / n;
+		self[QTR_Z] = qz / n;
+	}
+}
+
+void rotation_filter_init(RotationFilter* self, double rot_var)
+{
+	self->def_rot_var = rot_var;
+	rotation_filter_reset(self);
+}
+
+void rotation_filter_reset(RotationFilter* self)
+{
+	self->rot_speed = 0;
+	self->rot_speed_variance = self->def_rot_var;
+	self->rot_speed_time = FL_UNDEFINED_TIME;
+	self->last_azimuth = 0;
+	self->last_azimuth_time = FL_UNDEFINED_TIME;
+}
+
+void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot_speed, double rot_speed_variance)
+{
+	if (KNOWN_TIME(self->rot_speed_time)) {
+		const double dt = time - self->rot_speed_time;
+
+		/* propagate/diffuse sigma^2 */
+		const double s2wzp = self->rot_speed_variance + dt * SQUARE(FL_DEFAULT_SPIN_SIGMA);
+		const double s2wzk = s2wzp + rot_speed_variance;
+		if (s2wzk > 0) {
+			const double inv_s2k = 1.0 / s2wzk;
+			LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=" TIME_FORMAT ", w=%.4g, s2w=%.4g): __wze:%g->%g, __s2wze:%g->%g"),
+						time, rot_speed, rot_speed_variance, self->rot_speed,
+						inv_s2k * (s2wzp * rot_speed + rot_speed_variance * self->rot_speed),
+						self->rot_speed_variance, inv_s2k * s2wzp * rot_speed_variance * 2);
+
+			self->rot_speed = inv_s2k * (s2wzp * rot_speed + rot_speed_variance * self->rot_speed);
+			self->rot_speed_variance = inv_s2k * s2wzp * rot_speed_variance * 2;
+		} else {
+			self->rot_speed = rot_speed;
+			self->rot_speed_variance = rot_speed_variance;
+		}
+	} else {
+		self->rot_speed = rot_speed;
+		self->rot_speed_variance = rot_speed_variance;
+	}
+
+	self->rot_speed_time = time;
+}
+
+void rotation_filter_new_azimuth(RotationFilter* self, double time, double azimuth, double vhe, double speed)
+{
+	if (speed > 0 && KNOWN_TIME(self->last_azimuth_time)) {
+		const double dt = time - self->last_azimuth_time;
+
+		if (speed * vhe * dt > 0) {
+			/* circular difference */
+			double db = azimuth - self->last_azimuth;
+			if (db > M_PI) {
+				db -= M_2PI;
+			} else if (db < -M_PI) {
+				db += M_2PI;
+			}
+			const double rot_speed = db / dt;
+			const double rot_speed_variance = 4 * fl_square(atan2(2 * sqrt(speed * vhe), FL_DEFAULT_VELOCITY_SIGMA));
+			rotation_filter_new_rot_speed(self, time, rot_speed, rot_speed_variance);
+		}
+	}
+
+	self->last_azimuth = azimuth;
+	self->last_azimuth_time = time;
+}
diff --git a/lbs-server/src/fused/Orientation.h b/lbs-server/src/fused/Orientation.h
new file mode 100644
index 0000000..5d33972
--- /dev/null
+++ b/lbs-server/src/fused/Orientation.h
@@ -0,0 +1,177 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    Orientation.h
+ * @brief   Orientation filter.
+ */
+
+#pragma once
+#ifndef __ORIENTATION_H_
+#define __ORIENTATION_H_
+
+#include <stdbool.h>
+#include "Vector.h"
+#include "Matrix.h"
+#include "math-ext.h"
+#include "AccelerometerFilter.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef struct {
+	double def_rot_var;        /* Initial variance of z rotation speed. */
+	double rot_speed;          /* Angular z rotation speed (horizontal plane). */
+	double rot_speed_variance; /* Z rotation speed variance. */
+	double rot_speed_time;     /* Time of last z rotation speed update. */
+	double last_azimuth;       /* Last reported azimuth (North bearing). */
+	double last_azimuth_time;  /* Time of last azimuth update. */
+} RotationFilter;
+
+typedef struct {
+	Matrix_3d rotation;                /* Current rotation matrix */
+	double weight2;                    /* Square weight (inverse covariance) vector of matrix rows */
+    Vector_3d last_angular_velocity;   /* Last reported angular velocity. */
+	double last_angular_velocity_time; /* Time when last angular velocity was reported. */
+	AccelerometerFilter acc_filter;    /* Accelerometer filter. */
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+	RotationFilter rotation_filter;    /* Filters z-rotation speed. */
+#else
+	double z_rot_speed;                /* Angular z rotation speed (horizontal plane). */
+#endif
+} Orientation;
+
+extern void orientation_init(Orientation* self,
+		MotionDetectorStateChangeListener on_motion_state_changed, double var_rot);
+
+extern void orientation_exit(Orientation* self);
+extern void orientation_reset(Orientation* self);
+
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+
+/** (double) */
+#define orientation_get_z_rot_speed(self) rotation_filter_get_rot_speed(&(self)->rotation_filter)
+
+#define orientation_new_azimuth(self, time, azimuth, vhe, speed) rotation_filter_new_azimuth(&(self)->rotation_filter, time, azimuth, vhe, speed)
+
+#else
+
+/** (double) */
+#define orientation_get_z_rot_speed(self) ((self)->z_rot_speed)
+
+#endif
+
+/**
+ * @brief Update system rotation matrix given a measurement of the device angular velocity.
+ * @param[in] t  Internal time of the measurement
+ * @param[in] wm  3D vector of angular velocity [rad/s] in device coordinate frame
+ * @param[in] s2wm  Estimated variance of the measuremnt
+ */
+extern bool orientation_new_angular_velocity(Orientation* self,
+		double time, const_Vector_3d_ref angular_velocity, double s2wm);
+
+extern void orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acceleration);
+
+extern void orientation_get_orientation(Orientation* self, double* o);
+
+/** Labels for quaternion components. */
+typedef enum {
+	QTR_X,
+	QTR_Y,
+	QTR_Z,
+	QTR_0,
+	QUATERNION_DIMENSION
+} _quaternion_component;
+
+typedef       double        Quaternion[QUATERNION_DIMENSION];
+typedef       double*       Quaternion_ref;
+typedef const double* const_Quaternion_ref;
+
+/**
+ * @brief Returns quaternion norm.
+ * @param[in] self  Input quaternion.
+ * @return  (double) Norm of quaternion.
+ */
+#define quaternion_norm(self) sqrt(SQUARE(self[QTR_0]) + SQUARE(self[QTR_X]) + SQUARE(self[QTR_Y]) + SQUARE(self[QTR_Z]))
+
+/** Normalizes quaternion */
+extern void quaternion_normalize(Quaternion_ref self);
+
+/**
+ * @brief Returns rotation angle assuming quaternion represents rotation.
+ * @param[in] self  Input quaternion. Must be normed.
+ */
+extern double quaternion_get_rotation_angle(const_Quaternion_ref self);
+
+/**
+ * @brief Calculates quaternion representing rotation multiplied by some number.
+ * @param[out] self  Output quaternion.
+ * @param[in] rot_vector  Rotation vector.
+ * @param[in] scale  Scale to multiply rotation vector.
+ */
+extern void quaternion_from_rotation_vector(Quaternion_ref self, const_Vector_3d_ref rot_vector, double scale);
+
+/**
+ * @brief Returns rotation vector assuming quaternion represents rotation.
+ * @param[in] self  Input quaternion. Must be normed.
+ * @param[out] rot  Rotation vector representing same rotation as input quaternion.
+ */
+extern void quaternion_get_rotation_vector(const_Quaternion_ref self, Vector_3d_ref rot);
+
+/**
+ * @brief Maps a quaternion into system rotation matrix assuming quaternion represents rotation.
+ * @param[in] self  Input quaternion. Must be normed.
+ * @param[out] rot_matrix  Corresponding system rotation 3D matrix
+ */
+extern void quaternion_to_rotation_matrix(const_Quaternion_ref self, Matrix_3d_ref rot_matrix);
+
+/**
+ * @brief Creates quaternion representing rotation from given rotation matrix.
+ * @param m  3D rotation matrix.
+ * @return  Normed quaternion representing rotation with nonnegative scalar part.
+ */
+extern void quaternion_from_rotation_matrix(Quaternion_ref self, const_Matrix_3d_ref m);
+
+/**
+ * @brief Initialization of the filter (constructor).
+ * @param[in] self  Object pointer
+ * @param[in] rot_var  Default rotation variance.
+ */
+extern void rotation_filter_init(RotationFilter* self, double rot_var);
+
+/** Resets filter to initial state */
+extern void rotation_filter_reset(RotationFilter* self);
+
+/**
+ * @brief Axial (1D) Kalman blending of angular velocity along one axis (turn)
+ * @param[in] time  Internal time
+ * @param[in] rot_speed  Angular velocity projection on the axis.
+ * @param[in] rot_speed_variance  Corresponding variance.
+ */
+extern void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot_speed, double rot_speed_variance);
+
+extern void rotation_filter_new_azimuth(RotationFilter* self, double time, double azimuth, double vhe, double speed);
+
+/** (double) Returns current rotation speed along axis */
+#define rotation_filter_get_rot_speed(self) ((self)->rot_speed)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __ORIENTATION_H_ */
diff --git a/lbs-server/src/fused/PeaceDetector.c b/lbs-server/src/fused/PeaceDetector.c
new file mode 100644
index 0000000..a0c2c96
--- /dev/null
+++ b/lbs-server/src/fused/PeaceDetector.c
@@ -0,0 +1,57 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "PeaceDetector.h"
+#include "Conversions.h"
+
+void peace_detector_init(PeaceDetector *self, double stable_level, long stable_number)
+{
+	self->stable_level = stable_level;
+	self->stable_number = stable_number;
+	peace_detector_reset(self);
+}
+
+void peace_detector_reset(PeaceDetector *self)
+{
+	self->stable_counter = self->stable_number;
+	self->last_timestamp = FL_UNDEFINED_TIME;
+}
+
+bool peace_detector_is_stable(PeaceDetector *self)
+{
+	return self->stable_counter <= 0;
+}
+
+bool peace_detector_new_data(PeaceDetector *self, double timestamp, const_Vector_3d_ref data)
+{
+	if ((!KNOWN_TIME(self->last_timestamp)) || self->last_timestamp > timestamp) {
+		self->stable_counter = self->stable_number;
+	} else {
+		const double length = vector_3d_distance(self->previous_measurement, data);
+
+		if (length > self->stable_level) {
+			self->stable_counter = self->stable_counter < 0 ? 0 : self->stable_number;
+		} else {
+			self->stable_counter = self->stable_counter > 1 ? self->stable_counter - 1 : -1;
+		}
+	}
+	self->last_timestamp = timestamp;
+	if (!peace_detector_is_stable(self)) {
+		vector_3d_copy(self->previous_measurement, data);
+		return false;
+	}
+	return true;
+}
diff --git a/lbs-server/src/fused/PeaceDetector.h b/lbs-server/src/fused/PeaceDetector.h
new file mode 100644
index 0000000..92cf06f
--- /dev/null
+++ b/lbs-server/src/fused/PeaceDetector.h
@@ -0,0 +1,38 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#pragma once
+#ifndef __PEACEDETECTOR_H_
+#define __PEACEDETECTOR_H_
+
+#include "Vector.h"
+#include <stdbool.h>
+
+
+typedef struct {
+	double stable_level;			/* Noise level below which the data are treated as stable. */
+	long stable_number;				/* Number of stable measurements to treat data as stable. */
+	long stable_counter;			/* Down counter of stable data. < 0 means stable state. */
+	Vector_3d previous_measurement;	/* Previous measurement. */
+	double last_timestamp;			/* Timestamp of last measurement. */
+} PeaceDetector;
+
+extern void peace_detector_init(PeaceDetector *self, double stable_level, long stable_number);
+extern void peace_detector_reset(PeaceDetector *self);
+extern bool peace_detector_is_stable(PeaceDetector *self);
+extern bool peace_detector_new_data(PeaceDetector *self, double timestamp, const_Vector_3d_ref data);
+
+#endif /* __PEACEDETECTOR_H_ */
diff --git a/lbs-server/src/fused/TangentFrame.c b/lbs-server/src/fused/TangentFrame.c
new file mode 100644
index 0000000..c042897
--- /dev/null
+++ b/lbs-server/src/fused/TangentFrame.c
@@ -0,0 +1,125 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "TangentFrame.h"
+#include "Conversions.h"
+
+#include <memory.h>
+
+#define globalToTangent(tf, g, t) matrix_3d_multiply_vector_3d((tf)->fsr, g, t)
+#define tangentToGlobal(tf, t, g) vector_3d_multiply_matrix_3d(t, (tf)->fsr, g)
+
+void earth_global_to_spherical(const_Vector_3d_ref p, double* latitude, double* longitude, double* altitude)
+{
+	const double yz2 = fl_square(p[Y]) + fl_square(p[Z]);
+	const double ry = atan2(p[Y], p[Z]);
+	*longitude = conversions_radians_to_longitude(ry);
+
+	const double xyz2 = yz2 + fl_square(p[X]);
+	const double rx = atan2(p[X], sqrt(yz2));
+	*latitude = conversions_radians_to_latitude(rx);
+
+	*altitude = sqrt(xyz2) - EARTH_RADIUS;
+}
+
+void earth_spherical_to_global(double latitude, double longitude, double radius, Vector_3d_ref p)
+{
+	const double rx = conversions_degrees_to_radians(latitude);
+	const double ry = conversions_degrees_to_radians(longitude);
+	const double sin_rx = radius * sin(rx);
+	const double cos_rx = radius * cos(rx);
+	vector_3d_set(p, sin_rx, cos_rx * sin(ry), cos_rx * cos(ry));
+}
+
+void tangent_frame_init(TangentFrame* self)
+{
+	memset(self, 0, sizeof(*self));
+	self->fsr[0][0] = 1;
+	self->fsr[1][1] = 1;
+	self->fsr[2][2] = 1;
+	self->cos_lat = 1;
+}
+
+void tangent_frame_create(TangentFrame* self, Vector_3d_ref pos, Vector_3d_ref vel)
+{
+	Vector_3d p;
+	tangentToGlobal(self, pos, p);
+	Vector_3d v;
+	tangentToGlobal(self, vel, v);
+
+	matrix_3d_from_vector_3d(p, self->fsr, &self->lat, &self->lon, &self->sin_lat, &self->cos_lat);
+
+	globalToTangent(self, p, pos);
+	globalToTangent(self, v, vel);
+}
+
+/**
+ * @brief Returns 2D vector in tangent coordinate system pointing from given 2D point to the North direction.
+ * @param[in] self      Tangent coordinate system.
+ * @param[in] p         2D point coordinates in tangent coordinate system.
+ * @param[out] u        2D vector pointing to the North in tangent coordinate system.
+ */
+static void tangent_frame_to_north(const TangentFrame* self, const_Vector_2d_ref p, Vector_2d_ref u)
+{
+	u[PLANAR_X] = EARTH_RADIUS * self->cos_lat - p[PLANAR_X] * self->sin_lat;
+	u[PLANAR_Y] =                              - p[PLANAR_Y] * self->sin_lat;
+}
+
+/**
+ * @brief Returns azimuth in tangent coordinate system from given 2D point.
+ * @param[in] self      Tangent coordinate system.
+ * @param[in] p         2D point coordinates in tangent coordinate system.
+ * @return              Azimuth from given 2D point, i.e angle in radians from North to the given point.
+ */
+static double tangent_frame_azimuth(const TangentFrame* self, const_Vector_2d_ref p)
+{
+	Vector_2d u;
+	tangent_frame_to_north(self, p, u);
+	return atan2(-u[PLANAR_Y], u[PLANAR_X]);
+}
+
+void tangent_frame_tangent_to_spherical(const TangentFrame* self, const_Vector_3d_ref pos,
+		double* latitude, double* longitude, double* altitude)
+{
+	Vector_3d p;
+	tangentToGlobal(self, pos, p);
+	earth_global_to_spherical(p, latitude, longitude, altitude);
+}
+
+void tangent_frame_spherical_to_tangent(const TangentFrame* self,
+		double latitude, double longitude, double radius, Vector_3d_ref pos)
+{
+	Vector_3d pg;
+	earth_spherical_to_global(latitude, longitude, radius, pg);
+	globalToTangent(self, pg, pos);
+}
+
+void tangent_frame_spherical_to_tangent_speed(const TangentFrame* self,
+		const_Vector_3d_ref pos, double speed, double azimuth, double climb, Vector_3d_ref v)
+{
+	double rb = conversions_degrees_to_radians(azimuth) - tangent_frame_azimuth(self, pos);
+	vector_3d_set(v, speed * cos(rb), speed * sin(rb), climb);
+}
+
+void tangent_frame_tangent_to_spherical_speed(const TangentFrame* self,
+		const_Vector_3d_ref pos, const_Vector_3d_ref vel, double* speed, double* direction, double* climb)
+{
+	Vector_2d to_north;
+	tangent_frame_to_north(self, pos, to_north);
+	*direction = conversions_radians_to_positive_degrees(-vector_2d_angle(vel, to_north));
+	*speed = vector_2d_length(vel);
+	*climb = vel[Z];
+}
diff --git a/lbs-server/src/fused/TangentFrame.h b/lbs-server/src/fused/TangentFrame.h
new file mode 100644
index 0000000..8a1103d
--- /dev/null
+++ b/lbs-server/src/fused/TangentFrame.h
@@ -0,0 +1,154 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    TangentFrame.h
+ * @brief   Tangent frame.
+ */
+
+#pragma once
+#ifndef __TANGENTFRAME_H_
+#define __TANGENTFRAME_H_
+
+#include "Matrix.h"
+#include "Vector.h"
+#include "math-ext.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Convert spherical coordinate in radians to arc-length along the Earth's great circle in meters.
+ * @param[in] radians  Angle spanned by the great arc.
+ * @return  (double) Corresponding value in meters along Earth's surface.
+ */
+#define earth_radians_to_meters(radians) ((radians) * EARTH_RADIUS)
+
+/**
+ * @brief Convert arc-length in meters along the Earth's great circle to arc angle in radians.
+ * @param[in] meters  Arc length in meters.
+ * @return	(double) Corresponding angle in radians.
+ */
+#define earth_meters_to_radians(meters) ((meters) * (1.0 / EARTH_RADIUS));
+
+/**
+ * @brief Convert point in global coordinates to spherical coordinates.
+ * @param[in] p  Represents coordinates of a point in global (Earth) coordinate system.
+ * @param[out] latitude	 Latitude of provided point.
+ * @param[out] longitude  Longitude of provided point.
+ * @param[out] altitude  Altitude of provided point.
+ */
+extern void earth_global_to_spherical(const_Vector_3d_ref p, double* latitude, double* longitude, double* altitude);
+
+/**
+ * @brief Convert point in spherical coordinates to global coordinates.
+ * @param[in] latitude	Latitude.
+ * @param[in] longitude	Longitude.
+ * @param[in] altitude	Altitude.
+ * @param[out] p  Represents coordinates of a point in global (Earth) coordinate system.
+ */
+extern void earth_spherical_to_global(double latitude, double longitude, double altitude, Vector_3d_ref p);
+
+
+
+/**
+ * Global frame coordinate system:
+ * - Center in the middle of the Earth.
+ * - OX directed to the North Pole (latitude = 90).
+ * - OY directed to the latitude = 0 and longitude = 90;
+ * - OZ directed to the latitude = 0 and longitude = 0;
+ */
+
+/**
+ * Tangent frame coordinate system:
+ * - OX directed to the North.
+ * - OY directed to the East;
+ * - OZ directed to the up;
+ */
+
+/** Tangent space T(p)E to the embedding manifold E=SxR at point p */
+typedef struct {
+	double lat;     /* latitude */
+	double lon;     /* longitude */
+	double sin_lat; /* sin(latitude) */
+	double cos_lat; /* cos(latitude) */
+	Matrix_3d fsr;  /* push-forward (tangent to global) coordinate system rotation matrix */
+} TangentFrame;
+
+extern void tangent_frame_init(TangentFrame* self);
+
+/**
+ * @brief Create coordinate map from spherical M = S^2 x R to the tangent space TM.
+ *        Notice that only two angles are being used, what means the course on the tangent plane
+ *        is not straight North (x-axis), but still described by the bearing.
+ * @param[in/out] self	Tangent coordinate system.
+ * @param[in/out] pos	Position in old tangent frame
+ * @param[in/out] vel	Velocity in old tangent frame
+ */
+extern void tangent_frame_create(TangentFrame* self, Vector_2d_ref pos, Vector_2d_ref vel);
+
+/**
+ * @brief Converts coordinates in tangent coordinate system to spherical coordinate system.
+ * @param[in] self  Tangent coordinate system.
+ * @param[in] pos  Coordinates of given point in tangent coordinate system.
+ * @param[out] latitude  Latitude in degrees.
+ * @param[out] longitude Longitude in degrees.
+ * @param[out] altitude  Altitude in meters.
+ */
+extern void tangent_frame_tangent_to_spherical(const TangentFrame* self,
+		const_Vector_3d_ref pos, double* latitude, double* longitude, double* altitude);
+
+/**
+ * @brief Converts coordinates in spherical coordinate system to tangent coordinate system.
+ * @param[in] self  Tangent coordinate system.
+ * @param[in] latitude  Latitude in degrees.
+ * @param[in] longitude	Longitude in degrees.
+ * @param[in] radius  Distance from the center of the Earth (altitude + Earth radius) in meters.
+ * @param[out] pos  Coordinates of given point in tangent coordinate system.
+ */
+extern void tangent_frame_spherical_to_tangent(const TangentFrame* self,
+		double latitude, double longitude, double radius, Vector_3d_ref pos);
+
+/**
+ * @brief Converts speed in spherical coordinate system to tangent coordinate system.
+ * @param[in] self		Tangent coordinate system.
+ * @param[in] pos		Coordinates of location in tangent coordinate system.
+ * @param[in] speed		Horizontal speed in m/s.
+ * @param[in] azimuth	Bearing (angle between true North and speed direction clockwise) in radians.
+ * @param[in] climb		Vertical speed in m/s (positive up).
+ * @param[out] v		Speed in given location in tangent coordinate system.
+ */
+extern void tangent_frame_spherical_to_tangent_speed(const TangentFrame* self,
+		const_Vector_3d_ref pos, double speed, double azimuth, double climb, Vector_3d_ref v);
+
+/**
+ * @brief Converts speed in tangent coordinate system to spherical coordinate system.
+ * @param[in] self		Tangent coordinate system.
+ * @param[in] pos		Coordinates of location in tangent coordinate system.
+ * @param[in] vel		Speed in given location in tangent coordinate system.
+ * @param[out] speed	Horizontal speed in m/s.
+ * @param[out] azimuth	Bearing (angle between true North and speed direction clockwise) in radians.
+ * @param[out] climb	Vertical speed in m/s (positive up).
+ */
+extern void tangent_frame_tangent_to_spherical_speed(const TangentFrame* self,
+		const_Vector_3d_ref pos, const_Vector_3d_ref vel, double* speed, double* azimuth, double* climb);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __TANGENTFRAME_H_ */
diff --git a/lbs-server/src/fused/TimeOffset.c b/lbs-server/src/fused/TimeOffset.c
new file mode 100644
index 0000000..87af4e3
--- /dev/null
+++ b/lbs-server/src/fused/TimeOffset.c
@@ -0,0 +1,87 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "TimeOffset.h"
+#include "Conversions.h"
+#include "parameters.h"
+
+void time_offset_init(TimeOffset* self)
+{
+	moving_average_1d_filter_init(&self->filter, 1 << FL_TIMO_AEMA_PLATEAU_BITS);
+	time_offset_reset(self);
+}
+
+void time_offset_reset(TimeOffset* self)
+{
+	moving_average_1d_filter_reset(&self->filter);
+	self->first_reference_time = FL_UNDEFINED_TIME;
+	self->last_reference_time = FL_UNDEFINED_TIME;
+	self->reference_count = 0;
+	self->correct_time = FL_UNDEFINED_TIME;
+}
+
+void time_offset_exit(TimeOffset* self)
+{
+	moving_average_1d_filter_exit(&self->filter);
+}
+
+static void time_offset_new(TimeOffset* self, double dt)
+{
+	moving_average_1d_filter_process(&self->filter, dt);
+}
+
+void time_offset_reference_time(TimeOffset* self, double time)
+{
+	if (self->reference_count == 0) {
+		self->first_reference_time = time;
+	}
+	self->last_reference_time = time;
+	self->reference_count++;
+
+	if (KNOWN_TIME(self->correct_time)) {
+		const double dt = self->last_reference_time - (self->correct_time - time_offset_get(self));
+		if (dt < 0) {
+			time_offset_new(self, self->correct_time - self->last_reference_time);
+		}
+	}
+}
+
+double time_offset_correct_time(TimeOffset* self, double time)
+{
+	self->correct_time = time;
+	if (KNOWN_TIME(self->last_reference_time)) {
+		double dt = self->correct_time - time_offset_get(self)
+				- self->last_reference_time;
+		if (dt < 0) {
+			time_offset_new(self, self->correct_time - self->last_reference_time);
+			dt = 0;
+		} else if (self->reference_count > 1) {
+			const double mean_sensor_dt = (self->last_reference_time - self->first_reference_time) / (self->reference_count - 1);
+			if (dt > mean_sensor_dt) {
+				time_offset_new(self, self->correct_time - self->last_reference_time - mean_sensor_dt);
+				dt = mean_sensor_dt;
+			}
+		}
+		return self->last_reference_time + dt;
+	}
+
+	return self->correct_time;
+}
+
+double time_offset_get(const TimeOffset* self)
+{
+	return moving_average_1d_filter_get_estimate(&self->filter);
+}
diff --git a/lbs-server/src/fused/TimeOffset.h b/lbs-server/src/fused/TimeOffset.h
new file mode 100644
index 0000000..4f74491
--- /dev/null
+++ b/lbs-server/src/fused/TimeOffset.h
@@ -0,0 +1,65 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    TimeOffset.h
+ * @brief   Time offset.
+ */
+
+#pragma once
+#ifndef __TIMEOFFSET_H_
+#define __TIMEOFFSET_H_
+
+#include "MovingAverageFilters.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef struct {
+	MovingAverage1dFilter filter;
+	double first_reference_time; /* Arrival time of the first sensor event. */
+	double last_reference_time;  /* Arrival time of the last sensor event. */
+	unsigned reference_count;    /* At 100Hz rate of arrival this will suffice for ~32 years. */
+	double correct_time;         /* Arrival time of the last position measurement event. */
+} TimeOffset;
+
+extern void time_offset_init(TimeOffset* self);
+extern void time_offset_reset(TimeOffset* self);
+extern void time_offset_exit(TimeOffset* self);
+
+/**
+ * @brief Supplies reference time
+ * @param[in] time  Reference time in [s].
+ */
+extern void time_offset_reference_time(TimeOffset* self, double time);
+
+/*
+ * @brief Convert location timestamp to internal time in seconds.
+ *        The time-offset is adjusted if necessary to ensure temporal ordering of events.
+ * @param[in] time  Location time
+ * @return          Internal time.
+ */
+extern double time_offset_correct_time(TimeOffset* self, double time);
+
+extern double time_offset_get(const TimeOffset* self);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __TIMEOFFSET_H_ */
diff --git a/lbs-server/src/fused/Vector.c b/lbs-server/src/fused/Vector.c
new file mode 100644
index 0000000..77afc60
--- /dev/null
+++ b/lbs-server/src/fused/Vector.c
@@ -0,0 +1,48 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "Vector.h"
+
+void vector_3d_normalization(Vector_3d_ref self)
+{
+	double len = vector_3d_length(self);
+	if (len > 0) {
+		double out = 1.0 / len;
+		self[X] *= out;
+		self[Y] *= out;
+		self[Z] *= out;
+	}
+}
+
+double vector_3d_distance(const_Vector_3d_ref p, const_Vector_3d_ref q)
+{
+	return sqrt(fl_square(p[X] - q[X]) + fl_square(p[Y] - q[Y]) + fl_square(p[Z] - q[Z]));
+}
+
+void vector_3d_linear_combination(double a1, const_Vector_3d_ref v1,
+		double a2, const_Vector_3d_ref v2, Vector_3d_ref v)
+{
+	v[X] = a1 * v1[X] + a2 * v2[X];
+	v[Y] = a1 * v1[Y] + a2 * v2[Y];
+	v[Z] = a1 * v1[Z] + a2 * v2[Z];
+}
+
+double vector_3d_angle(const_Vector_3d_ref v1, const_Vector_3d_ref v2)
+{
+	Vector_3d v;
+	vector_3d_cross_vector_3d(v1, v2, v);
+	return atan2(vector_3d_length(v), vector_3d_dot_product(v1, v2));
+}
diff --git a/lbs-server/src/fused/Vector.h b/lbs-server/src/fused/Vector.h
new file mode 100644
index 0000000..b375f8e
--- /dev/null
+++ b/lbs-server/src/fused/Vector.h
@@ -0,0 +1,201 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    Vector.h
+ * @brief   vector.
+ */
+
+#pragma once
+#ifndef __VECTOR_H_
+#define __VECTOR_H_
+
+#include "math-ext.h"
+
+#include <memory.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Labels for 3D vectors indexes. */
+typedef enum {
+	X,
+	Y,
+	Z,
+	GEO_DIMENSION
+} fl_geo_spatial;
+
+typedef       double        Vector_3d[GEO_DIMENSION];
+typedef       double*       Vector_3d_ref;
+typedef const double* const_Vector_3d_ref;
+
+
+#define vector_3d_clear(self) memset(self, 0, sizeof(Vector_3d))
+#define vector_3d_clear_array(self) memset(self, 0, sizeof(self))
+
+/**
+ * @brief  Inlined initialization of 3D vector.
+ * @param[in] v  3D vector
+ * @param[in] x  Initial x coordinate
+ * @param[in] y  Initial y coordinate
+ * @param[in] z  Initial z coordinate
+ */
+#define vector_3d_set(v, x, y, z)\
+	do {\
+		(v)[X] = x;\
+		(v)[Y] = y;\
+		(v)[Z] = z;\
+	} while (0)
+
+#define vector_3d_copy(dst, src) memcpy(dst, src, sizeof(Vector_3d))
+
+/**
+ * @brief Inlined difference of two 3D vectors.
+ * @param[in] u  3D vector
+ * @param[in] v  3D vector
+ * @param[out] out  Difference of a @u and a @v
+ */
+#define vector_3d_sub_vector_3d(u, v, out)\
+	do {\
+		(out)[X] = (u)[X] - (v)[X];\
+		(out)[Y] = (u)[Y] - (v)[Y];\
+		(out)[Z] = (u)[Z] - (v)[Z];\
+	} while (0)
+
+/**
+ * @brief Inlined cross-product of two 3D vectors.
+ * @param[in] u  3D vector
+ * @param[in] v  3D vector
+ * @param[out] out   Cross-product of a @u and a @v
+ */
+#define vector_3d_cross_vector_3d(u, v, out)\
+	do {\
+		(out)[X] = (u)[Y] * (v)[Z] - (u)[Z] * (v)[Y];\
+		(out)[Y] = (u)[Z] * (v)[X] - (u)[X] * (v)[Z];\
+		(out)[Z] = (u)[X] * (v)[Y] - (u)[Y] * (v)[X];\
+	} while (0)
+
+/**
+ * @brief Inlined product of 3D vector and number.
+ * @param[in] v  3D vector
+ * @param[in] d  Number
+ * @param[out] out  Cross-product of a @v and a @d
+ */
+#define vector_3d_mul_number(v, d, out)\
+	do {\
+		(out)[X] = (v)[X] * (d);\
+		(out)[Y] = (v)[Y] * (d);\
+		(out)[Z] = (v)[Z] * (d);\
+	} while (0)
+
+extern void vector_3d_normalization(Vector_3d_ref v);
+
+/**
+ * @brief Dot product of two 3D vectors.
+ * @param[in] u  3D vector
+ * @param[in] v  3D vector
+ * @return	     Scalar product of a @u and a @v.
+ */
+#define vector_3d_dot_product(u, v) ((u)[X] * (v)[X] + (u)[Y] * (v)[Y] + (u)[Z] * (v)[Z])
+
+/**
+ * @brief Squared length of a 3D vector.
+ * @param[in] x  3D vector
+ * @return       Squared length of @a x, i.e. |x|^2 = x * x
+ */
+#define vector_3d_length2(x) vector_3d_dot_product(x, x)
+
+/**
+ * @brief Length of a 3D vector.
+ * @param[in] x  3D vector
+ * @return       Length of @a x, i.e. |x| = sqrt(x * x)
+ */
+#define vector_3d_length(x) sqrt(vector_3d_length2(x))
+
+/**
+ * @brief Distance between two 3D vectors.
+ * @param[in] p  First 3D vector
+ * @param[in] q  Second 3D vector
+ * @return       Euclidean distance between vectors.
+ */
+extern double vector_3d_distance(const_Vector_3d_ref p, const_Vector_3d_ref q);
+
+extern void vector_3d_linear_combination(double a1, const_Vector_3d_ref v1,
+		double a2, const_Vector_3d_ref v2, Vector_3d_ref v);
+
+/**
+ * @brief Angle between two vectors.
+ * @param[in] v1  First 3D vector
+ * @param[in] v2  Second 3D vector
+ * @return        Angle between vectors in radians. Always in the range [0, PI].
+ */
+extern double vector_3d_angle(const_Vector_3d_ref v1, const_Vector_3d_ref v2);
+
+/** Labels for 2D vectors indexes. */
+typedef enum {
+	PLANAR_X,
+	PLANAR_Y,
+	PLANAR_DIMENSION
+} fl_planar;
+
+typedef       double        Vector_2d[PLANAR_DIMENSION];
+typedef       double*       Vector_2d_ref;
+typedef const double* const_Vector_2d_ref;
+
+/**
+ * @brief Dot product of two 2D vectors.
+ * @param[in] u  2D vector
+ * @param[in] v  2D vector
+ * @return       (double) Dot (scalar) product of a @u and a @v.
+ */
+#define vector_2d_dot_product(u, v) ((u)[PLANAR_X] * (v)[PLANAR_X] + (u)[PLANAR_Y] * (v)[PLANAR_Y])
+
+/**
+ * @brief Cross product of two 2D vectors.
+ * @param[in] u  2D vector
+ * @param[in] v  2D vector
+ * @return       (double) Cross product of a @u and a @u i.e. |u||v|sin(uv).
+ */
+#define vector_2d_cross_product(u, v) ((u)[PLANAR_X] * (v)[PLANAR_Y] - (u)[PLANAR_Y] * (v)[PLANAR_X])
+
+/**
+ * @brief Squared length of a 2D vector in Euclidean metric.
+ * @param[in] v  2D vector
+ * @return	     (double) Squared length of a @v, i.e. |v|^2 = v * v
+ */
+#define vector_2d_length2(v) vector_2d_dot_product(v, v)
+
+/**
+ * @brief Length of a 2D vector in Euclidean metric.
+ * @param[in] v  2D vector
+ * @return	     (double) Length of a @v, i.e. |v| = sqrt(v * v)
+ */
+#define vector_2d_length(v) sqrt(vector_2d_length2(v))
+
+/**
+ * @brief Angle between two vectors.
+ * @param[in] v1  First 2D vector
+ * @param[in] v2  Second 2D vector
+ * @return	      (double) Angle between vectors in radians.
+ */
+#define vector_2d_angle(v1, v2) atan2(vector_2d_cross_product(v1, v2), vector_2d_dot_product(v1, v2))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __VECTOR_H_ */
diff --git a/lbs-server/src/fused/Waema3dEstimator.c b/lbs-server/src/fused/Waema3dEstimator.c
new file mode 100644
index 0000000..853cce0
--- /dev/null
+++ b/lbs-server/src/fused/Waema3dEstimator.c
@@ -0,0 +1,59 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "Waema3dEstimator.h"
+#include "debug_util.h"
+
+void waema3d_estimator_reset(Waema3dEstimator* self, double alpha, double max_evidence, double ascale2)
+{
+	vector_3d_clear(self->wf);
+	vector_3d_clear(self->value);
+	self->max_evidence         = max_evidence;
+	self->ascale2              = ascale2;
+	self->alpha                = alpha;
+	self->scale2               = ascale2;
+	self->evidence             = 0;
+	self->weight_norm          = 1;
+}
+
+void waema3d_estimator_set(Waema3dEstimator* self, double scale2, double evidence, double weight_norm, const_Vector_3d_ref w0)
+{
+	self->scale2               = scale2;
+	self->evidence             = evidence;
+	self->weight_norm          = weight_norm;
+	vector_3d_copy(self->value, w0);
+}
+
+const_Vector_3d_ref waema3d_estimator_filter(Waema3dEstimator* self, const_Vector_3d_ref wm)
+{
+	const double fw2 = vector_3d_length2(self->wf);
+	const double weight = exp(-fw2 * self->scale2);
+	if (self->evidence < self->max_evidence) {
+		self->evidence += weight;
+		if (self->evidence > self->max_evidence) {
+			self->evidence = self->max_evidence;
+		}
+		self->weight_norm = 1.0 / self->evidence;
+		self->scale2 = self->ascale2 * (1.0 + fl_square(self->evidence) * self->alpha);
+	}
+	const double update_rate = weight * self->weight_norm;
+	const double decay_rate  = 1.0 - update_rate;
+	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("fw2=%g, e=%g, s2=%g, w=%g"),
+			fw2, self->evidence, self->scale2, weight);
+	vector_3d_linear_combination(decay_rate, self->value, update_rate, wm, self->value);
+	vector_3d_sub_vector_3d(wm, self->value, self->wf);
+	return self->wf;
+}
diff --git a/lbs-server/src/fused/Waema3dEstimator.h b/lbs-server/src/fused/Waema3dEstimator.h
new file mode 100644
index 0000000..5903450
--- /dev/null
+++ b/lbs-server/src/fused/Waema3dEstimator.h
@@ -0,0 +1,56 @@
+/*
+ *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+/**
+ * @file    Waema3dEstimator.h
+ * @brief   Weighted AEMA filter.
+ */
+
+#pragma once
+#ifndef __WAEMA3DESTIMATOR_H_
+#define __WAEMA3DESTIMATOR_H_
+
+#include "Vector.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Weighted AEMA filter. This is generalization of the 3D AEMA filter by
+ * using 'evidence' as the sum of weights rather than sample count.
+ * The sample weights are estimated using Gaussian curve with time-decreasing variance.
+ */
+typedef struct {
+	double    max_evidence;
+	double    ascale2;
+	double    alpha;
+	double    scale2;      /* = 1 / (2 variance) */
+	double    evidence;    /* min(accumulated weight, plateau_evidence) */
+	double    weight_norm; /* = 1 / evidence */
+	Vector_3d wf;
+	Vector_3d value;       /* filter output (estimated value) */
+} Waema3dEstimator;
+
+extern void waema3d_estimator_reset(Waema3dEstimator* self, double alpha, double aevidence, double ascale2);
+extern void waema3d_estimator_set(Waema3dEstimator* self, double scale2, double evidence, double weight_norm, const_Vector_3d_ref w0);
+extern const_Vector_3d_ref waema3d_estimator_filter(Waema3dEstimator* self, const_Vector_3d_ref wm);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __WAEMA3DESTIMATOR_H_ */
diff --git a/lbs-server/src/fused/accelerometer-filter.c b/lbs-server/src/fused/accelerometer-filter.c
deleted file mode 100644
index 4647bc2..0000000
--- a/lbs-server/src/fused/accelerometer-filter.c
+++ /dev/null
@@ -1,146 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
-#endif
-
-#define __FL_ACCELEROMETER_FILTER_C__
-
-#define _USE_MATH_DEFINES
-#include <math.h>
-#include "earth.h"
-#include "lp-3d-filter.h"
-#include "gravity-estimator.h"
-#include "accelerometer-filter.h"
-#include "debug_util.h"
-
-#define DEFAULT_SAMPLING_FREQUENCY           ((fl_hertz)10.0) /* [Hz] accelerometer sampling frequency */
-#define DEFAULT_ACCELERATION_SIGMA2          SQUARE(FL_ACCEL_NOISE_LEVEL)  /* [(m/s^2)^2] */
-#define FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD (((fl_seconds)1.0) * ((fl_hertz)1.0))
-
-void fl_accel_init(motion_changed_cb motion_changed, gpointer handler)
-{
-	static const fl_acceleration_vector av = {0, 0, EARTH_GRAVITY};
-
-	LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0);
-	fl_lp3d_init();
-	fl_gres_init();
-	fl_moti_init(motion_changed, handler);
-	__frequency_estimator.samples          = 0;
-	__frequency_estimator.last_update_time = FL_UNDEFINED_TIME;
-	fl_lp3d_reset_to(av);
-	fl_accel_set_sampling_frequency(DEFAULT_SAMPLING_FREQUENCY, FL_UNDEFINED_TIME);
-	LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0);
-}
-
-void fl_accel_exit(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0);
-	fl_moti_exit();
-	fl_gres_exit();
-	fl_lp3d_exit();
-	LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0);
-}
-
-void fl_accel_reset(const_fl_acceleration_vector_ref av)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	__frequency_estimator.samples          = 0;
-	__frequency_estimator.last_update_time = FL_UNDEFINED_TIME;
-	fl_moti_reset();
-	fl_lp3d_reset_to(av);
-}
-
-void fl_accel_set_sampling_frequency(fl_hertz f, fl_seconds t)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(f=%5.2f, t=%.3g)"), f, t);
-	if (f <= 0) {
-		LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("f=%5.2f <= 0"), f);
-		return;
-	}
-	__frequency_estimator.samples               = 0;
-	__frequency_estimator.last_update_time      = t;
-	__frequency_estimator.last_update_frequency = f;
-	fl_lp3d_set_sampling_frequency(f);
-}
-
-void fl_accel_process(
-	fl_seconds                       t,
-	const_fl_acceleration_vector_ref am,
-	fl_acceleration_vector_ref       al,
-	fl_vector_3d_ref                 gu,
-	fl_real                         *w2gu)
-{
-	fl_real                          g2;
-	fl_real                          af2;
-	const_fl_acceleration_vector_ref af;
-
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.2f, a=(% 5.2f, % 5.2f, % 5.2f))"), t, am[GEO_SPATIAL_X], am[GEO_SPATIAL_Y], am[GEO_SPATIAL_Z]);
-	if (__frequency_estimator.last_update_time > FL_UNDEFINED_TIME) {
-		fl_seconds dt;
-
-		++(__frequency_estimator.samples);
-		dt = t - __frequency_estimator.last_update_time;
-		if (dt > 0) {
-			fl_hertz f, df;
-
-			f = __frequency_estimator.samples / dt;
-			df = fabs(f - __frequency_estimator.last_update_frequency);
-			if (dt * df >= FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD)
-				fl_accel_set_sampling_frequency(f, t);
-		}
-	} else
-		__frequency_estimator.last_update_time = t;
-
-	af  = fl_lp3d_process(am);
-	af2 = fl_vector_3d_length2(af);
-
-	/* the z-value is passed through 2nd order Butterworth, then 1st order EMA */
-	fl_real overlap = fl_vector_3d_dot_product(af, am);
-	g2 = fl_gres_process(overlap);
-
-	LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=%.2f, a=(% 5.2f, % 5.2f, % 5.2f)), |af|=%5.2f, <g>=%5.2f"), t, am[GEO_SPATIAL_X], am[GEO_SPATIAL_Y], am[GEO_SPATIAL_Z], sqrt(af2), sqrt(g2));
-	if (af2 > SQUARE(FL_ACCEL_NOISE_LEVEL) && g2 > 1) {
-		fl_real norm_out = 1.0 / sqrt(af2);
-
-		gu[GEO_SPATIAL_X] = af[GEO_SPATIAL_X] * norm_out;
-		gu[GEO_SPATIAL_Y] = af[GEO_SPATIAL_Y] * norm_out;
-		gu[GEO_SPATIAL_Z] = af[GEO_SPATIAL_Z] * norm_out;
-		*w2gu = __gres.base.decay_rate / (fl_square(gu[GEO_SPATIAL_X]) + fl_square(gu[GEO_SPATIAL_Y]) + fl_square(gu[GEO_SPATIAL_Z] - 1) + GEO_SPATIAL_DIMENSION * DEFAULT_ACCELERATION_SIGMA2);
-	} else {
-		gu[GEO_SPATIAL_X] = 0;
-		gu[GEO_SPATIAL_Y] = 0;
-		gu[GEO_SPATIAL_Z] = 1; /* default */
-		*w2gu = __gres.base.decay_rate / (GEO_SPATIAL_DIMENSION * DEFAULT_ACCELERATION_SIGMA2);
-	}
-
-	fl_acceleration g = sqrt(g2);
-	if (g > 1) {
-		fl_real calibration;
-
-		calibration = EARTH_GRAVITY / g;
-		al[GEO_SPATIAL_X] = calibration * am[GEO_SPATIAL_X] - EARTH_GRAVITY * gu[GEO_SPATIAL_X];
-		al[GEO_SPATIAL_Y] = calibration * am[GEO_SPATIAL_Y] - EARTH_GRAVITY * gu[GEO_SPATIAL_Y];
-		al[GEO_SPATIAL_Z] = calibration * am[GEO_SPATIAL_Z] - EARTH_GRAVITY * gu[GEO_SPATIAL_Z];
-	} else {
-		al[GEO_SPATIAL_X] = am[GEO_SPATIAL_X];
-		al[GEO_SPATIAL_Y] = am[GEO_SPATIAL_Y];
-		al[GEO_SPATIAL_Z] = am[GEO_SPATIAL_Z];
-	}
-	fl_moti_process(t, al);
-}
diff --git a/lbs-server/src/fused/accelerometer-filter.h b/lbs-server/src/fused/accelerometer-filter.h
deleted file mode 100644
index 8e70f1e..0000000
--- a/lbs-server/src/fused/accelerometer-filter.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    accelerometer-filter.h
- * @brief   Accelerometer calibration (scale and sampling frequency), and
- *          low-pass 3D filtering.
- */
-
-#pragma once
-#ifndef __FL_ACCELEROMETER_FILTER_H__
-#define __FL_ACCELEROMETER_FILTER_H__
-
-#include "types.h"
-#include "motion-detector.h"
-#include "parameters.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/***************************************************************************//**
- * [public] Initializaton of the singleton unit (static constructor).
- *
- * @param[in] motion_changed
- *      Callback to be invoked when the detected state of motion changes.
- * @param[in] handler
- *      Arbitrary user parameter passed to the callback.
- */
-void fl_accel_init(motion_changed_cb motion_changed, gpointer handler);
-
-/***************************************************************************//**
- * [public] Cleanup of the singleton unit (static destructor).
- */
-void fl_accel_exit(void);
-
-/***************************************************************************//**
- * [public] Reset of the internal state back to initial one. This function is
- * used for repetitive testing and module soft restarts.
- *
- * @param[in] av
- *      Stationary acceleration state to be set (typically {0, 0, g}).
- */
-void fl_accel_reset(const_fl_acceleration_vector_ref av);
-
-/***************************************************************************//**
- * [public] Adapted internal state to the given sampling frequency. Call this
- * method whenever the measured frequency changes significantly.
- *
- * @param[in] f
- *      Measured accelerometer sampling frequency in Hertz.
- * @param[in] t
- *      Time of the event in seconds.
- */
-void fl_accel_set_sampling_frequency(fl_hertz samplingFrequency, fl_seconds t);
-
-/***************************************************************************//**
- * [public] Processing of a single sample from the accelerometer. This function
- * performs:
- * - estimation of the sampling frequency,
- * - estimation of the gravity direction via low-pass 3D filter  (cf. LP3D unit),
- * - estimation of the gravity value (cf. GRES unit) and scale correction,
- * - estimation of the linear acceleration i.e. with subtracted gravity component,
- * - motion state detection (cf. MOTI unit)
- *
- * @param[in] t
- *      Time of the event in seconds.
- * @param[in] am
- *      Measured 3D acceleration vector transformend to local frame.
- * @param[out] al
- *      Linear acceleration in local frame.
- * @param[out] gu
- *      Unit vector along gravity direction oriented upwards.
- * @param[out] w2gu
- *      Estimated inverse covariance (squared weight) of @a gu.
- */
-void fl_accel_process(fl_seconds t, const_fl_acceleration_vector_ref am, fl_acceleration_vector_ref al, fl_vector_3d_ref gu, fl_real *w2gu);
-
-
-/**
- * Accelerometer calibration (scale and sampling frequency), and low-pass 3D filtering.
- */
-
-#if defined(__FL_ACCELEROMETER_FILTER_C__)
-	#ifdef GTEST
-		#define PRIVATE
-	#else
-		#define PRIVATE \
-		static
-	#endif
-#else
-	#define PRIVATE \
-	extern
-#endif
-
-typedef struct {
-	unsigned    samples;
-	fl_seconds  last_update_time;
-	fl_hertz    last_update_frequency;
-} _fl_accel_frequency_estimator;
-
-PRIVATE _fl_accel_frequency_estimator __frequency_estimator;
-
-#if !defined(__FL_ACCELEROMETER_FILTER_C__)
-	#undef PRIVATE
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_ACCELEROMETER_FILTER_H__ */
diff --git a/lbs-server/src/fused/aema-estimator.c b/lbs-server/src/fused/aema-estimator.c
deleted file mode 100644
index 170abf3..0000000
--- a/lbs-server/src/fused/aema-estimator.c
+++ /dev/null
@@ -1,170 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_DETAIL */
-#endif
-
-#define __FL_AEMA_ESTIMATOR_C__
-
-#include <math.h>
-#include "aema-estimator.h"
-#include "debug_util.h"
-
-void __aema_init(fl_aema_estimator* self, unsigned plateau_samples)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](bits=%u)"), self, plateau_samples);
-	self->plateau_samples = plateau_samples;
-	self->samples         = 0;
-	self->update_rate     = 1;
-	self->decay_rate      = 0;
-}
-
-/***************************************************************************//**
- * [protected] Cleanup of the base class (destructor).
- */
-static inline void __aema_exit(fl_aema_estimator *self)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self);
-}
-
-void fl_aema1d_init(fl_aema1d_estimator* self, unsigned plateau_samples)
-{
-	__aema_init(&self->base, plateau_samples);
-	self->value = 0;
-}
-
-void fl_aema2d_init(fl_aema2d_estimator* self, unsigned plateau_samples)
-{
-	__aema_init(&self->base, plateau_samples);
-	self->value[AXIAL_H] = 0;
-	self->value[AXIAL_V] = 0;
-}
-
-void fl_aema3d_init(fl_aema3d_estimator* self, unsigned plateau_samples)
-{
-	__aema_init(&self->base, plateau_samples);
-	self->value[GEO_SPATIAL_X] = 0;
-	self->value[GEO_SPATIAL_Y] = 0;
-	self->value[GEO_SPATIAL_Z] = 0;
-}
-
-void fl_aema1d_exit(fl_aema1d_estimator *self)
-{
-	__aema_exit(&self->base);
-}
-
-void fl_aema2d_exit(fl_aema2d_estimator *self)
-{
-	__aema_exit(&self->base);
-}
-
-void fl_aema3d_exit(fl_aema3d_estimator *self)
-{
-	__aema_exit(&self->base);
-}
-
-void __aema_reset(fl_aema_estimator* self)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self);
-	self->samples     = 0;
-	self->update_rate = 1;
-	self->decay_rate  = 0;
-}
-
-void fl_aema1d_reset(fl_aema1d_estimator* self)
-{
-	__aema_reset(&self->base);
-	self->value = 0;
-}
-
-void fl_aema2d_reset(fl_aema2d_estimator* self)
-{
-	__aema_reset(&self->base);
-	self->value[PLANAR_X] = 0;
-	self->value[PLANAR_Y] = 0;
-}
-
-void fl_aema3d_reset(fl_aema3d_estimator* self)
-{
-	__aema_reset(&self->base);
-	self->value[GEO_SPATIAL_X] = 0;
-	self->value[GEO_SPATIAL_Y] = 0;
-	self->value[GEO_SPATIAL_Z] = 0;
-}
-
-fl_real fl_aema1d_process(fl_aema1d_estimator* self, fl_real value)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value);
-	__aema_process(&self->base);
-	/* exponential moving avarege */
-	self->value = self->base.decay_rate  * self->value
-				+ self->base.update_rate *       value;
-
-	return self->value;
-}
-
-const_fl_vector_2d_ref fl_aema2d_process(fl_aema2d_estimator* self, fl_real value_H, fl_real value_V)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V);
-
-	__aema_process(&self->base);
-#define UPDATE_VALUE(i)	self->value[AXIAL_##i] = self->base.decay_rate  * self->value[AXIAL_##i] + self->base.update_rate * value_##i;
-	UPDATE_VALUE(H);
-	UPDATE_VALUE(V);
-#undef UPDATE_VALUE
-
-	return self->value;
-}
-
-const_fl_vector_3d_ref fl_aema3d_process(fl_aema3d_estimator* self, const_fl_vector_3d_ref value)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[0], value[1], value[2]);
-
-	__aema_process(&self->base);
-#define UPDATE_VALUE(i)	self->value[i] = self->base.decay_rate  * self->value[i] + self->base.update_rate * value[i];
-	UPDATE_VALUE(GEO_SPATIAL_X);
-	UPDATE_VALUE(GEO_SPATIAL_Y);
-	UPDATE_VALUE(GEO_SPATIAL_Z);
-#undef UPDATE_VALUE
-
-	return self->value;
-}
-
-void fl_aema1d_set_estimate(fl_aema1d_estimator* self, fl_real value)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value);
-	__aema_set_estimate(&self->base);
-	self->value = value;
-}
-
-void fl_aema2d_set_estimate(fl_aema2d_estimator* self, fl_real value_H, fl_real value_V)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V);
-	__aema_set_estimate(&self->base);
-	self->value[AXIAL_H] = value_H;
-	self->value[AXIAL_V] = value_V;
-}
-
-void fl_aema3d_set_estimate(fl_aema3d_estimator* self, const_fl_vector_3d_ref value)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[0], value[1], value[2]);
-	__aema_set_estimate(&self->base);
-	self->value[GEO_SPATIAL_X] = value[GEO_SPATIAL_X];
-	self->value[GEO_SPATIAL_Y] = value[GEO_SPATIAL_Y];
-	self->value[GEO_SPATIAL_Z] = value[GEO_SPATIAL_Z];
-}
diff --git a/lbs-server/src/fused/aema-estimator.h b/lbs-server/src/fused/aema-estimator.h
deleted file mode 100644
index 36282f5..0000000
--- a/lbs-server/src/fused/aema-estimator.h
+++ /dev/null
@@ -1,312 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    aema-estimator.h
- * @brief   Adaptive Exponential Moving Average (1st order) filter
- */
-
-#pragma once
-#ifndef __FL_AEMA_ESTIMATOR_H__
-#define __FL_AEMA_ESTIMATOR_H__
-
-#include "types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#if defined(__FL_AEMA_ESTIMATOR_C__)
-	#define INLINE \
-	inline
-#else
-	#define INLINE \
-	static inline
-#endif
-
-typedef struct {
-	unsigned plateau_samples; /* number of samples to collect before EMA kicks-in */
-	unsigned samples;         /* min(number of samples collected, plateau_samples) */
-	fl_real  update_rate;     /* = 1 / samples */
-	fl_real  decay_rate;      /* = 1 - update_rate */
-} fl_aema_estimator;
-
-typedef struct {
-	fl_aema_estimator base;   /* base object */
-	fl_real           value;  /* filter output (estimated value) */
-} fl_aema1d_estimator;
-
-typedef struct {
-	fl_aema_estimator base;   /* base object */
-	fl_vector_2d      value;  /* filter output (estimated value) */
-} fl_aema2d_estimator;
-
-typedef struct {
-	fl_aema_estimator base;   /* base object */
-	fl_vector_3d      value;  /* filter output (estimated value) */
-} fl_aema3d_estimator;
-
-/***************************************************************************//**
- * [public] Initializaton of the 1D AEMA object (class constructor).
- *
- * @param[in] self
- *      Object pointer
- * @param[in] plateau_samples
- *      Number of samples to collect before switching to standard EMA mode.
- */
-void fl_aema1d_init(fl_aema1d_estimator *self, unsigned plateau_samples);
-
-/***************************************************************************//**
- * [public] Initializaton of the 2D AEMA object (class constructor).
- *
- * @param[in] self
- *      Object pointer
- * @param[in] plateau_samples
- *      Number of samples to collect before switching to standard EMA mode.
- */
-void fl_aema2d_init(fl_aema2d_estimator *self, unsigned plateau_samples);
-
-/***************************************************************************//**
- * [public] Initializaton of the 3D AEMA object (class constructor).
- *
- * @param[in] self
- *      Object pointer
- * @param[in] plateau_samples
- *      Number of samples to collect before switching to standard EMA mode.
- */
-void fl_aema3d_init(fl_aema3d_estimator *self, unsigned plateau_samples);
-
-/***************************************************************************//**
- * [public] Cleanup of the 1D AEMA object (class destructor).
- */
-void fl_aema1d_exit(fl_aema1d_estimator *self);
-
-/***************************************************************************//**
- * [public] Cleanup of the 2D AEMA object (class destructor).
- */
-void fl_aema2d_exit(fl_aema2d_estimator *self);
-
-/***************************************************************************//**
- * [public] Cleanup of the 3D AEMA object (class destructor).
- */
-void fl_aema3d_exit(fl_aema3d_estimator *self);
-
-/***************************************************************************//**
- * [public] Reset the 1D AEMA object back to initial state. This function is
- * used for repetitive testing and module soft restarts.
- *
- * @param[in] self
- *      Object pointer
- */
-void fl_aema1d_reset(fl_aema1d_estimator *self);
-
-/***************************************************************************//**
- * [public] Reset the 2D AEMA object back to initial state. This function is
- * used for repetitive testing and module soft restarts.
- *
- * @param[in] self
- *      Object pointer
- */
-void fl_aema2d_reset(fl_aema2d_estimator *self);
-
-/***************************************************************************//**
- * [public] Reset the 3D AEMA object back to initial state. This function is
- * used for repetitive testing and module soft restarts.
- *
- * @param[in] self
- *      Object pointer
- */
-void fl_aema3d_reset(fl_aema3d_estimator *self);
-
-/***************************************************************************//**
- * [public] Processing of a single data sample.
- *
- * @param[in] self
- *      Object pointer
- * @param[in] value
- *      Input value to the filter.
- *
- * @return
- *      Output value of the filter after processing. This is the same as
- *      returned by fl_aema1d_get_estimate(self).
- */
-fl_real fl_aema1d_process(fl_aema1d_estimator *self, fl_real value);
-
-/***************************************************************************//**
- * [public] Processing of a double data sample.
- *
- * @param[in] self
- *      Object pointer
- * @param[in] value_H
- *      First (horizontal) input value to the filter.
- * @param[in] value_V
- *      Second (vertical) input value to the filter.
- *
- * @return
- *      Output value of the filter after processing. This is the same as
- *      returned by fl_aema2d_get_estimate(self).
- */
-const_fl_vector_2d_ref fl_aema2d_process(fl_aema2d_estimator *self, fl_real value_X, fl_real value_Y);
-
-/***************************************************************************//**
- * [public] Processing of a triple data sample.
- *
- * @param[in] self
- *      Object pointer
- * @param[in] value
- *      Input 3D vector value to the filter.
- *
- * @return
- *      Output value of the filter after processing. This is the same as
- *      returned by fl_aema3d_get_estimate(self).
- */
-const_fl_vector_3d_ref fl_aema3d_process(fl_aema3d_estimator *self, const_fl_vector_3d_ref value);
-
-/***************************************************************************//**
- * [public] Bring the filter to a stationary state (EMA mode, input = output).
- * This method is used for testing.
- *
- * @param[in] self
- *      Object pointer
- * @param[in] value
- *      I/o value of the filter.
- */
-void fl_aema1d_set_estimate(fl_aema1d_estimator *self, fl_real value);
-
-/***************************************************************************//**
- * [public] Bring the filter to a stationary state (EMA mode, input = output).
- * This method is used for testing.
- *
- * @param[in] self
- *      Object pointer
- * @param[in] value_H
- *      First (horizontal) i/o value of the filter.
- * @param[in] value_V
- *      Second (vertical) i/o value of the filter.
- */
-void fl_aema2d_set_estimate(fl_aema2d_estimator *self, fl_real value_X, fl_real value_Y);
-
-/***************************************************************************//**
- * [public] Bring the filter to a stationary state (EMA mode, input = output).
- * This method is used for testing.
- *
- * @param[in] self
- *      Object pointer
- * @param[in] value
- *      I/o 3D vector value of the filter.
- */
-void fl_aema3d_set_estimate(fl_aema3d_estimator *self, const_fl_vector_3d_ref value);
-
-/***************************************************************************//**
- * [public] Fetch the current filter output.
- *
- * @param[in] self
- *      Object pointer
- *
- * @return
- *      Output value after last processed sample.
- */
-INLINE fl_real fl_aema1d_get_estimate(const fl_aema1d_estimator *self)
-{
-	return self->value;
-}
-
-/***************************************************************************//**
- * [public] Fetch the current filter output.
- *
- * @param[in] self
- *      Object pointer
- *
- * @return
- *      Output 2D vector after last processed sample.
- */
-INLINE const_fl_vector_2d_ref fl_aema2d_get_estimate(const fl_aema2d_estimator *self)
-{
-	return self->value;
-}
-
-/***************************************************************************//**
- * [public] Fetch the current filter output.
- *
- * @param[in] self
- *      Object pointer
- *
- * @return
- *      Output 3D vector after last processed sample.
- */
-INLINE const_fl_vector_3d_ref fl_aema3d_get_estimate(const fl_aema3d_estimator *self)
-{
-	return self->value;
-}
-
-/***************************************************************************//**
- * [protected] Initializaton of the base object (class constructor).
- *
- * @param[in] self
- *      Object pointer
- * @param[in] plateau_samples
- *      Number of samples to collect before switching to standard EMA mode.
- */
-void __aema_init(fl_aema_estimator *self, unsigned plateau_samples);
-
-/***************************************************************************//**
- * [protected] Reset the AEMA object back to initial state. This function is
- * used for repetitive testing and module soft restarts.
- *
- * @param[in] self
- *      Object pointer
- */
-void __aema_reset(fl_aema_estimator *self);
-
-/***************************************************************************//**
- * [protected] Processing of a data sample. This is the common part of the
- * derived processing methods, which implements the AEMA update/decay rate.
- *
- * @param[in] self
- *      Object pointer
- */
-INLINE void __aema_process(fl_aema_estimator *self)
-{
-	if (self->samples < self->plateau_samples) {
-		self->samples++;
-		self->update_rate = 1.0 / self->samples;
-		self->decay_rate  = 1.0 - self->update_rate;
-	}
-}
-
-/***************************************************************************//**
- * [protected] Bring the base filter to a stationary state (EMA mode). This
- * method is used for testing.
- *
- * @param[in] self
- *      Object pointer
- */
-INLINE void __aema_set_estimate(fl_aema_estimator* self)
-{
-	self->samples     = self->plateau_samples;
-	self->update_rate = 1.0 / self->plateau_samples;
-	self->decay_rate  = 1.0 - self->update_rate;
-}
-
-#if !defined(__FL_AEMA_ESTIMATOR_C__)
-	#undef INLINE
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_AEMA_ESTIMATOR_H__ */
diff --git a/lbs-server/src/fused/conversions.h b/lbs-server/src/fused/conversions.h
deleted file mode 100644
index 57f3f73..0000000
--- a/lbs-server/src/fused/conversions.h
+++ /dev/null
@@ -1,204 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    conversions.h
- * @brief   Units conversion functions
- */
-
-#pragma once
-#ifndef __FL_CONVERSIONS_H__
-#define __FL_CONVERSIONS_H__
-
-#define _USE_MATH_DEFINES
-#include "math-ext.h"
-#include "earth.h"
-#include "types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#if defined(__FL_CONVERSIONS_C__)
-	#define INLINE \
-	inline /* thank checkpatchinit_tizen for this contraption */
-#else
-	#define INLINE \
-	static inline
-#endif
-
-#define FL_TIMESTAMP_UNITS 1 /**< [s] */
-
-/***************************************************************************//**
- * [public] Convert location time-stamp to seconds. Notice, on Tizen this is a
- * formal methd since time-stamps are also resolved in seconds.
- *
- * @param[in] timestamp
- *      Time-stamp received from the location source (GPS, WPS).
- *
- * @return
- *      Corresponding value in seconds
- */
-INLINE fl_seconds fl_timestamp_to_seconds(fl_timestamp timestamp)
-{
-	return (fl_seconds)timestamp / FL_TIMESTAMP_UNITS;
-}
-
-/***************************************************************************//**
- * [public] Convert time in seconds to location time-stamp. The necesary
- * up-rounding is is half the time-stamp unit.
- *
- * @param[in] timestamp
- *      Time-stamp received from the location source (GPS, WPS).
- *
- * @return
- *      Corresponding value in seconds
- */
-INLINE fl_timestamp fl_seconds_to_timestamp(fl_seconds t)
-{
-	return (fl_timestamp)(t * FL_TIMESTAMP_UNITS + 0.5);
-}
-
-/***************************************************************************//**
- * [public] Convert degrees to radians.
- *
- * @param[in] degrees
- *      Value in degrees.
- *
- * @return
- *      Corresponding value in radians.
- */
-INLINE fl_radians fl_degrees_to_radians(fl_degrees degrees)
-{
-	return (M_PI / 180) * degrees;
-}
-
-/***************************************************************************//**
- * [public] Convert radians to degrees.
- *
- * @param[in] radians
- *      Value in radians.
- *
- * @return
- *      Corresponding value in degrees.
- */
-INLINE fl_degrees fl_radians_to_degrees(fl_radians radians)
-{
-	return (180 / M_PI) * radians;
-}
-
-/***************************************************************************//**
- * [public] Convert radians to degrees in the [0,360) range
- *
- * @param[in] radians
- *      Arbitrary value in radians.
- *
- * @return
- *      Value in degrees projected onto the [0,360) branch.
- */
-fl_degrees fl_radians_to_positive_degrees(fl_radians radians);
-
-/***************************************************************************//**
- * [public] Convert radians to degrees in the [-180,180) range.
- *
- * @param[in] radians
- *      Arbitrary value in radians.
- *
- * @return
- *      Value in degrees projected onto the [-180,180) branch.
- */
-fl_degrees fl_radians_to_balanced_degrees(fl_radians radians);
-
-/** [public] Convert radians to latitude degrees [-90,90] */
-#define fl_radians_to_latitude	fl_radians_to_balanced_degrees
-
-/** [public] Convert radians to longitude degrees [0, 360) */
-#define fl_radians_to_longitude fl_radians_to_positive_degrees
-
-/** [public] Convert radians to longitude degrees [0, 360) */
-#define fl_radians_to_cog       fl_radians_to_positive_degrees
-
-/***************************************************************************//**
- * [public] Convert speed in [km/h] to velocity in [m/s].
- *
- * @note The 'speed' is an i/o value of location interfaces, while 'velocity'
- * expressed in SI units is used internally by the fused location engine.
- *
- * @param[in] km_h
- *      Speed in kilometers per hour.
- *
- * @return
- *      Corresponding value in meters per second.
- */
-INLINE fl_velocity fl_km_h_to_m_s(fl_km_h km_h)
-{
-	return km_h * (1000.0 / (60.0 * 60.0));
-}
-
-/***************************************************************************//**
- * [public] Convert velocity in [m/s] to speed in [km/h].
- *
- * @note The 'speed' is an i/o value of location interfaces, while 'velocity'
- * expressed in SI units is used internally by the fused location engine.
- *
- * @param[in] velocity
- *      Velocity in meters per second.
- *
- * @return
- *      Corresponding value in kilometers per hour.
- */
-INLINE fl_km_h fl_m_s_to_km_h(fl_velocity velocity)
-{
-	return velocity * ((60.0 * 60.0) / 1000.0);
-}
-
-/***************************************************************************//**
- * [public] Convert spherical coordinate in radians to arc-length along
- * the Earth's great circle in meters.
- *
- * @param[in] radians
- *      Angle spanned by the great arc.
- *
- * @return
- *      Corresponding value in meters along Earth's surface.
- */
-INLINE fl_meters fl_radians_to_meters(fl_radians radians)
-{
-	return radians * EARTH_RADIUS;
-}
-
-/***************************************************************************//**
- * [public] Convert arc-length in meters along the Earth's great circle to arc
- * angle in radians.
- *
- * @param[in] meters
- *      Arc length in meters.
- *
- * @return
- *      Corresponding angle in radians.
- */
-INLINE fl_radians fl_meters_to_radians(fl_meters meters)
-{
-	return meters * (1.0 / EARTH_RADIUS);
-}
-
-#undef INLINE
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_CONVERSIONS_H_ */
diff --git a/lbs-server/src/fused/earth.h b/lbs-server/src/fused/earth.h
deleted file mode 100644
index 4291251..0000000
--- a/lbs-server/src/fused/earth.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    earth.h
- * @brief   Constants and methods related to the Earth model.
- */
-
-#pragma once
-#ifndef __FL_EARTH_H__
-#define __FL_EARTH_H__
-
-#include "types.h"
-
-/***************************************************************************//**
- * [public] Mean Earth gravity value in [m/s^2].
- *
- * Notice that, strictly speaking, the mean Earth gravity acceleration at surface
- * level is negative; the positive value we measure with an accelerometer is the
- * ground reaction which counters the gravity and prevents us from falling into
- * the interior. Also notice that given the inaccuracy (i.e. bias
- * & miscalibration) of mobile device acceleromenters at ~1 [m/s^2] the number
- * of significant digits in the reference value below is way too many.
- */
-#define EARTH_GRAVITY ((fl_acceleration)9.80665)
-
-/***************************************************************************//**
- * [public] Mean Earth radius in [m].
- */
-#define EARTH_RADIUS  ((fl_meters)6371000.0)
-
-#endif /* __FL_EARTH_H__ */
diff --git a/lbs-server/src/fused/gravity-estimator.c b/lbs-server/src/fused/gravity-estimator.c
deleted file mode 100644
index ed8356a..0000000
--- a/lbs-server/src/fused/gravity-estimator.c
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
-#endif
-
-#define __FL_GRAVITY_ESTIMATOR_C__
-
-#include "gravity-estimator.h"
-#include "debug_util.h"
-
-void fl_gres_init(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	fl_aema1d_init(&__gres, 1 << FL_GRES_AEMA_PLATEAU_BITS);
-}
-
-void fl_gres_exit(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	fl_aema1d_exit(&__gres);
-}
-
-void fl_gres_reset(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	fl_aema1d_reset(&__gres);
-}
diff --git a/lbs-server/src/fused/gravity-estimator.h b/lbs-server/src/fused/gravity-estimator.h
deleted file mode 100644
index 9d4a638..0000000
--- a/lbs-server/src/fused/gravity-estimator.h
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    gravity-estimator.h
- * @brief   Estimator of the gravity value used for accelerometer scale
- *          calibration.
- */
-
-#pragma once
-#ifndef __FL_GRAVITY_ESTIMATOR_H__
-#define __FL_GRAVITY_ESTIMATOR_H__
-
-#include "parameters.h"
-#include "aema-estimator.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#if defined(__FL_GRAVITY_ESTIMATOR_C__)
-	#define INLINE \
-	static inline /* thank checkpatchinit_tizen for this contraption */
-	#define PRIVATE
-#else
-	#define INLINE \
-	static inline
-	#define PRIVATE \
-	extern
-#endif
-
-PRIVATE fl_aema1d_estimator __gres;
-
-/***************************************************************************//**
- * [public] Initializaton of the singleton unit (static constructor).
- */
-void fl_gres_init(void);
-
-/***************************************************************************//**
- * [public] Cleanup of the singleton unit (static destructor).
- */
-void fl_gres_exit(void);
-
-/***************************************************************************//**
- * [public] Bring the internal state back to initial one. This function is used
- * for repetitive testing and module soft restarts.
- */
-void fl_gres_reset(void);
-
-/***************************************************************************//**
- * [public] Processing of a single data sample.
- *
- * @param[in] g2
- *      Modulus squared of the measured acceleration projected onto z-axis.
- *
- * @return
- *      Estimated value of the Earth gravity.
- */
-INLINE fl_real fl_gres_process(fl_real g2)
-{
-	return fl_aema1d_process(&__gres, g2);
-}
-
-/***************************************************************************//**
- * [public] Bring the filter to a stationary state (EMA mode, input = output).
- * This method is used for testing.
- *
- * @param[in] g2
- *      Modulus squared value of Earth gravity measured by the accelerometer.
- */
-INLINE void fl_gres_set_estimate(fl_real g2)
-{
-	fl_aema1d_set_estimate(&__gres, g2);
-}
-
-/***************************************************************************//**
- * [public] Fetch the current gravity estimate.
- *
- * @param[in] self
- *      Object pointer
- *
- * @return
- *      Output value after last processed sample.
- */
-/** Retrieve current estimate */
-INLINE fl_real fl_gres_get_estimate(void)
-{
-	return fl_aema1d_get_estimate(&__gres);
-}
-
-#if !defined(__FL_GRAVITY_ESTIMATOR_C__)
-	#undef PRIVATE
-	#undef INLINE
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif  /* __FL_GRAVITY_ESTIMATOR_H__ */
diff --git a/lbs-server/src/fused/gyroscope-filter.c b/lbs-server/src/fused/gyroscope-filter.c
deleted file mode 100644
index 7c5e4e2..0000000
--- a/lbs-server/src/fused/gyroscope-filter.c
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
-#endif
-
-#define __FL_GYROSCOPE_FILTER_C__
-
-#define _USE_MATH_DEFINES
-#include <math.h>
-#include "math-ext.h"
-#include "gravity-estimator.h"
-#include "gyroscope-filter.h"
-#include "debug_util.h"
-
-#define GYRO_PLATEAU_EVIDENCE  (1 << FL_GYRO_AEMA_PLATEAU_BITS)
-#define GYRO_SCALE2_I          (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_I)))
-#define GYRO_SCALE2_F          (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_F)))
-
-static fl_real __gyro_alpha;
-
-void fl_gyro_init(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0);
-	__gyro_alpha = (SQUARE(FL_GYRO_SPIN_SIGMA_I / FL_GYRO_SPIN_SIGMA_F) - 1) / SQUARE((fl_real)GYRO_PLATEAU_EVIDENCE);
-	fl_gyro_reset();
-	LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0);
-}
-
-void fl_gyro_exit(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-}
-
-void fl_gyro_reset(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	__gyro_bias.scale2               = GYRO_SCALE2_I;
-	__gyro_bias.evidence             = 0;
-	__gyro_bias.weight_norm          = 1;
-	__gyro_bias.value[GEO_SPATIAL_X] = 0;
-	__gyro_bias.value[GEO_SPATIAL_Y] = 0;
-	__gyro_bias.value[GEO_SPATIAL_Z] = 0;
-}
-
-void fl_gyro_set(const_fl_spin_vector_ref w0)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%g, %g, %g)"), w0[GEO_SPATIAL_X], w0[GEO_SPATIAL_Y], w0[GEO_SPATIAL_Z]);
-	__gyro_bias.scale2               = GYRO_SCALE2_F;
-	__gyro_bias.evidence             = GYRO_PLATEAU_EVIDENCE;
-	__gyro_bias.weight_norm          = 1.0 / GYRO_PLATEAU_EVIDENCE;
-	__gyro_bias.value[GEO_SPATIAL_X] = w0[GEO_SPATIAL_X];
-	__gyro_bias.value[GEO_SPATIAL_Y] = w0[GEO_SPATIAL_Y];
-	__gyro_bias.value[GEO_SPATIAL_Z] = w0[GEO_SPATIAL_Z];
-}
-
-const_fl_spin_vector_ref fl_gyro_process(fl_seconds t, const_fl_spin_vector_ref wm)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs)"), t);
-	static fl_spin_vector wf;
-				  fl_real weight;
-				  fl_real update_rate;
-				  fl_real decay_rate;
-				  fl_real fw2;
-
-	fw2 = fl_vector_3d_length2(wf);
-	weight = exp(-fw2 * __gyro_bias.scale2);
-	if (__gyro_bias.evidence < GYRO_PLATEAU_EVIDENCE) {
-		__gyro_bias.evidence += weight;
-		if (__gyro_bias.evidence > GYRO_PLATEAU_EVIDENCE)
-			__gyro_bias.evidence = GYRO_PLATEAU_EVIDENCE;
-		__gyro_bias.weight_norm = 1.0 / __gyro_bias.evidence;
-		__gyro_bias.scale2 = GYRO_SCALE2_I * (1.0 + fl_square(__gyro_bias.evidence) * __gyro_alpha);
-	}
-	update_rate = weight * __gyro_bias.weight_norm;
-	decay_rate  = 1.0 - update_rate;
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs), fw2=%g, e=%g, s2=%g, w=%g"), t, fw2, __gyro_bias.evidence, __gyro_bias.scale2, weight);
-
-# define UPDATE_VALUE(i)    __gyro_bias.value[i] = decay_rate  * __gyro_bias.value[i] + update_rate * wm[i];
-	UPDATE_VALUE(GEO_SPATIAL_X);
-	UPDATE_VALUE(GEO_SPATIAL_Y);
-	UPDATE_VALUE(GEO_SPATIAL_Z);
-# undef UPDATE_VALUE
-
-	wf[GEO_SPATIAL_X] = wm[GEO_SPATIAL_X] - __gyro_bias.value[GEO_SPATIAL_X];
-	wf[GEO_SPATIAL_Y] = wm[GEO_SPATIAL_Y] - __gyro_bias.value[GEO_SPATIAL_Y];
-	wf[GEO_SPATIAL_Z] = wm[GEO_SPATIAL_Z] - __gyro_bias.value[GEO_SPATIAL_Z];
-
-	return wf;
-}
diff --git a/lbs-server/src/fused/gyroscope-filter.h b/lbs-server/src/fused/gyroscope-filter.h
deleted file mode 100644
index 95783cc..0000000
--- a/lbs-server/src/fused/gyroscope-filter.h
+++ /dev/null
@@ -1,109 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    gyroscope-filter.h
- * @brief   Correction of the gyroscope bias.
- */
-
-#pragma once
-#ifndef __FL_GYROSCOPE_FILTER_H__
-#define __FL_GYROSCOPE_FILTER_H__
-
-#include "types.h"
-#include "parameters.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/***************************************************************************//**
- * [public] Initializaton of the singleton unit (static constructor).
- */
-void fl_gyro_init(void);
-
-/***************************************************************************//**
- * [public] Cleanup of the singleton unit (static destructor).
- */
-void fl_gyro_exit(void);
-
-/***************************************************************************//**
- * [public] Reset of the internal state back to initial one.
- */
-void fl_gyro_reset(void);
-
-/***************************************************************************//**
- * [public] Bring the filter to a stationary state (EMA mode, input = output).
- * This method is used for testing.
- *
- * @param[in] w0
- *      3D vector of angular velocity measured by the gyroscope in inertial
- *      coordinate frame.
- */
-void fl_gyro_set(const_fl_spin_vector_ref w0);
-
-/***************************************************************************//**
- * [public] Processing of a vector data sample. This method performs:
- * - estimation of the gyroscope bias with the weighted AEMA filter; the
- *   individual samples are weighted by the distance from average using Gaussian
- *   curve with monotonically-decreasing sigma parameter;
- * - correction of the measurement by the estimated bias.
- *
- * @param[in] t
- *      Event time in seconds.
- * @param[in] wm
- *      3D vector of measured angular velocity.
- *
- * @return
- *      3D vector of bias-corrected angular velocity.
- */
-const_fl_spin_vector_ref fl_gyro_process(fl_seconds t, const_fl_spin_vector_ref wm);
-
-#if defined(__FL_GYROSCOPE_FILTER_C__)
-	#ifdef GTEST
-		#define PRIVATE
-	#else
-		#define PRIVATE \
-		static
-	#endif
-#else
-	#define PRIVATE \
-	extern
-#endif
-
-/** Weighted AEMA filter. This is generalization of the 3D AEMA filter by
- * using 'evidence' as the sum of weights rather than sample count. The
- * sample weights are estimated using Gaussian curve with time-decreasing
- * variance.
- */
-typedef struct {
-	fl_real      scale2;      /* = 1 / (2 variance) */
-	fl_real      evidence;    /* min(accumulated weight, plateau_evidence) */
-	fl_real      weight_norm; /* = 1 / evidence */
-	fl_vector_3d value;       /* filter output (estimated value) */
-} fl_waema3d_estimator;
-
-PRIVATE fl_waema3d_estimator __gyro_bias;
-
-#if !defined(__FL_GYROSCOPE_FILTER_C__)
-	#undef PRIVATE
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_GYROSCOPE_FILTER_H__ */
diff --git a/lbs-server/src/fused/kalman-filter.c b/lbs-server/src/fused/kalman-filter.c
deleted file mode 100644
index 97b3223..0000000
--- a/lbs-server/src/fused/kalman-filter.c
+++ /dev/null
@@ -1,1126 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
-#endif
-
-#define __FL_KALMAN_FILTER_C__
-
-#include <float.h>
-#include <memory.h>
-#include "math-ext.h"
-#include "earth.h"
-#include "conversions.h"
-#include "accelerometer-filter.h"
-#include "gyroscope-filter.h"
-#include "aema-estimator.h"
-#include "kalman-filter.h"
-#include "debug_util.h"
-
-#define TIME_FORMAT                     "%.3fs"
-
-#define PRECISION                       1e-4
-#define PRECISION2                      SQUARE(PRECISION)
-
-/* the bigger the default standard deviantions, the swifter response to first measurements */
-#define INITIAL_POSITION_SIGMA2         SQUARE(1024 * M_2PI * EARTH_RADIUS)
-#define INITIAL_VELOCITY_SIGMA2         (1024 * 1024 * SQUARE(FL_DEFAULT_VELOCITY_SIGMA))
-#define INITIAL_ACCELERATION_SIGMA2     DEFAULT_ACCELERATION_SIGMA2
-
-#define __init_time_offset()            fl_aema1d_init(&__timestamp_offset, 1 << FL_TIMO_AEMA_PLATEAU_BITS)
-#define __exit_time_offset()            fl_aema1d_exit(&__timestamp_offset)
-#define __new_time_offset(dt)           fl_aema1d_process(&__timestamp_offset, dt)
-#define __get_time_offset()             fl_aema1d_get_estimate(&__timestamp_offset)
-
-PRIVATE void __rectify_state(_fl_kalf_state *src)
-{
-	unsigned i;
-
-	for (i = GEO_SPATIAL_DIMENSION;  i--;) {
-		_fl_kalf_sigma_matrix_ref s2xi;
-						  fl_real uuvv, uvvu;
-
-		s2xi = src->s2x[i];
-		/* diagonal elements */
-	#define CHECK(u) \
-		if (s2xi[STATE_##u][STATE_##u] < 0) { \
-			LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): src.s2x[%u][" #u "][" #u "] = %g < 0"), src, i, s2xi[STATE_##u][STATE_##u]); \
-			s2xi[STATE_##u][STATE_##u] = 0; \
-		}
-		CHECK(P);
-		CHECK(V);
-		CHECK(A);
-	#undef CHECK
-
-		/* partial determinats */
-	#define CHECK(u, v) \
-		uuvv = s2xi[STATE_##u][STATE_##u] * s2xi[STATE_##v][STATE_##v]; \
-		uvvu = s2xi[STATE_##u][STATE_##v] * s2xi[STATE_##v][STATE_##u]; \
-		if (uuvv < uvvu) { \
-			LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): det(src.s2x[%u], " #u ", " #v ") = %g < 0"), src, i, uuvv - uvvu); \
-			s2xi[STATE_##u][STATE_##v] = \
-			s2xi[STATE_##v][STATE_##u] *= sqrt(uuvv / uvvu); \
-		}
-		CHECK(P, V);
-		CHECK(P, A);
-		CHECK(A, V);
-	#undef CHECK
-	}
-}
-
-void fused_engine_init(motion_changed_cb on_motion_changed,
-						predicted_updated_cb on_location_changed, gpointer handler)
-{
-	LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("(motion_cb=%p, location_cb=%p, interval=%gs, handler=%p)"),
-				  on_motion_changed, on_location_changed, handler);
-
-	fl_accel_init(on_motion_changed, handler);
-	fl_gyro_init();
-	__init_time_offset();
-	__started                   = FALSE;
-	__on_location_changed       = on_location_changed;
-	__handler                   = handler;
-	fl_kalf_reset();
-	LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"));
-}
-
-void fused_engine_exit(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0);
-	__exit_time_offset();
-	fl_gyro_exit();
-	fl_accel_exit();
-	LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0);
-}
-
-void fused_engine_start(void)
-{
-	if (__on_location_changed) {
-		if (__started == FALSE) {
-			__started = TRUE;
-			LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("(): OK"), 0);
-			return;
-		} else {
-			LOG_FUSED_DEV(DBG_ERR, LEAVE_FUNCTION_PREFIX("ALREADY_STARTED"));
-			return;
-		}
-	} else {
-		LOG_FUSED_DEV(DBG_ERR, LEAVE_FUNCTION_PREFIX("UNINITIALIZED"));
-		return;
-	}
-}
-
-void fused_engine_stop(void)
-{
-	if (__started) {
-		__started = FALSE;
-		LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(): OK"), 0);
-	} else {
-		LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(): E_NOT_STARTED"), 0);
-	}
-}
-
-void fl_kalf_reset(void)
-{
-	unsigned i;
-
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	__last_notification.time     = FL_UNDEFINED_TIME;
-	__time_of.sensor_event.first = FL_UNDEFINED_TIME;
-	__time_of.sensor_event.last  = FL_UNDEFINED_TIME;
-	__time_of.sensor_event.count = 0;
-	__time_of.last_measurement   = FL_UNDEFINED_TIME;
-	__time_of.last_p_fixing      = FL_UNDEFINED_TIME;
-	__time_of.last_v_fixing      = FL_UNDEFINED_TIME;
-	__time_of.last_a_fixing      = FL_UNDEFINED_TIME;
-	__time_of.last_r_fixing      = FL_UNDEFINED_TIME;
-	__time_of.last_r_diffusion   = FL_UNDEFINED_TIME;
-	__time_of.last_w_fixing      = FL_UNDEFINED_TIME;
-#if (FL_KALMAN_ANGULAR_VELOCITY)
-	__time_of.last_wz_fixing     = FL_UNDEFINED_TIME;
-	__time_of.last_b_fixing      = FL_UNDEFINED_TIME;
-#endif
-	__time_of.last_reference     = FL_UNDEFINED_TIME;
-
-	memset(&__sp,  0, sizeof(__sp));
-	memset(&__se,  0, sizeof(__se));
-	memset(__wp,   0, sizeof(__wp));
-	memset(__we,   0, sizeof(__we));
-	memset(&__sre, 0, sizeof(__sre));
-	memset(&__srp, 0, sizeof(__srp));
-	__srp.w2[GEO_SPATIAL_X] = M_1_PI / 3;
-	__srp.w2[GEO_SPATIAL_Y] = M_1_PI / 3;
-	__srp.w2[GEO_SPATIAL_Z] = M_1_PI / 3;
-	__sre.w2[GEO_SPATIAL_X] = M_1_PI / 3;
-	__sre.w2[GEO_SPATIAL_Y] = M_1_PI / 3;
-	__sre.w2[GEO_SPATIAL_Z] = M_1_PI / 3;
-	__s2wp = DEFAULT_ROTATION_SIGMA2;
-	__s2we = DEFAULT_ROTATION_SIGMA2;
-	__wze   = 0;
-#if (FL_KALMAN_ANGULAR_VELOCITY)
-	__s2wze = DEFAULT_ROTATION_SIGMA2;
-	__be    = 0;
-#endif
-
-	memset(&__te, 0, sizeof(__te));
-	__sp.x[GEO_SPATIAL_Z][STATE_P] = EARTH_RADIUS;
-	__se.x[GEO_SPATIAL_Z][STATE_P] = EARTH_RADIUS;
-	for (i = GEO_SPATIAL_DIMENSION;  i;) {
-		--i;
-		/* diagonal terms */
-		__sp.s2x[i][STATE_P][STATE_P] = INITIAL_POSITION_SIGMA2;
-		__sp.s2x[i][STATE_V][STATE_V] = INITIAL_VELOCITY_SIGMA2;
-		__sp.s2x[i][STATE_A][STATE_A] = INITIAL_ACCELERATION_SIGMA2;
-		__se.s2x[i][STATE_P][STATE_P] = INITIAL_POSITION_SIGMA2;
-		__se.s2x[i][STATE_V][STATE_V] = INITIAL_VELOCITY_SIGMA2;
-		__se.s2x[i][STATE_A][STATE_A] = INITIAL_ACCELERATION_SIGMA2;
-		__sre.m[i][i] = 1;
-		__srp.m[i][i] = 1;
-		__te.fsr[i][i] = 1;
-	}
-	__te.crx = 1;
-	/* pass-throughs */
-	__fix_status = LOCATION_STATUS_NO_FIX;
-}
-
-PRIVATE fl_seconds __timestamp_to_seconds(fl_timestamp timestamp)
-{
-	__time_of.last_measurement = fl_timestamp_to_seconds(timestamp);
-	if (__time_of.sensor_event.last != FL_UNDEFINED_TIME) {
-		fl_seconds dt;
-
-		dt = __time_of.last_measurement - __get_time_offset() - __time_of.sensor_event.last;
-		if (dt < 0) {
-			__new_time_offset(__time_of.last_measurement - __time_of.sensor_event.last);
-			dt = 0;
-		} else if (__time_of.sensor_event.count > 1) {
-			fl_seconds mean_sensor_dt = (__time_of.sensor_event.last - __time_of.sensor_event.first) / (__time_of.sensor_event.count - 1);
-
-			if (dt > mean_sensor_dt) {
-				__new_time_offset(__time_of.last_measurement - __time_of.sensor_event.last - mean_sensor_dt);
-				dt = mean_sensor_dt;
-			}
-		}
-		return __time_of.sensor_event.last + dt;
-	}
-	return __time_of.last_measurement;
-}
-
-PRIVATE void __matrix_3d_multiply_matrix_3d(const_fl_matrix_3d_ref m_1, const_fl_matrix_3d_ref m_2, fl_matrix_3d_ref out)
-{
-	unsigned i, j;
-
-	i = GEO_SPATIAL_DIMENSION;
-	do {
-		--i;
-		j = GEO_SPATIAL_DIMENSION;
-		do {
-			--j;
-			out[i][j] = m_1[i][GEO_SPATIAL_X] * m_2[GEO_SPATIAL_X][j]
-					  + m_1[i][GEO_SPATIAL_Y] * m_2[GEO_SPATIAL_Y][j]
-					  + m_1[i][GEO_SPATIAL_Z] * m_2[GEO_SPATIAL_Z][j];
-		} while (j);
-	} while (i);
-}
-
-PRIVATE void __create_push_forward_map(_fl_kalf_state_matrix_ref x, LocationStatus status)
-{
-	fl_real      crx, srx;
-	fl_real      cry, sry;
-	fl_real      yz2, r2;
-	fl_vector_3d p;
-	fl_vector_3d v;
-
-	/* pull-back: from the old tangent frame to global coordinates */
-	p[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X];
-	p[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y];
-	p[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z];
-	v[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X];
-	v[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y];
-	v[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z];
-
-	yz2 = fl_square(p[GEO_SPATIAL_Y]) + fl_square(p[GEO_SPATIAL_Z]);
-	r2  = yz2 + fl_square(p[GEO_SPATIAL_X]);
-	if (r2 > PRECISION2) {
-		fl_real _r = 1.0 / sqrt(r2);
-		fl_real yz = sqrt(yz2);
-
-		__te.rx = atan2(p[GEO_SPATIAL_X], yz);
-		__te.srx = srx = _r * p[GEO_SPATIAL_X];
-		__te.crx = crx = _r * yz;
-		if (yz > PRECISION) {
-			fl_real _yz = 1.0 / yz;
-
-			__te.ry = atan2(p[GEO_SPATIAL_Y], p[GEO_SPATIAL_Z]);
-			sry = _yz * p[GEO_SPATIAL_Y];
-			cry = _yz * p[GEO_SPATIAL_Z];
-		} else {
-UndefinedLongitude:
-			__te.ry = 0;
-			sry = 0;
-			cry = 1;
-		}
-	} else {
-		__te.rx  = 0;
-		__te.srx = srx = 0;
-		__te.crx = crx = 1;
-		goto UndefinedLongitude;
-	}
-	/* new push-forward map */
-	__te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] =  crx;
-	__te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] =  0;
-	__te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] =  srx;
-
-	__te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] = -sry * srx;
-	__te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] =  cry;
-	__te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] =  sry * crx;
-
-	__te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] = -cry * srx;
-	__te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] = -sry;
-	__te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] =  cry * crx;
-	/* push-forward: from the global onto the new tangent frame */
-	x[GEO_SPATIAL_X][STATE_P] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z];
-	x[GEO_SPATIAL_Y][STATE_P] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z];
-	x[GEO_SPATIAL_X][STATE_V] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z];
-	x[GEO_SPATIAL_Y][STATE_V] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z];
-	x[GEO_SPATIAL_Z][STATE_P] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z];
-	x[GEO_SPATIAL_Z][STATE_V] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z];
-}
-
-PRIVATE void __spherical_to_tangent(
-	const fl_spherical_position *pos,
-	const fl_tangent_velocity   *vel,
-	const fl_sigma              *sigma,
-	fl_position_vector_ref       pm,
-	fl_velocity_vector_ref       vm,
-	fl_vector_3d_ref             s2pm,
-	fl_vector_3d_ref             s2vm)
-{
-	fl_real                      rp, rx, ry, rb;
-	fl_real                      crx, srx;
-	fl_position_vector           pg;
-	int                          spatial;
-
-	spatial = (pos->status == LOCATION_STATUS_3D_FIX);
-	/* pass-through */
-	__fix_status = pos->status;
-	/* global coordinates */
-	rx = fl_degrees_to_radians(pos->latitude);
-	ry = fl_degrees_to_radians(pos->longitude);
-	rp = spatial ? EARTH_RADIUS + pos->altitude : __se.x[GEO_SPATIAL_Z][STATE_P];
-	srx = rp * sin(rx);
-	crx = rp * cos(rx);
-	pg[GEO_SPATIAL_X] = srx;
-	pg[GEO_SPATIAL_Y] = crx * sin(ry);
-	pg[GEO_SPATIAL_Z] = crx * cos(ry);
-	/* push-forward map: new position onto the current tangent space */
-	pm[GEO_SPATIAL_X] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z];
-	pm[GEO_SPATIAL_Y] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z];
-	pm[GEO_SPATIAL_Z] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z];
-
-	fl_real cd, cd2, sd2;
-	cd = pm[GEO_SPATIAL_Z] / rp;
-	cd2 = SQUARE(cd);
-	sd2 = 1 - cd2;
-	if (sd2 < 0) {
-		cd2 = 1;
-		sd2 = 0;
-	}
-	/* Notice: in principle we should rotate the diagonal sigma matrix here (two
-	 * matrix multiplications). This approximation assumes the old and new points
-	 * are close. */
-	if (sigma->of_altitude > 0) {
-		fl_real s2h, s2v;
-
-		s2h = fl_square(sigma->of_horizontal_pos);
-		s2v = fl_square(sigma->of_altitude);
-		s2pm[GEO_SPATIAL_X] =
-		s2pm[GEO_SPATIAL_Y] = cd2 * s2h + sd2 * s2v;
-		s2pm[GEO_SPATIAL_Z] = sd2 * s2h + cd2 * s2v;
-	} else {
-		s2pm[GEO_SPATIAL_X] =
-		s2pm[GEO_SPATIAL_Y] =
-		s2pm[GEO_SPATIAL_Z] = fl_square(sigma->of_horizontal_pos);
-	}
-
-	if (vel && vm) {
-		fl_velocity vl; /* velocity vector length */
-
-		/* The velocity is given in the tangent frame of p (which is different than
-		 * the currecnt tangent one). In principle we should pull v back into the
-		 * global frame using p as the transformation generator, and then push it
-		 * forward onto our frame. Except first time when the two frames are
-		 * randomly far apart this composition is almost always near-identity, thus
-		 * can safely be approximated with tiny correction to the bearing.
-		 */
-		rb = fl_degrees_to_radians(vel->direction)
-		   - atan2(pm[GEO_SPATIAL_Y] * __te.srx,
-				  -pm[GEO_SPATIAL_X] * __te.srx + EARTH_RADIUS * __te.crx);
-		vl = fl_km_h_to_m_s(vel->speed);
-		  vm[GEO_SPATIAL_X] = vl * cos(rb);
-		  vm[GEO_SPATIAL_Y] = vl * sin(rb);
-		  vm[GEO_SPATIAL_Z] = spatial ? fl_km_h_to_m_s(vel->climb) : __se.x[GEO_SPATIAL_Z][STATE_V];
-		if (sigma->of_climb > 0) {
-			s2vm[GEO_SPATIAL_X] =
-			s2vm[GEO_SPATIAL_Y] = (cd2 * fl_square(sigma->of_speed) + sd2 * fl_square(sigma->of_climb));
-			s2vm[GEO_SPATIAL_Z] = (sd2 * fl_square(sigma->of_speed) + cd2 * fl_square(sigma->of_climb));
-		} else {
-			s2vm[GEO_SPATIAL_X] =
-			s2vm[GEO_SPATIAL_Y] =
-			s2vm[GEO_SPATIAL_Z] = fl_square(sigma->of_speed);
-		}
-	}
-}
-
-PRIVATE void __tangent_to_spherical(
-	const _fl_kalf_state_matrix_ref x,
-	const _fl_kalf_sigma_cube_ref   s2x,
-	fl_spherical_position*          pos,
-	fl_tangent_velocity*            vel,
-	fl_sigma*                       sigma)
-{
-	if (pos) {
-		fl_position_vector p;
-
-		/* pull-back map: from the tangent frame to global coordinates */
-		p[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X];
-		p[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y];
-		p[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z];
-
-		fl_real yz2, yz;
-		fl_real xyz2;
-		fl_radians rx;
-		fl_radians ry;
-
-		yz2 = fl_square(p[GEO_SPATIAL_Y]) + fl_square(p[GEO_SPATIAL_Z]);
-		yz  = sqrt(yz2);
-		if (yz2 > PRECISION2)
-			ry = atan2(p[GEO_SPATIAL_Y], p[GEO_SPATIAL_Z]);
-		else
-			ry = 0; /* avoid #IND values */
-		xyz2 = yz2 + fl_square(p[GEO_SPATIAL_X]);
-		if (xyz2 > PRECISION2)
-			rx = atan2(p[GEO_SPATIAL_X], yz);
-		else
-			rx = 0; /* avoid #IND values */
-		pos->timestamp = fl_seconds_to_timestamp(__last_notification.time > FL_UNDEFINED_TIME ? __last_notification.time : 0);
-		pos->latitude  = fl_radians_to_latitude(rx);
-		pos->longitude = fl_radians_to_longitude(ry);
-		pos->altitude  = sqrt(xyz2) - EARTH_RADIUS;
-		pos->status    = __fix_status;
-
-		fl_real rb;
-		fl_real u[PLANAR_DIMENSION];
-
-		u[PLANAR_X] = EARTH_RADIUS * __te.crx - x[GEO_SPATIAL_X][STATE_P] * __te.srx;
-		u[PLANAR_Y] =                         - x[GEO_SPATIAL_Y][STATE_P] * __te.srx;
-		rb = -atan2(x[GEO_SPATIAL_X][STATE_V] * u[PLANAR_Y] - x[GEO_SPATIAL_Y][STATE_V] * u[PLANAR_X],
-					x[GEO_SPATIAL_X][STATE_V] * u[PLANAR_X] + x[GEO_SPATIAL_Y][STATE_V] * u[PLANAR_Y]);
-
-		if (vel) {
-			vel->timestamp = pos->timestamp;
-			vel->speed     = fl_m_s_to_km_h(sqrt(fl_square(x[GEO_SPATIAL_Y][STATE_V]) + fl_square(x[GEO_SPATIAL_X][STATE_V])));
-			vel->direction = fl_radians_to_cog(rb);
-			vel->climb     = fl_m_s_to_km_h(x[GEO_SPATIAL_Z][STATE_V]);
-		}
-		if (sigma) {
-			sigma->level             = LOCATION_ACCURACY_LEVEL_NONE;
-			sigma->of_horizontal_pos = sqrt((s2x[GEO_SPATIAL_X][STATE_P][STATE_P] + s2x[GEO_SPATIAL_Y][STATE_P][STATE_P]) * 0.5);
-			sigma->of_altitude       =       s2x[GEO_SPATIAL_Z][STATE_P][STATE_P];
-			sigma->of_speed          = sqrt((s2x[GEO_SPATIAL_X][STATE_V][STATE_V] + s2x[GEO_SPATIAL_Y][STATE_V][STATE_V]) * 0.5);
-			sigma->of_climb          =       s2x[GEO_SPATIAL_Z][STATE_V][STATE_V];
-		}
-	}
-}
-
-PRIVATE void __kalman_2d1o(fl_seconds t, _fl_kalf_state_component s, const_fl_vector_3d_ref xm, const_fl_vector_3d_ref s2xm)
-{
-	unsigned i;
-
-	/* @note WPS-specific: restrict the update to horizontal position */
-	i = PLANAR_DIMENSION;
-	do {
-		--i;
-		   const_fl_vector_3d_ref s2pis;
-		_fl_kalf_state_matrix_ref s2ei;
-		_fl_kalf_state_matrix_ref s2pi;
-						  fl_real s2k, s2mi, s2piss;
-
-		s2mi   =     s2xm[i];
-		s2ei   = __se.s2x[i];
-		s2pi   = __sp.s2x[i];
-		s2pis  = s2pi[s];
-		s2piss = s2pis[s];
-		s2k    = s2mi + s2piss;
-		if (s2k > PRECISION2) {
-			fl_real inv_s2k;
-			fl_real dx;
-
-			inv_s2k = 1.0 / s2k;
-			dx = xm[i] - __sp.x[i][s];
-
-			/* reduced Joseph form in special 3x1 case: subtract scaled projection */
-		#define SET_FIELD(a, b) s2ei[a][b] = inv_s2k * ((s2pi[a][b] * s2piss - s2pis[a] * s2pis[b]) + s2pi[a][b] * s2mi)
-		#define CPY_FIELD(a, b) s2ei[a][b] = s2ei[b][a]
-			SET_FIELD(STATE_P, STATE_P);
-			SET_FIELD(STATE_P, STATE_V);
-			SET_FIELD(STATE_P, STATE_A);
-			CPY_FIELD(STATE_V, STATE_P);
-			SET_FIELD(STATE_V, STATE_V);
-			SET_FIELD(STATE_V, STATE_A);
-			CPY_FIELD(STATE_A, STATE_P);
-			CPY_FIELD(STATE_A, STATE_V);
-			SET_FIELD(STATE_A, STATE_A);
-		#undef CPY_FIELD
-		#undef SET_FIELD
-
-		#define SET_FIELD(a) __se.x[i][a] = __sp.x[i][a] + dx * (inv_s2k * s2pis[a])
-			SET_FIELD(STATE_P);
-			SET_FIELD(STATE_V);
-			SET_FIELD(STATE_A);
-		#undef SET_FIELD
-		}
-	} while (i);
-	/* the third coordinate is of `special care': we want to preserve it in the tangent frame, along with its sigma */
-	if (s == STATE_P)
-		__se.x[GEO_SPATIAL_Z][STATE_P] = xm[GEO_SPATIAL_Z];
-
-	__copy_state(&__sp, &__se);
-	__time_of.last_reference = t;
-}
-
-/* Update of the 2D position */
-PRIVATE void __kalman_p(fl_seconds t, const_fl_position_vector_ref pm, const_fl_vector_3d_ref s2pm)
-{
-	__predict_pv(t);
-	__kalman_2d1o(t, STATE_P, pm, s2pm);
-	__time_of.last_p_fixing  = t;
-	__time_of.last_reference = t;
-}
-
-/* Update of the 3D position & velocity */
-PRIVATE void __kalman_pv(
-	fl_seconds                   t,
-	const_fl_position_vector_ref pm,   /* p-measurement */
-	const_fl_velocity_vector_ref vm,   /* v-measurement */
-		  const_fl_vector_3d_ref s2pm, /* p-measurement variance */
-		  const_fl_vector_3d_ref s2vm  /* v-measurement variance */
-) {
-	unsigned i;
-
-	__predict_pv(t);
-	for (i = GEO_SPATIAL_DIMENSION;  i;) {
-		--i;
-
-		_fl_kalf_matrix_2d s2k;
-		fl_real            det;
-		/*  shortcuts */
-		_const_fl_kalf_sigma_matrix_ref s2xpi = __sp.s2x[i];
-		const fl_real                    *xpi = __sp  .x[i];
-
-		/* Sk = Sp + Sm */
-		s2k[STATE_P][STATE_P] = s2xpi[STATE_P][STATE_P] + s2pm[i];
-		s2k[STATE_P][STATE_V] = /* == s2pi[STATE_P][STATE_V] */
-		s2k[STATE_V][STATE_P] = s2xpi[STATE_V][STATE_P];
-		s2k[STATE_V][STATE_V] = s2xpi[STATE_V][STATE_V] + s2vm[i];
-		det = s2k[STATE_P][STATE_P] * s2k[STATE_V][STATE_V] - s2k[STATE_V][STATE_P] * s2k[STATE_P][STATE_V];
-		if (fl_square(det) > PRECISION2) {
-			fl_real            denom;
-			_fl_kalf_matrix_2d s2i;
-			_fl_kalf_matrix_2d k0, k1;
-
-			/* Si = Sk^{-1} */
-			denom = 1.0 / det;
-			s2i[STATE_P][STATE_P] =  denom * s2k[STATE_V][STATE_V];
-			s2i[STATE_P][STATE_V] =
-			s2i[STATE_V][STATE_P] = -denom * s2k[STATE_V][STATE_P];
-			s2i[STATE_V][STATE_V] =  denom * s2k[STATE_P][STATE_P];
-
-			/* K = Sp.Si */
-		#define SET_K0(j, k)	k0[j][k] = s2xpi[j][STATE_P] * s2i[STATE_P][k] + s2xpi[j][STATE_V] * s2i[STATE_V][k]
-			SET_K0(STATE_P, STATE_P);
-			SET_K0(STATE_P, STATE_V);
-			SET_K0(STATE_V, STATE_P);
-			SET_K0(STATE_V, STATE_V);
-		#undef SET_K0
-			/* 1 - K = Sm.Si, use positive form */
-			k1[STATE_P][STATE_P] = s2pm[i] * s2i[STATE_P][STATE_P];
-			k1[STATE_P][STATE_V] = s2pm[i] * s2i[STATE_P][STATE_V];
-			k1[STATE_V][STATE_P] = s2vm[i] * s2i[STATE_V][STATE_P];
-			k1[STATE_V][STATE_V] = s2vm[i] * s2i[STATE_V][STATE_V];
-			/* state update */
-			__se.x[i][STATE_P] = k0[STATE_P][STATE_P] *  pm[i]       + k0[STATE_P][STATE_V] *  vm[i]
-							   + k1[STATE_P][STATE_P] * xpi[STATE_P] + k1[STATE_P][STATE_V] * xpi[STATE_V];
-			__se.x[i][STATE_V] = k0[STATE_V][STATE_P] *  pm[i]       + k0[STATE_V][STATE_V] *  vm[i]
-							   + k1[STATE_V][STATE_P] * xpi[STATE_P] + k1[STATE_V][STATE_V] * xpi[STATE_V];
-			/* covariance update Se = K1,K1.Sp + K,K.Sm */
-			_fl_kalf_sigma_matrix_ref s2ei = __se.s2x[i];
-
-		#define SET_S2E(j, k)	s2ei[j][k] = \
-									  k1[j][STATE_P] * k1[k][STATE_P] * s2xpi[STATE_P][STATE_P] + k0[j][STATE_P] * k0[k][STATE_P] * s2pm[i] \
-									+ k1[j][STATE_P] * k1[k][STATE_V] * s2xpi[STATE_P][STATE_V]                                             \
-									+ k1[j][STATE_V] * k1[k][STATE_P] * s2xpi[STATE_V][STATE_P]                                             \
-									+ k1[j][STATE_V] * k1[k][STATE_V] * s2xpi[STATE_V][STATE_V] + k0[j][STATE_V] * k0[k][STATE_V] * s2vm[i]
-
-			SET_S2E(STATE_P, STATE_P);
-			SET_S2E(STATE_P, STATE_V);
-			SET_S2E(STATE_V, STATE_P);
-			SET_S2E(STATE_V, STATE_V);
-		#undef SET_S2E
-		}
-		/* upon arrival of new position-velocity pair (a maximally uncertain) the pair (p, v) decorrelates from a: */
-		__se.s2x[i][STATE_P][STATE_A] =	0;
-		__se.s2x[i][STATE_V][STATE_A] = 0;
-		__se.s2x[i][STATE_A][STATE_P] =	0;
-		__se.s2x[i][STATE_A][STATE_V] = 0;
-		__se.s2x[i][STATE_A][STATE_A] = __sp.s2x[i][STATE_A][STATE_A];
-	}
-	__copy_state(&__sp, &__se);
-	__time_of.last_p_fixing  = t;
-	__time_of.last_v_fixing  = t;
-	__time_of.last_reference = t;
-}
-
-#if (FL_KALMAN_ANGULAR_VELOCITY)
-void __kalman_wz(fl_seconds t, fl_real wzm, fl_real s2wzm)
-{
-	fl_real s2wzp;
-
-	if (__time_of.last_wz_fixing > FL_UNDEFINED_TIME) {
-		fl_real dt = t - __time_of.last_wz_fixing;
-		fl_real s2wzk;
-	#define wzp __wze /* constant prediction */
-
-		/* propagate/diffuse sigma^2 */
-		s2wzp = __s2wze + dt * SQUARE(FL_DEFAULT_SPIN_SIGMA);
-		s2wzk = s2wzp + s2wzm;
-		if (s2wzk > PRECISION2) {
-			fl_real inv_s2k;
-
-			inv_s2k = 1.0 / s2wzk;
-			LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=" TIME_FORMAT ", w=%.4g, s2w=%.4g): __wze:%g->%g, __s2wze:%g->%g"),
-						t, wzm, s2wzm, __wze, inv_s2k * (s2wzp * wzm + s2wzm * wzp),
-						__s2wze, inv_s2k * s2wzp * s2wzm * 2);
-			__s2wze = inv_s2k * s2wzp * s2wzm * 2;
-			__wze   = inv_s2k * (s2wzp * wzm + s2wzm * wzp);
-		} else {
-			__wze   = wzm;
-			__s2wze = s2wzm;
-		}
-	#undef wzp
-	} else {
-		__wze   = wzm;
-		__s2wze = s2wzm;
-	}
-	__time_of.last_wz_fixing = t;
-}
-#endif
-
-/* Blend two unit 3D vectors usign inverse covariances as weights (to avoid singularities) */
-PRIVATE void __inv_kalman_u(
-	fl_seconds t,
-	const_fl_vector_3d_ref u1,  fl_real w2u1,
-	const_fl_vector_3d_ref u2,  fl_real w2u2,
-	fl_vector_3d_ref       out, fl_real *w2out)
-{
-	unsigned i;
-
-	*w2out = 0.5 * (w2u1 + w2u2);
-	if (*w2out > PRECISION2) {
-		fl_real  e2    = 0;
-		fl_real _w2out = 1.0 / (*w2out);
-
-		i = GEO_SPATIAL_DIMENSION;
-		do {
-			fl_real  e;
-			--i;
-			out[i] = e = _w2out * (u1[i] * w2u1 + u2[i] * w2u2);
-			e2 += SQUARE(e);
-		} while (i);
-
-		if (e2 > PRECISION2) {
-			fl_real _e;
-
-			/* normalize to unity */
-			_e = 1.0 / sqrt(e2);
-			i = GEO_SPATIAL_DIMENSION;
-			do
-				out[--i] *= _e;
-			while (i);
-		}
-	} else {
-		i = GEO_SPATIAL_DIMENSION;
-		do
-			out[--i] = 0;
-		while (i);
-	}
-}
-
-/* 3D cross-product with normalization */
-PRIVATE void __cross_product(const_fl_vector_3d_ref in1, const_fl_vector_3d_ref in2, fl_vector_3d_ref out)
-{
-	fl_real out2;
-
-	/* The cross-product preserves system handedness: it's right-handed in the
-	 * right-handed system, and left-handed in the left-handed one.
-	 */
-	VECTOR_CROSS_VECTOR_3D(in1, in2, out);
-
-	out2 = fl_square(out[GEO_SPATIAL_X])
-		 + fl_square(out[GEO_SPATIAL_Y])
-		 + fl_square(out[GEO_SPATIAL_Z]);
-	if (fl_square(out2 - 1) > PRECISION2) {
-		fl_real _out = 1.0 / sqrt(out2);
-		/* normalize */
-		out[GEO_SPATIAL_X] *= _out;
-		out[GEO_SPATIAL_Y] *= _out;
-		out[GEO_SPATIAL_Z] *= _out;
-	}
-}
-
-PRIVATE void __signal_updated_location(fl_seconds t)
-{
-	if (FL_MIN_NOTIFICATION_FILTRATION && __started && __time_of.last_p_fixing > FL_UNDEFINED_TIME) {
-		t += __get_time_offset();
-		if ((t - __last_notification.time) * 1000 >= FL_MIN_NOTIFICATION_INTERVAL) {
-			__get_prediction_vector(t, &__last_notification);
-			__on_location_changed(&__last_notification, __handler);
-		}
-	}
-}
-
-/* Filter input: Enter the raw position measurement */
-void fused_engine_process_position_2d_event(const fl_spherical_position* pos, const fl_sigma* sigma)
-{
-	if (pos && sigma) {
-		if (pos->status >= LOCATION_STATUS_2D_FIX) {
-					fl_seconds t;
-			fl_position_vector pm;   /* p-measurement */
-				  fl_vector_3d s2pm; /* p-measurement variance */
-
-			t = __timestamp_to_seconds(pos->timestamp);
-			LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g�, %g�), sp=%gm)"),
-						  t,
-						  pos->latitude,
-						  pos->longitude,
-						  sigma->of_horizontal_pos);
-			__spherical_to_tangent(pos, NULL, sigma, pm, NULL, s2pm, NULL);
-			/* Kalman merging of 2D position */
-			__kalman_p(t, pm, s2pm);
-			__create_push_forward_map(__se.x, pos->status);
-			memcpy(__sp.x, __se.x, sizeof(__sp.x));
-			__signal_updated_location(t);
-		} else {
-			LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(location.pos.status=%d)"), pos->status);
-		}
-	} else {
-		LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%p, %p): INVALID ARGUMENT"), pos, sigma);
-	}
-}
-
-/* Filter input: Enter the raw position measurement */
-void fused_engine_process_position_2x3d_event(const fl_spherical_position *pos, const fl_tangent_velocity *vel, const fl_sigma *sigma)
-{
-	if (pos && vel && sigma) {
-		if (pos->status >= LOCATION_STATUS_2D_FIX) {
-					fl_seconds t;
-			fl_position_vector pm;   /* p-measurement */
-			fl_velocity_vector vm;   /* v-measurement */
-				  fl_vector_3d s2pm; /* p-measurement variance */
-				  fl_vector_3d s2vm; /* v-measurement variance */
-
-			t = __timestamp_to_seconds(pos->timestamp);
-			LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g�, %g�, %gm), cog=%g�, sog=%gkm/h, climb=%gkm/h, sp=%gm, sv=%gm/s)"),
-						  t,
-						  pos->latitude,
-						  pos->longitude,
-						  pos->altitude,
-						  vel->direction,
-						  vel->speed,
-						  vel->climb,
-						  sigma->of_horizontal_pos,
-						  sigma->of_speed);
-			__spherical_to_tangent(pos, vel, sigma, pm, vm, s2pm, s2vm);
-			fl_real vh = fl_km_h_to_m_s(vel->speed);
-		#if (FL_KALMAN_ANGULAR_VELOCITY)
-			fl_real vhe = sqrt(fl_square(__se.x[GEO_SPATIAL_X][STATE_V]) + fl_square(__se.x[GEO_SPATIAL_Y][STATE_V]));
-			fl_real b   = fl_degrees_to_radians(vel->direction);
-			if (vh > PRECISION && __time_of.last_b_fixing > FL_UNDEFINED_TIME) {
-				fl_real dt = t - __time_of.last_b_fixing;
-				if (vh * vhe * dt > 0) {
-					fl_radians db;
-						fl_real wzm, s2wzm;
-
-					/* circular difference */
-					db = b - __be;
-					if (db > M_PI)
-						db -= M_2PI;
-					else if (db < -M_PI)
-						db += M_2PI;
-
-					wzm = db / dt;
-					s2wzm = 4 * fl_square(atan2(2 * sqrt(vh * vhe), FL_DEFAULT_VELOCITY_SIGMA));
-					__kalman_wz(t, wzm, s2wzm);
-				}
-			}
-			__be = b;
-			__time_of.last_b_fixing = t;
-		#endif
-			__kalman_pv(t, pm, vm, s2pm, s2vm);
-			__create_push_forward_map(__se.x, pos->status);
-			memcpy(__sp.x, __se.x, sizeof(__sp.x));
-			__signal_updated_location(t);
-		} else {
-			LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(location.pos.status=%d)"), pos->status);
-		}
-	} else {
-		LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%p, %p): INVALID ARGUMENT"), pos, sigma);
-	}
-}
-
-void fused_engine_get_location(fl_spherical_position* pos, fl_tangent_velocity* vel, fl_sigma* sigma)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(), __last_notification.time=%gs"), __last_notification.time);
-	__tangent_to_spherical(__sp.x, __sp.s2x, pos, vel, sigma);
-}
-
-PRIVATE void __get_prediction_vector(fl_seconds t, fl_position_4d* prediction)
-{
-	prediction->time = t;
-	/* pull-back map: from the old tangent frame back to global coordinates */
-	prediction->spatial[GEO_SPATIAL_X] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X];
-	prediction->spatial[GEO_SPATIAL_Y] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y];
-	prediction->spatial[GEO_SPATIAL_Z] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z];
-}
-
-fl_real fl_kalf_get_distance(const_fl_position_vector_ref p, const_fl_position_vector_ref q)
-{
-	return sqrt(fl_square(p[GEO_SPATIAL_X] - q[GEO_SPATIAL_X])
-			  + fl_square(p[GEO_SPATIAL_Y] - q[GEO_SPATIAL_Y])
-			  + fl_square(p[GEO_SPATIAL_Z] - q[GEO_SPATIAL_Z]));
-}
-
-PRIVATE void __quaternion_to_rotation(_const_fl_kalf_quaternion_ref q, fl_matrix_3d_ref out)
-{
-	out[GEO_SPATIAL_X][GEO_SPATIAL_X] = SQUARE(q[QTR_0]) + SQUARE(q[QTR_X]) - SQUARE(q[QTR_Y]) - SQUARE(q[QTR_Z]);
-	out[GEO_SPATIAL_X][GEO_SPATIAL_Y] = 2 * (q[QTR_X] * q[QTR_Y] + q[QTR_Z] * q[QTR_0]);
-	out[GEO_SPATIAL_X][GEO_SPATIAL_Z] = 2 * (q[QTR_X] * q[QTR_Z] - q[QTR_Y] * q[QTR_0]);
-
-	out[GEO_SPATIAL_Y][GEO_SPATIAL_X] = 2 * (q[QTR_Y] * q[QTR_X] - q[QTR_Z] * q[QTR_0]);
-	out[GEO_SPATIAL_Y][GEO_SPATIAL_Y] = SQUARE(q[QTR_0]) - SQUARE(q[QTR_X]) + SQUARE(q[QTR_Y]) - SQUARE(q[QTR_Z]);
-	out[GEO_SPATIAL_Y][GEO_SPATIAL_Z] = 2 * (q[QTR_Y] * q[QTR_Z] + q[QTR_X] * q[QTR_0]);
-
-	out[GEO_SPATIAL_Z][GEO_SPATIAL_X] = 2 * (q[QTR_Z] * q[QTR_X] + q[QTR_Y] * q[QTR_0]);
-	out[GEO_SPATIAL_Z][GEO_SPATIAL_Y] = 2 * (q[QTR_Z] * q[QTR_Y] - q[QTR_X] * q[QTR_0]);
-	out[GEO_SPATIAL_Z][GEO_SPATIAL_Z] = SQUARE(q[QTR_0]) - SQUARE(q[QTR_X]) - SQUARE(q[QTR_Y]) + SQUARE(q[QTR_Z]);
-}
-
-//PRIVATE void __process_linear_acceleration(fl_seconds t, const_fl_acceleration_vector_ref al)
-//{
-//	unsigned i;
-//
-//	for (i = GEO_SPATIAL_DIMENSION;  i--;) {
-//		__se.  x[i][STATE_A] = 0;
-//		__se.s2x[i][STATE_A][STATE_A] = DEFAULT_ACCELERATION_SIGMA2;
-//	}
-//	__predict_pv(t);
-//	__time_of.last_a_fixing  = t;
-//	__signal_updated_location(t);
-//}
-
-void fused_engine_process_sensory_event(fl_seconds t, fl_sensory_source sensor_id, const_sensor_vector_3d_ref data)
-{
-	if (__time_of.sensor_event.count == 0)
-		__time_of.sensor_event.first = t;
-	__time_of.sensor_event.last = t;
-	__time_of.sensor_event.count++;
-
-	if (__time_of.last_measurement > FL_UNDEFINED_TIME) {
-		fl_seconds dt = t - (__time_of.last_measurement - __get_time_offset());
-		if (dt < 0)
-			__new_time_offset(__time_of.last_measurement - t);
-	}
-	if (data) {
-		switch (sensor_id) {
-		case ACCELEROMETER: {
-			fl_acceleration_vector am;
-			fl_acceleration_vector al;
-			fl_real                w2gu;
-			fl_vector_3d           gu;
-			unsigned               i;
-
-			LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", RAC, a=(%g, %g, %g)[m/s^2])"), t, data[DEV_SPATIAL_X], data[DEV_SPATIAL_Y], data[DEV_SPATIAL_Z]);
-			/* extrapolate the rotation matrix */
-			__predict_rw(t);
-
-			/* float, math system -> double, geographic system, rotated into local (tangent) frame */
-			am[GEO_SPATIAL_X] = __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z];
-			am[GEO_SPATIAL_Y] = __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z];
-			am[GEO_SPATIAL_Z] = __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z];
-			/* pass through gravity filter to estimate |g| and downward direction */
-			fl_accel_process(t, am, al, gu, &w2gu);
-			if (w2gu > PRECISION2) {
-				fl_vector_3d gn;
-
-				/* rotate back to device frame */
-				VECTOR_DOT_MATRIX_3D(gu, __srp.m, gn);
-				/* w2gn = w2gu because w2gu is diagonal and srp orthogonal */
-				/* update rotation matrix */
-				__inv_kalman_u(t, gn, w2gu, __srp.m[GEO_SPATIAL_Z], __srp.w2[GEO_SPATIAL_Z], __sre.m[GEO_SPATIAL_Z], &__sre.w2[GEO_SPATIAL_Z]);
-				__cross_product(__sre.m[GEO_SPATIAL_Z], __srp.m[GEO_SPATIAL_X], __sre.m[GEO_SPATIAL_Y]);
-				__cross_product(__sre.m[GEO_SPATIAL_Y], __sre.m[GEO_SPATIAL_Z], __sre.m[GEO_SPATIAL_X]);
-
-				__copy_system_rotation(&__srp, &__sre);
-				__time_of.last_r_fixing = t;
-			}
-			/* process linear acceleration */
-			for (i = GEO_SPATIAL_DIMENSION;  i--;) {
-				__se.  x[i][STATE_A] = 0;
-				__se.s2x[i][STATE_A][STATE_A] = DEFAULT_ACCELERATION_SIGMA2;
-			}
-			__predict_pv(t);
-			__time_of.last_a_fixing = t;
-			__signal_updated_location(t);
-		//	__process_linear_acceleration(t, al);
-			return;
-		}
-		case GYROSCOPE: {
-			fl_spin_vector  wm;
-
-			/* float, degrees, math/device coordinates (right-handed) -> double, radians, geographic/tangent coordinates (left-handed)  */
-			wm[GEO_SPATIAL_X] = -fl_degrees_to_radians(data[DEV_SPATIAL_X]);
-			wm[GEO_SPATIAL_Y] = -fl_degrees_to_radians(data[DEV_SPATIAL_Y]);
-			wm[GEO_SPATIAL_Z] = -fl_degrees_to_radians(data[DEV_SPATIAL_Z]);
-			LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", GYR, w=(%g, %g, %g)[rad/s])"), t, wm[GEO_SPATIAL_X], wm[GEO_SPATIAL_Y], wm[GEO_SPATIAL_Z]);
-			__integrate_rw(t, fl_gyro_process(t, wm), DEFAULT_ROTATION_SIGMA2);
-			return;
-		}
-		default:
-			/* prevent compiler warning */
-			return;
-		}
-	}
-	LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(t=" TIME_FORMAT ", sensor_id=%d, data=%p)"), t, sensor_id, data);
-}
-
-PRIVATE void __integrate_rw(fl_seconds t, const_fl_spin_vector_ref wm, fl_real s2wm)
-{
-	unsigned i;
-
-	if (__time_of.last_w_fixing > FL_UNDEFINED_TIME) {
-		fl_seconds dt;
-
-		dt = t - __time_of.last_w_fixing;
-		if (dt > 0) {
-			fl_real w2;
-
-			__wp[GEO_SPATIAL_X] = 0.5 * (__we[GEO_SPATIAL_X] + wm[GEO_SPATIAL_X]);
-			__wp[GEO_SPATIAL_Y] = 0.5 * (__we[GEO_SPATIAL_Y] + wm[GEO_SPATIAL_Y]);
-			__wp[GEO_SPATIAL_Z] = 0.5 * (__we[GEO_SPATIAL_Z] + wm[GEO_SPATIAL_Z]);
-			w2 = fl_vector_3d_length2(__wp);
-			fl_real wz = __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_X] * __wp[GEO_SPATIAL_X]
-					   + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * __wp[GEO_SPATIAL_Y]
-					   + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * __wp[GEO_SPATIAL_Z];
-
-		#if (FL_KALMAN_ANGULAR_VELOCITY)
-			__kalman_wz(t, wz, SQUARE(FL_DEFAULT_SPIN_SIGMA));
-		#else
-			__wze = wz;
-		#endif
-			__predict_pv(t);
-			__se.s2x[GEO_SPATIAL_X][STATE_A][STATE_A] = fl_square(__sp.x[GEO_SPATIAL_X][STATE_A] - __se.x[GEO_SPATIAL_X][STATE_A]);
-			__se.s2x[GEO_SPATIAL_Y][STATE_A][STATE_A] = fl_square(__sp.x[GEO_SPATIAL_Y][STATE_A] - __se.x[GEO_SPATIAL_Y][STATE_A]);
-			__copy_state(&__se, &__sp);
-			__time_of.last_reference = t;
-
-			/* Integrate over dt: We use the fact that for any scalar t the quaternions
-			 *   [exp(w), exp(tw)] = 0
-			 * commute.
-			 */
-			fl_real dt2 = dt * dt / 24;
-
-			if (w2 > PRECISION2) {
-				fl_real             w;
-				fl_real             wt, swtw;
-				_fl_kalf_quaternion qwt;
-				fl_matrix_3d        dsr;
-				fl_real             qwt2;
-
-				w = sqrt(w2);
-				wt = w * dt * 0.5;
-				swtw = sin(wt) / w; /* system rotates opposite to the device */
-				qwt[QTR_0] = cos(wt);
-				qwt[QTR_X] = swtw * __wp[GEO_SPATIAL_X] + dt2 * (__we[GEO_SPATIAL_Y] * wm[GEO_SPATIAL_Z] - __we[GEO_SPATIAL_Z] * wm[GEO_SPATIAL_Y]);
-				qwt[QTR_Y] = swtw * __wp[GEO_SPATIAL_Y] + dt2 * (__we[GEO_SPATIAL_Z] * wm[GEO_SPATIAL_X] - __we[GEO_SPATIAL_X] * wm[GEO_SPATIAL_Z]);
-				qwt[QTR_Z] = swtw * __wp[GEO_SPATIAL_Z] + dt2 * (__we[GEO_SPATIAL_X] * wm[GEO_SPATIAL_Y] - __we[GEO_SPATIAL_Y] * wm[GEO_SPATIAL_X]);
-				qwt2 = fl_square(qwt[QTR_0]) + fl_vector_3d_length2(&qwt[QTR_X]);
-				if (qwt2 > PRECISION2) {
-					fl_real norm_qwt = 1.0 / sqrt(qwt2);
-
-					qwt[QTR_0] *= norm_qwt;
-					qwt[QTR_X] *= norm_qwt;
-					qwt[QTR_Y] *= norm_qwt;
-					qwt[QTR_Z] *= norm_qwt;
-				}
-				__quaternion_to_rotation(qwt, dsr);
-				__matrix_3d_multiply_matrix_3d(__srp.m, dsr, __sre.m);
-				__copy_system_rotation(&__srp, &__sre);
-				__time_of.last_r_fixing = t;
-			}
-		}
-	}
-	i = GEO_SPATIAL_DIMENSION;
-	do {
-		--i;
-		// Kalmaning the rotation rates would amount to an EMA low-pass filter - we don't want it
-		__wp[i] = __we[i] = wm[i];
-	} while (i);
-	__s2we = s2wm;
-	__time_of.last_w_fixing = t;
-}
-
-PRIVATE void __predict_rw(fl_seconds t)
-{
-	unsigned   i;
-
-	if (__time_of.last_w_fixing > FL_UNDEFINED_TIME) {
-		fl_seconds dt;
-
-		dt = t - __time_of.last_w_fixing;
-		if (dt > 0) {
-			i = GEO_SPATIAL_DIMENSION;
-			do {
-				--i;
-				__wp[i] = __we[i];
-			} while (i);
-
-			/* Integrate over dt: We use the fact that for any scalar t the quaternions
-			 *   [exp(w), exp(tw)] = 0
-			 * commute.
-			 */
-			fl_real w2;
-
-			w2 = fl_vector_3d_length2(__wp);
-			if (w2 > PRECISION2) {
-				fl_real             w;
-				fl_real             wt, swt;
-				_fl_kalf_quaternion qwt;
-				fl_matrix_3d        srp; /* local copy */
-				fl_matrix_3d        dsr;
-
-				w = sqrt(w2);
-				wt = w * dt * 0.5;
-				swt = sin(wt) / w;
-				qwt[QTR_0] = cos(wt);
-				qwt[QTR_X] = swt * __wp[GEO_SPATIAL_X];
-				qwt[QTR_Y] = swt * __wp[GEO_SPATIAL_Y];
-				qwt[QTR_Z] = swt * __wp[GEO_SPATIAL_Z];
-				__quaternion_to_rotation(qwt, dsr);
-				/* inlined memcpy() */
-				i = sizeof(__srp.m) / sizeof(fl_real);
-				do {
-					--i;
-					srp[0][i] = __srp.m[0][i];
-				} while (i);
-				__matrix_3d_multiply_matrix_3d(srp, dsr, __srp.m);
-			}
-		}
-	}
-	/* otherwise there's no q change */
-}
-
-PRIVATE void __predict_pv(fl_seconds t)
-{
-	fl_seconds dt;
-	unsigned i;
-
-	if (__time_of.last_reference > FL_UNDEFINED_TIME)
-		dt = t - __time_of.last_reference;
-	else
-		/* no prior data */
-		return;
-
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT "), dt=" TIME_FORMAT), t, dt);
-	if (dt > PRECISION2) {
-		fl_real wt = __wze * dt;
-		if (fabs(__wze) > PRECISION) {
-			fl_real cwt, swt;
-			fl_real cwtw, swtw;
-			fl_real inv_wz;
-
-			cwt = cos(wt);
-			swt = sin(wt);
-			inv_wz = 1.0 / __wze;
-			cwtw = (cwt - 1) * inv_wz;
-			swtw =  swt      * inv_wz;
-
-			__sp.x[GEO_SPATIAL_X][STATE_P] = __se.x[GEO_SPATIAL_X][STATE_V] * swtw + __se.x[GEO_SPATIAL_Y][STATE_V] * cwtw + __se.x[GEO_SPATIAL_X][STATE_P];
-			__sp.x[GEO_SPATIAL_Y][STATE_P] = __se.x[GEO_SPATIAL_Y][STATE_V] * swtw - __se.x[GEO_SPATIAL_X][STATE_V] * cwtw + __se.x[GEO_SPATIAL_Y][STATE_P];
-			__sp.x[GEO_SPATIAL_X][STATE_V] = __se.x[GEO_SPATIAL_X][STATE_V] * cwt  - __se.x[GEO_SPATIAL_Y][STATE_V] * swt;
-			__sp.x[GEO_SPATIAL_Y][STATE_V] = __se.x[GEO_SPATIAL_Y][STATE_V] * cwt  + __se.x[GEO_SPATIAL_X][STATE_V] * swt;
-		} else {
-			__sp.x[GEO_SPATIAL_X][STATE_P] = __se.x[GEO_SPATIAL_X][STATE_P] - dt * (wt * __se.x[GEO_SPATIAL_Y][STATE_V] * 0.5 - __se.x[GEO_SPATIAL_X][STATE_V]);
-			__sp.x[GEO_SPATIAL_Y][STATE_P] = __se.x[GEO_SPATIAL_Y][STATE_P] + dt * (wt * __se.x[GEO_SPATIAL_X][STATE_V] * 0.5 + __se.x[GEO_SPATIAL_Y][STATE_V]);
-			__sp.x[GEO_SPATIAL_X][STATE_V] = __se.x[GEO_SPATIAL_X][STATE_V] - wt * (wt * __se.x[GEO_SPATIAL_X][STATE_V] * 0.5 + __se.x[GEO_SPATIAL_Y][STATE_V]);
-			__sp.x[GEO_SPATIAL_Y][STATE_V] = __se.x[GEO_SPATIAL_Y][STATE_V] - wt * (wt * __se.x[GEO_SPATIAL_Y][STATE_V] * 0.5 - __se.x[GEO_SPATIAL_X][STATE_V]);
-		}
-		__sp.x[GEO_SPATIAL_Z][STATE_P] = __se.x[GEO_SPATIAL_Z][STATE_P] + dt * __se.x[GEO_SPATIAL_Z][STATE_V];
-		__sp.x[GEO_SPATIAL_Z][STATE_V] = __se.x[GEO_SPATIAL_Z][STATE_V];
-
-		__sp.x[GEO_SPATIAL_X][STATE_A] = -__sp.x[GEO_SPATIAL_Y][STATE_V] * __wze;
-		__sp.x[GEO_SPATIAL_Y][STATE_A] =  __sp.x[GEO_SPATIAL_X][STATE_V] * __wze;
-		__sp.x[GEO_SPATIAL_Z][STATE_A] = 0;
-
-		for (i = GEO_SPATIAL_DIMENSION;  i;) {
-			--i;
-
-			_const_fl_kalf_sigma_matrix_ref s2ei = __se.s2x[i];
-				  _fl_kalf_sigma_matrix_ref s2pi = __sp.s2x[i];
-
-			s2pi[STATE_A][STATE_A] =  s2ei[STATE_A][STATE_A];
-			s2pi[STATE_A][STATE_V] =  s2ei[STATE_A][STATE_V]
-								   +  s2ei[STATE_A][STATE_A] * dt;
-			s2pi[STATE_A][STATE_P] =  s2ei[STATE_A][STATE_P]
-								   + (s2ei[STATE_A][STATE_V]
-								   +  s2ei[STATE_A][STATE_A] * dt / 2) * dt;
-
-			s2pi[STATE_V][STATE_A] =  s2pi[STATE_A][STATE_V]; /* by symmetry */
-			s2pi[STATE_V][STATE_V] =  s2ei[STATE_V][STATE_V]
-								   + (s2ei[STATE_V][STATE_A] * 2
-								   +  s2ei[STATE_A][STATE_A] * dt) * dt;
-			s2pi[STATE_V][STATE_P] =  s2ei[STATE_V][STATE_P]
-								   + (s2ei[STATE_V][STATE_V]
-								   +  s2ei[STATE_P][STATE_A]
-								   + (s2ei[STATE_V][STATE_A] * 3
-								   +  s2ei[STATE_A][STATE_A] * dt) / 2 * dt) * dt;
-
-			s2pi[STATE_P][STATE_A] =  s2pi[STATE_A][STATE_P]; /* by symmetry */
-			s2pi[STATE_P][STATE_V] =  s2pi[STATE_V][STATE_P]; /* by symmetry */
-			s2pi[STATE_P][STATE_P] =  s2ei[STATE_P][STATE_P]
-								   + (s2ei[STATE_P][STATE_V] * 2
-								   + (s2ei[STATE_V][STATE_V]
-								   +  s2ei[STATE_A][STATE_P]
-								   + (s2ei[STATE_A][STATE_V]
-								   +  s2ei[STATE_A][STATE_A] * dt / 4) * dt) * dt) * dt;
-		}
-		// process noise
-		__sp.s2x[GEO_SPATIAL_X][STATE_A][STATE_A] += fl_square(__sp.x[GEO_SPATIAL_X][STATE_A] - __se.x[GEO_SPATIAL_X][STATE_A]);
-		__sp.s2x[GEO_SPATIAL_Y][STATE_A][STATE_A] += fl_square(__sp.x[GEO_SPATIAL_Y][STATE_A] - __se.x[GEO_SPATIAL_Y][STATE_A]);
-		__rectify_state(&__sp);
-	}
-}
diff --git a/lbs-server/src/fused/kalman-filter.h b/lbs-server/src/fused/kalman-filter.h
deleted file mode 100644
index 42fde96..0000000
--- a/lbs-server/src/fused/kalman-filter.h
+++ /dev/null
@@ -1,619 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    kalman-filter.h
- * @brief   Implementation of a 4D (2D position + 2D velocity) Kalman filter
- */
-
-#pragma once
-#ifndef __FL_KALMAN_FILTER_H__
-#define __FL_KALMAN_FILTER_H__
-
-#include <glib.h>
-#include "types.h"
-#include "parameters.h"
-#include "motion-detector.h"
-#include "aema-estimator.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/***************************************************************************//**
- * [public] Initializaton of the singleton unit (static constructor). This
- * corresponds to service load.
- *
- * @param[in] on_motion_state_changed
- *      Callback to be invoked when the detected state of motion changes. This
- *      argument is optional, and can be NULL.
- * @param[in] on_location_changed
- *      Callback to be invoked when the predicted or updated location changes.
- *      This argument is required.
- * @param[in] handler
- *      Arbitrary user parameter passed to the callback.
- */
-void fused_engine_init(
-	motion_changed_cb on_motion_state_changed,
-	predicted_updated_cb     on_location_changed,
-	gpointer                handler);
-
-/***************************************************************************//**
- * [public] Cleanup of the singleton unit (static destructor). This corresponds
- * to service unload.
- */
-void fused_engine_exit(void);
-
-/***************************************************************************//**
- * [public] Start processing the location and sensory inputs, and sending back
- * notifications. This corresponds to service start.
- */
-void fused_engine_start(void);
-
-/***************************************************************************//**
- * [public] Stops processing the inputs, and sending notifications. This
- * corresponds to service stop.
- */
-void fused_engine_stop(void);
-
-/***************************************************************************//**
- * [public] Reset of the internal state back to the initial one. The handlers
- * and start/stop state are not changed. This function is used for
- * initialization, soft restarts, and repetitive testing.
- */
-void fl_kalf_reset(void);
-
-/***************************************************************************//**
- * [public] Process 2D (horizontal) position without change of the altitude or
- * speed (WPS).	Kalman merging is carried out in tangent frame, and the origin
- * is moved after each measurement so that the predictions always start at
- * (0, 0, R + h).
- *      - Clocks synchronization
- *      - Kalman data blending
- *      - Repositioning of the tangent frame
- *
- * @param[in] pos
- *      Spherical position in geographic coordinates
- * @param[in] sigma
- *      Level and standard deviations.
- */
-void fused_engine_process_position_2d_event(const fl_spherical_position* pos, const fl_sigma* sigma);
-
-/***************************************************************************//**
- * [public] Process 3D position along with 3D velocity (GPS). Kalman merging is
- * carried out in tangent frame, and the origin is moved after each measurement
- * so that the predictions always start at (0, 0, R + h).
- *      - Clocks synchronization
- *      - Kalman data blending
- *      - Repositioning of the tangent frame
- * If FL_KALMAN_ANGULAR_VELOCITY is enabled the angular velocity is derived
- * from consecutive measurements of linear velocities and merged with gyroscope
- * readings.
- *
- * @param[in] pos
- *      Spherical position in geographic coordinates, and altitude
- * @param[in] vel
- *      Horizontal and vertical velocity (speed and climb)
- * @param[in] sigma
- *      Level and standard deviations.
- */
-void fused_engine_process_position_2x3d_event(const fl_spherical_position* pos, const fl_tangent_velocity* vel, const fl_sigma* sigma);
-
-/***************************************************************************//**
- * [public] Process sensory events (acceleration & gyrocope). The time offest is
- * corrected if necessary.
- *      - Clocks synchronization
- *      - Device-to-tangent frame orientation
- *      - Prediction
- *
- * @param[in] t
- *      Sensory time-stamp
- * @param[in] sensor_id
- *      Sensor identifier
- * @param[in] data
- *      Vector of sensor-specific data.
- */
-void fused_engine_process_sensory_event(fl_seconds t, fl_sensory_source sensor_id, const_sensor_vector_3d_ref data);
-
-/***************************************************************************//**
- * [public] Retrieve the last processed location, typically during the
- * on_location_changed notification. This two-step procedure of extracting the
- * location allows to spare the non-trivial conversion between internal and
- * interface formats whenever the user code decides not to forward it through
- * the API.
- *
- * @param[out] pos
- *      Current position in API format
- * @param[in] sensor_id
- *      Current velocity in API format
- * @param[in] data
- *      Current uncertainty in API format
- */
-void fused_engine_get_location(fl_spherical_position* pos, fl_tangent_velocity* vel, fl_sigma* sigma);
-
-/***************************************************************************//**
- * [public] Compute distance along the Earth great arc between two arbitrary
- * points. Notice that the altitude is ignored, i.e. this is metric is of a
- * projective space.
- *
- * @param[out] p
- *      Position 3D vector in global Cartesian coordinate frame.
- * @param[out] q
- *      Position 3D vector in global Cartesian coordinate frame.
- *
- * @return
- *      Spherical distance between @a p and @a q.
- */
-fl_real fl_kalf_get_distance(const_fl_position_vector_ref p, const_fl_position_vector_ref q);
-
-#if defined(__FL_KALMAN_FILTER_C__)
-	#define INLINE \
-	inline
-#else
-	#define INLINE \
-	inline
-#endif
-#if defined(__FL_KALMAN_FILTER_C__)
-	#ifdef GTEST
-		#define PRIVATE
-	#else
-		#define PRIVATE \
-		static
-	#endif
-#else
-	#define PRIVATE
-#endif
-
-#define DEFAULT_ACCELERATION_SIGMA2      SQUARE(FL_ACCEL_NOISE_LEVEL)  /* [(m/s^2)^2] */
-#define DEFAULT_ROTATION_SIGMA2          SQUARE(1.0 / 256) /* [(rad/s)^2] */
-
-typedef enum {
-	STATE_P, /* position     [m]     */
-	STATE_V, /* velocity     [m/s]   */
-	STATE_A, /* acceleration [m/s^2] */
-	/* always the last */
-	STATE_DIMENSION
-} _fl_kalf_state_component;
-
-typedef             fl_real  _fl_kalf_state_matrix[GEO_SPATIAL_DIMENSION][STATE_DIMENSION];
-typedef             fl_real(*_fl_kalf_state_matrix_ref)[STATE_DIMENSION];
-typedef fl_real(*const _const_fl_kalf_state_matrix_ref)[STATE_DIMENSION];
-
-typedef             fl_real  _fl_kalf_state_vector[GEO_SPATIAL_DIMENSION];
-typedef             fl_real(*_fl_kalf_state_vector_ref);
-typedef const fl_real(*_const_fl_kalf_state_vector_ref);
-
-typedef enum {
-	QTR_X,
-	QTR_Y,
-	QTR_Z,
-	QTR_0,
-	/* always the last */
-	QUATERNION_DIMENSION
-} _fl_kalf_quaternion_component;
-
-typedef             fl_real  _fl_kalf_quaternion[QUATERNION_DIMENSION];
-typedef             fl_real(*_fl_kalf_quaternion_ref);
-typedef const fl_real(*_const_fl_kalf_quaternion_ref);
-
-//! position on the Earth surface in Cartesian coordinates
-typedef fl_meters position_vector[GEO_SPATIAL_DIMENSION];
-typedef struct {
-	     fl_seconds  time;
-	position_vector  position;
-} fl_prediction;
-
-typedef             fl_real  _fl_kalf_matrix_2d[PLANAR_DIMENSION][PLANAR_DIMENSION];
-typedef             fl_real(*_fl_kalf_matrix_2d_ref)[PLANAR_DIMENSION];
-typedef fl_real(*const _const_fl_kalf_matrix_2d_ref)[PLANAR_DIMENSION];
-
-typedef             fl_real  _fl_kalf_sigma_matrix[STATE_DIMENSION][STATE_DIMENSION];
-typedef             fl_real(*_fl_kalf_sigma_matrix_ref)[STATE_DIMENSION];
-typedef fl_real(*const _const_fl_kalf_sigma_matrix_ref)[STATE_DIMENSION];
-
-typedef _fl_kalf_state_matrix  _fl_kalf_sigma_cube[GEO_SPATIAL_DIMENSION];
-typedef _fl_kalf_state_matrix(*_fl_kalf_sigma_cube_ref);
-
-
-typedef struct {
-			 fl_matrix_3d m;     /* rotation matrix */
-			 fl_vector_3d w2;    /* square weight (inverse covariance) vector of matrix rows */
-} _fl_kalf_system_rotation;
-
-typedef struct {
-	_fl_kalf_state_matrix x;     /* state vector */
-	  _fl_kalf_sigma_cube s2x;   /* covariance matrix */
-} _fl_kalf_state;
-
-/** Tangent space T(p)E to the embedding manifold E=SxR at point p */
-typedef struct {
-			   fl_radians rx;    /* latitude */
-			   fl_radians ry;    /* longitude */
-				  fl_real srx;   /* sin(latitude) */
-				  fl_real crx;   /* cos(latitude) */
-			 fl_matrix_3d fsr;   /* push-forward (tangent to global) coordinate system rotation matrix */
-} _fl_kalf_tangent_frame;
-
-typedef struct {
-			   fl_seconds first; /* arrival of the first sensor event */
-			   fl_seconds last;  /* arrival of the last sensor event */
-				 unsigned count; /* at 100Hz rate of arrival this will suffice for ~32 years */
-} _fl_kalf_sensor_times;
-
-typedef struct {
-			   fl_seconds last_measurement;  /* arrival of the last position measurement event */
-	_fl_kalf_sensor_times sensor_event;
-			   fl_seconds last_p_fixing;     /* update of reference p (pe) */
-			   fl_seconds last_v_fixing;     /* update of reference v (ve) => p */
-			   fl_seconds last_a_fixing;     /* update of reference a (ae) => v, p */
-			   fl_seconds last_r_fixing;     /* update of reference sr (sre) */
-			   fl_seconds last_r_diffusion;  /* damping of sr weights */
-			   fl_seconds last_w_fixing;     /* update of reference w (we) => q, sr */
-#if (FL_KALMAN_ANGULAR_VELOCITY)
-			   fl_seconds last_wz_fixing;    /* update of reference wz */
-			   fl_seconds last_b_fixing;     /* update of reference b */
-#endif
-			   fl_seconds last_reference;    /* update of __se */
-} _fl_kalf_time_of;
-
-PRIVATE                 gboolean __started;
-PRIVATE         _fl_kalf_time_of __time_of;
-PRIVATE      fl_aema1d_estimator __timestamp_offset;
-PRIVATE           fl_position_4d __last_notification;
-PRIVATE     predicted_updated_cb __on_location_changed;
-PRIVATE                 gpointer __handler; /* location event handler */
-PRIVATE   _fl_kalf_tangent_frame __te;      /* tangent frame map */
-PRIVATE _fl_kalf_system_rotation __srp;     /* predicted system rotation */
-PRIVATE _fl_kalf_system_rotation __sre;     /* reference system rotation */
-PRIVATE           _fl_kalf_state __sp;      /* predicted 3D state */
-PRIVATE           _fl_kalf_state __se;      /* reference 3D state */
-PRIVATE             fl_vector_3d __wp;      /* predicted angular velocity */
-PRIVATE             fl_vector_3d __we;      /* reference angular velocity */
-PRIVATE                  fl_real __s2wp;
-PRIVATE                  fl_real __s2we;
-PRIVATE                  fl_real __wze;     /* reference angular z-velocity (horizontal plane) */
-#if (FL_KALMAN_ANGULAR_VELOCITY)
-PRIVATE                  fl_real __s2wze;
-PRIVATE               fl_radians __be;   /* reference North bearing */
-#endif
-PRIVATE           LocationStatus __fix_status; /* pass-through */
-
-/***************************************************************************//**
- * [private] Check for and correct the numerical errors which cause the
- * covariance matrix to cease being positive-definite. This operation has to be
- * performed regularly, or otherwise magic ensues, and every strange result
- * becomes possible.
- *
- * @param[in, out] src
- *      The state object to be rectified. This is either the predicted (__sp),
- *      or estimated (__se) one.
- */
-PRIVATE void __rectify_state(_fl_kalf_state *src);
-
-/***************************************************************************//**
- * [private] Get the latest estimate of the difference between location and
- * sensory clocks.
- *
- * @return
- *      Current time-offset value.
- */
-PRIVATE INLINE fl_seconds __get_time_offset(void)
-{
-	return __timestamp_offset.value;
-}
-
-/***************************************************************************//**
- * [private] Convert location timestamp to internal time in seconds. The
- * time-offset is adjusted if necessary to ensure temporal ordering of events.
- *
- * @param[in] timestamp
- *      Location timestamp (wall clock since 1970)
- *
- * @return
- *      Internal time.
- */
-PRIVATE fl_seconds __timestamp_to_seconds(fl_timestamp timestamp);
-
-/***************************************************************************//**
- * [private] Efficient (faster than memcpy) copy of the system rotation object.
- *
- * @param[out] dst
- *      Destination system rotation object
- * @param[out] src
- *      Source system rotation object
- */
-PRIVATE INLINE void __copy_system_rotation(_fl_kalf_system_rotation *dst, const _fl_kalf_system_rotation *src)
-{
-	unsigned i = sizeof(_fl_kalf_system_rotation) / sizeof(unsigned);
-	do {
-		--i;
-		((unsigned*)dst)[i] = ((const unsigned*)src)[i];
-	} while (i);
-}
-
-/***************************************************************************//**
- * [private] Efficient (faster than memcpy) copy of the state (3x3D position &
- * covariance) object.
- *
- * @param[out] dst
- *      Destination state object
- * @param[out] src
- *      Source state object
- */
-PRIVATE INLINE void __copy_state(_fl_kalf_state *dst, _fl_kalf_state *src)
-{
-	__rectify_state(src);
-	unsigned i = sizeof(_fl_kalf_state) / sizeof(unsigned);
-	do {
-		--i;
-		((unsigned*)dst)[i] = ((const unsigned*)src)[i];
-	} while (i);
-}
-
-/***************************************************************************//**
- * [private] Approximate squared weigt (inverse covariance) of a variable which
- * is a product of two unit vertors x = u*v, |u|=1, |v|=1. The exact expression is anything but
- * trivial.
- *
- * @param[in] w2u
- *      Inverse covariance of u: w2u = 1/(<u^2> - <u>^2)
- * @param[in] w2v
- *      Inverse covariance of v: w2v = 1/(<v^2> - <v>^2)
- *
- * @return
- *      Estimated weight of the product.
- */
-PRIVATE INLINE fl_real __product_weights2(fl_real w2u, fl_real w2v)
-{
-	return w2u * w2v / (w2u + w2v);
-}
-
-/***************************************************************************//**
- * [private] 3D cross-product of two unit vectors. The resultant vector is
- * normalized to unity, in case @a u1 and @a u2 are not orthogonal.
- *
- * @param[in] u1
- *      Input unit vector
- * @param[in] u2
- *      Input unit vector
- * @param[out] v
- *      Output unit vector
- */
-PRIVATE void __cross_product(const_fl_vector_3d_ref u1, const_fl_vector_3d_ref u2, fl_vector_3d_ref v);
-
-/***************************************************************************//**
- * [private] Create coordinate map from spherical M = S^2 x R to the tangent
- * space TM. Notice that only two angles are being used, what means the course
- * on the tangent plane is not straight North (x-axis), but still described by
- * the bearing.
- *
- * @param[in] x
- *     Position and velocity in old tangent frame
- * @param[out] status
- *     Status of the supplied data
- */
-PRIVATE void __create_push_forward_map(_fl_kalf_state_matrix_ref x, LocationStatus status);
-
-/***************************************************************************//**
- * [private] Convert API location to internal representation, i.e. perform
- * conversion of:
- *  - units to SI;
- *  - coordinate system M -> TM.
- *
- * @param[in] pos
- *      Position in API format
- * @param[in] vel
- *      Velocity in API format
- * @param[in] sigma
- *      Uncertainty in API format
- * @param[out] pm
- *      3D position on TM in SI units
- * @param[out] vm
- *      3D velocity on TM in SI units
- * @param[out] s2pm
- *      3D position variances on TM in SI units
- * @param[out] s2vm
- *      3D velocity variances on TM in SI units
- */
-PRIVATE void __spherical_to_tangent(
-	const fl_spherical_position *pos,
-	const fl_tangent_velocity   *vel,
-	const fl_sigma              *sigma,
-	fl_position_vector_ref       pm,
-	fl_velocity_vector_ref       vm,
-	fl_vector_3d_ref             s2pm,
-	fl_vector_3d_ref             s2vm);
-
-/***************************************************************************//**
- * [private] Convert tangent frame data to API location representation, i.e.
- * perform conversion of:
- *  - units from SI to API (degrees, km/h, timestamps);
- *  - coordinate system TM -> M.
- *
- * @param[in] x
- *      3x3 state matrix
- * @param[in] s2x
- *      3x3x3 covariance cube
- * @param[out] pos
- *      Position in API format
- * @param[out] vel
- *      Velocity in API format
- * @param[out] sigma
- *      Uncertainty in API format
- */
-PRIVATE void __tangent_to_spherical(
-	const _fl_kalf_state_matrix_ref x,
-	const _fl_kalf_sigma_cube_ref   s2x,
-	fl_spherical_position          *pos,
-	fl_tangent_velocity            *vel,
-	fl_sigma                       *sigma);
-
-/***************************************************************************//**
- * [private] Retrieve the current prediction in global frame M.
- *
- * @param[in] t
- *      Internal time
- * @param[out] prediction
- *      Spatio-temporal (4D) position in global coordinates.
- */
-PRIVATE void __get_prediction_vector(fl_seconds t, fl_position_4d *prediction);
-
-/***************************************************************************//**
- * [private] Map a quaternion into system rotation matrix.
- *
- * @param[in] q
- *      Quaternioninc (4D) representation of a rotation.
- * @param[out] sr
- *      Corresponding system rotation matrix (3x3D)
- */
-PRIVATE void __quaternion_to_rotation(_const_fl_kalf_quaternion_ref q, fl_matrix_3d_ref sr);
-
-/***************************************************************************//**
- * [private] Update system rotation matrix (__se) given a measurement of the
- * device angular velocity.
- *
- * @param[in] t
- *      Internal time of the measurement
- * @param[in] wm
- *      3D vector of angular velocity [rad/s] in device coordinate frame
- * @param[in] s2wm
- *      Estimated variance of the measuremnt.
- */
-PRIVATE void __integrate_rw(fl_seconds t, const_fl_spin_vector_ref wm, fl_real s2wm);
-
-/***************************************************************************//**
- * [private] Extrapolate the system rotation (__sp) to the given time assuming
- * constant angular velocity.
- *
- * @param[in] t
- *      Internal time
- */
-PRIVATE void __predict_rw(fl_seconds t);
-
-/***************************************************************************//**
- * [private] Planar (2D) single-order Kalman blending (of either position,
- * velocity, or acceleration). Assumes the specific prediction is already
- * performed.
- *
- * @param[in] t
- *      Internal time
- * @param[in] s
- *      State component identifier to which @a xm and @a s2xm refer.
- * @param[in] xm
- *      Measured data
- * @param[in] s2xm
- *      Data variances
- */
-PRIVATE void __kalman_2d1o(fl_seconds t, _fl_kalf_state_component s, const_fl_vector_3d_ref xm, const_fl_vector_3d_ref s2xm);
-
-/***************************************************************************//**
- * [private] Planar (2D) Kalman blending of position.
- *
- * @param[in] t
- *      Internal time
- * @param[in] pm
- *      Measured position
- * @param[in] s2pm
- *      Position variances
- */
-PRIVATE void __kalman_p(fl_seconds t, const_fl_position_vector_ref pm, const_fl_vector_3d_ref s2pm);
-
-/***************************************************************************//**
- * [private] Spatial (3D) Kalman blending of position and velocity.
- *
- * @param[in] t
- *      Internal time
- * @param[in] pm
- *      Measured position
- * @param[in] vm
- *      Measured velocity
- * @param[in] s2pm
- *      Position variances
- * @param[in] s2vm
- *      Velocity variances
- */
-PRIVATE void __kalman_pv(
-	fl_seconds                   t,
-	const_fl_position_vector_ref pm,
-	const_fl_velocity_vector_ref vm,
-	const_fl_vector_3d_ref       s2pm,
-	const_fl_vector_3d_ref       s2vm);
-
-/***************************************************************************//**
- * [private] Axial (1D) Kalman blending of angular velocity along z-axis (turn).
- *
- * @param[in] t
- *      Internal time
- * @param[in] wzm
- *      Z-projection of the measured angular velocity
- * @param[in] s2wzm
- *      Corresponding variance
- */
-void __kalman_wz(fl_seconds t, fl_real wzm, fl_real s2wzm);
-
-/***************************************************************************//**
- * [private] Kalman blending two unit 3D vectors usign inverse covariances as
- * weights (to avoid singularities).
- *
- * @param[in] t
- *      Internal time
- * @param[in] u1
- *      3D vector
- * @param[in] u2
- *      3D vector
- * @param[in] w2u1
- *      Inverse variance of @a u1 (can be zero)
- * @param[in] w2u2
- *      Inverse variance of @a u2 (can be zero)
- * @param[out] v
- *      Output 3D vector
- * @param[out] w2v
- *      Output inverse variance of @a v
- */
-PRIVATE void __inv_kalman_u(
-	fl_seconds t,
-	const_fl_vector_3d_ref u1,  fl_real w2u1,
-	const_fl_vector_3d_ref u2,  fl_real w2u2,
-	fl_vector_3d_ref       v,   fl_real *w2v);
-
-/***************************************************************************//**
- * [private] Extrapolate position, velocity, and the covariance-matrices up to
- * the given time.
- *
- * @param[in] t
- *      Internal time
- */
-PRIVATE void __predict_pv(fl_seconds t);
-
-/***************************************************************************//**
- * [private] Check the notification conditions; if satisfied get the current
- * prediction in global coordinate frame and signal it to the user.
- *
- * @param[in] t
- *      Internal time
- */
-PRIVATE void __signal_updated_location(fl_seconds t);
-
-#if !defined(__FL_KALMAN_FILTER_C__)
-	#undef PRIVATE
-	#undef INLINE
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_KALMAN_FILTER_H__ */
diff --git a/lbs-server/src/fused/lp-3d-filter.c b/lbs-server/src/fused/lp-3d-filter.c
deleted file mode 100644
index e6bfd71..0000000
--- a/lbs-server/src/fused/lp-3d-filter.c
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
-#endif
-
-#define __FL_LP_3D_FILTER_C__
-
-#include <memory.h>
-#include "math-ext.h"
-#include "lp-3d-filter.h"
-#include "debug_util.h"
-
-#define LP3D_INPUT_FORMAT     "% 6.2f"
-
-void fl_lp3d_init(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0);
-	__lp3d_G  =  0;
-	__lp3d_B1 = -2;
-	__lp3d_B2 =  1;
-	fl_lp3d_reset();
-	LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("(): OK"), 0);
-}
-
-void fl_lp3d_exit(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-}
-
-void fl_lp3d_set_sampling_frequency(fl_hertz f)
-{
-	if (f > 0) {
-		double omegaC  = tan(M_PI * FL_LP3D_CUTOFF_FREQUENCY / f);
-		double omegaC2 = fl_square(omegaC);
-		double _B0;
-
-		_B0  = 1.0 / (1 + 2 * omegaC * cos(M_PI_4) + omegaC2);
-		__lp3d_B1 = _B0 * 2*(omegaC2 - 1);
-		__lp3d_B2 = _B0 * (1 - 2 * omegaC * cos(M_PI_4) + omegaC2);
-		__lp3d_G  = _B0 * omegaC2;
-
-		LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%g Hz): OK"), f);
-		return;
-	} else {
-		LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%g Hz): E_INVALID_ARGUMENT"), f);
-		return;
-	}
-}
-
-void fl_lp3d_reset(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	memset(__lp3d_u, 0, sizeof(__lp3d_u));
-	memset(__lp3d_v, 0, sizeof(__lp3d_v));
-}
-
-void fl_lp3d_reset_to(const_fl_acceleration_vector_ref stationary)
-{
-	unsigned i;
-
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT "})"),
-				  stationary[0],
-				  stationary[1],
-				  stationary[2]);
-
-	i = GEO_SPATIAL_DIMENSION;
-	do {
-		--i;
-		/* up-conversion */
-		__lp3d_u[TIME_SHIFT_1][i] = stationary[i];
-		__lp3d_u[TIME_SHIFT_2][i] = stationary[i];
-		__lp3d_v[TIME_SHIFT_1][i] = stationary[i];
-		__lp3d_v[TIME_SHIFT_2][i] = stationary[i];
-	} while (i);
-}
-
-const_fl_acceleration_vector_ref fl_lp3d_process(const_fl_acceleration_vector_ref u)
-{
-	unsigned i;
-
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT "})"),
-				  u[0],
-				  u[1],
-				  u[2]);
-
-	i = GEO_SPATIAL_DIMENSION;
-	do {
-		--i;
-
-		fl_acceleration u0 = u[i];
-		#define         u1 __lp3d_u[TIME_SHIFT_1][i]
-		#define         u2 __lp3d_u[TIME_SHIFT_2][i]
-		fl_real         v0;
-		#define         v1 __lp3d_v[TIME_SHIFT_1][i]
-		#define         v2 __lp3d_v[TIME_SHIFT_2][i]
-
-		v0 = __lp3d_G * (u0 + 2 * u1 + u2) - __lp3d_B1 * v1 - __lp3d_B2 * v2;
-		u2 = u1;
-		u1 = u0;
-		v2 = v1;
-		v1 = v0;
-
-		#undef u1
-		#undef u2
-		#undef v1
-		#undef v2
-	} while (i);
-
-	return __lp3d_v[TIME_SHIFT_1];
-}
diff --git a/lbs-server/src/fused/lp-3d-filter.h b/lbs-server/src/fused/lp-3d-filter.h
deleted file mode 100644
index 9277cc8..0000000
--- a/lbs-server/src/fused/lp-3d-filter.h
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    lp-3d-filter.h
- * @brief   Gravity direction estimator via low-pass 3D 2nd order Butterworth
- *          filter.
- */
-
-#pragma once
-#ifndef __FL_LP_3D_FILTER_H__
-#define __FL_LP_3D_FILTER_H__
-
-#include "types.h"
-#include "math-ext.h"
-#include "parameters.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/***************************************************************************//**
- * [public] Initializaton of the singleton unit (static constructor).
- */
-void fl_lp3d_init(void);
-
-/***************************************************************************//**
- * [public] Cleanup of the singleton unit (static destructor).
- */
-void fl_lp3d_exit(void);
-
-/***************************************************************************//**
- * [public] Adjust internal parameters to the given sampling frequency and reset
- * the the past state memory.
- *
- * @param[in] f
- *      The sampling frequency in [Hz] of the accelerometer (or processing
- *      callback if downsampled).
- */
-void fl_lp3d_set_sampling_frequency(fl_hertz f);
-
-/***************************************************************************//**
- * [public] Reset the past state memory.
- */
-void fl_lp3d_reset(void);
-
-/***************************************************************************//**
- * [public] Reset to the given stationary state (input = output).
- *
- *  @param[in] stationary
- *      3D vector of acceleration measured in tangent coordinate frame.
- */
-void fl_lp3d_reset_to(const_fl_acceleration_vector_ref stationary);
-
-/***************************************************************************//**
- * [public] Process single input sample through the 3D Butterworth filter.
- *
- * @param[in] u
- *      3D acceleration input vector.
- *
- * @return
- *      3D acceleration output vector.
- */
-const_fl_acceleration_vector_ref fl_lp3d_process(const_fl_acceleration_vector_ref u);
-
-#if defined(__FL_LP_3D_FILTER_C__)
-	#define INLINE \
-	inline
-#else
-	#define INLINE
-#endif
-#if defined(__FL_LP_3D_FILTER_C__)
-	#ifdef GTEST
-		#define PRIVATE
-	#else
-		#define PRIVATE \
-		static
-	#endif
-#else
-	#define PRIVATE \
-	extern
-#endif
-
-typedef enum {
-	TIME_SHIFT_1, /* t - 1 */
-	TIME_SHIFT_2, /* t - 2 */
-	/* always the last */
-	TIME_SHIFT_COUNT
-} _fl_lp3d_time_shift;
-
-/* filter coefficients */
-PRIVATE                fl_real __lp3d_G;  /* omega^2/B_0 */
-PRIVATE                fl_real __lp3d_B1; /*     B_1/B_0 */
-PRIVATE                fl_real __lp3d_B2; /*     B_2/B_0 */
-/* memory of past states */
-PRIVATE fl_acceleration_vector __lp3d_u[TIME_SHIFT_COUNT];
-PRIVATE fl_acceleration_vector __lp3d_v[TIME_SHIFT_COUNT];
-
-#if (LP3D_FEATURE_SIGMA2)
-PRIVATE                fl_real __lp3d_u2[TIME_SHIFT_COUNT];
-PRIVATE                fl_real __lp3d_v2[TIME_SHIFT_COUNT];
-#endif
-
-#if !defined(__FL_LP_3D_FILTER_C__)
-	#undef PRIVATE
-	#undef INLINE
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_LP_3D_FILTER_H__ */
diff --git a/lbs-server/src/fused/math-ext.c b/lbs-server/src/fused/math-ext.c
deleted file mode 100644
index 6ff3419..0000000
--- a/lbs-server/src/fused/math-ext.c
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    math-ext.c
- * @brief   Math extensions
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
-#endif
-
-#define __FL_MATH_EXT_C__
-
-#include <stdio.h>
-#include "math-ext.h"
-#include "debug_util.h"
-
-#define PRECISION   1e-5
-
-double fl_asin(double x)
-{
-	if (x >= 1) {
-		if (x > 1 + PRECISION) {
-			LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%g) OUT_OF_DOMAIN"), x);
-		}
-		return  M_PI_2;
-	} else if (x <= -1) {
-		if (x < -(1 + PRECISION)) {
-			LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%g) OUT_OF_DOMAIN"), x);
-		}
-		return -M_PI_2;
-	} else
-		return asin(x);
-}
-
diff --git a/lbs-server/src/fused/math-ext.h b/lbs-server/src/fused/math-ext.h
index 61e4eef..07e5e3e 100644
--- a/lbs-server/src/fused/math-ext.h
+++ b/lbs-server/src/fused/math-ext.h
@@ -20,8 +20,8 @@
  */
 
 #pragma once
-#ifndef __FL_MATH_EXT_H__
-#define __FL_MATH_EXT_H__
+#ifndef __MATH_EXT_H__
+#define __MATH_EXT_H__
 
 #define _USE_MATH_DEFINES
 #include <math.h>
@@ -31,14 +31,6 @@
 extern "C" {
 #endif
 
-#if defined(__FL_MATH_EXT_C__)
-	#define INLINE \
-	static inline
-#else
-	#define INLINE \
-	static inline
-#endif
-
 #if !defined(M_PI) /* C99 */
 	#define M_PI       3.14159265358979323846   /* pi         */
 	#define M_PI_2     1.57079632679489661923   /* pi/2       */
@@ -54,154 +46,34 @@ extern "C" {
 	#define M_1_SQRT3  0.577350269189625764509  /* 1/sqrt(3)  */
 	#define M_2PI      (2 * M_PI)               /* 2*pi       */
 
-/*******************************************************************//**
- * [public] Second power of a number. Use this macro for preprocessed
- * values and static variables.
- *
- * @param[in] x
- *      Arbitrary number
- *
- * @return
- *      Second power of @a x.
+/**
+ * @brief Second power of a number. Use this macro for preprocessed values and static variables.
+ * @param[in] x  Arbitrary number
+ * @return  Second power of @a x.
  */
 #define SQUARE(x)   ((x) * (x))
 
-/*******************************************************************//**
- * [public] Second power of a number. Use for dynamic variables.
- *
- * @param[in] x
- *      Arbitrary number
- *
- * @return
- *      Second power of @a x.
- */
-INLINE double fl_square(double x)
-{
-	return SQUARE(x);
-}
-
-/*******************************************************************//**
- * [public] Domain-safe arcus-sine. If the argument runs out of asin()
- * domain it is cast-back onto the nearest edge. If it departs beyond
- * numerical error margin, the error is reported, and breakpoint fired.
- * No exception is thrown in release mode.
- *
- * @param[in] x
- *      Number in the [-1, 1] range.
- *
- * @return
- *      - arcus sine of @x if x < 1 && x > -1,
- *      - -pi if x <= -1,
- *      -  pi if x >=  1.
- */
-double fl_asin(double x);
-
-/*******************************************************************//**
- * [public] Squared length of a 2D vector in Euclidean metric.
- *
- * @param[in] x
- *      2D vector
- *
- * @return
- *      Squared length of @a x, i.e. |x|^2 = x.x
- */
-INLINE double fl_vector_2d_length2(const double x[])
-{
-	return SQUARE(x[0]) + SQUARE(x[1]);
-}
-
-/*******************************************************************//**
- * [public] Squared length of a 3D vector in Euclidean metric.
- *
- * @param[in] x
- *      3D vector
- *
- * @return
- *      Squared length of @a x, i.e. |x|^2 = x.x
- */
-INLINE double fl_vector_3d_length2(const double x[])
-{
-	return SQUARE(x[0]) + SQUARE(x[1]) + SQUARE(x[2]);
-}
-
-/*******************************************************************//**
- * [public] Inlined left-action of a linear operator on a 3D
- * covector.
- *
- * @param[in] m
- *      3x3D matrix
- * @param[in] v
- *      3D vector
- * @param[out] out
- *      Left-action of the linear operator @a m on the vector @a v, i.e.
- *      out = m.v
- */
-#define MATRIX_DOT_VECTOR_3D(m, v, out)\
-	do {\
-		(out)[GEO_SPATIAL_X] = (m)[GEO_SPATIAL_X][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_X][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_X][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\
-		(out)[GEO_SPATIAL_Y] = (m)[GEO_SPATIAL_Y][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\
-		(out)[GEO_SPATIAL_Z] = (m)[GEO_SPATIAL_Z][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\
-	} while (0)
-
-/*******************************************************************//**
- * [public] Inlined right-action of a linear operator on a 3D
- * covector. In Euclidean metric this is the same as left action of the
- * transposed operator on the vector.
- *
- * @param[in] v
- *      3D vector
- * @param[in] m
- *      3x3D matrix
- * @param[out] out
- *      Right-action of the linear operator @a m on the covector @a v, i.e.
- *      out = v.m
- */
-#define VECTOR_DOT_MATRIX_3D(v, m, out)\
-	do {\
-		(out)[GEO_SPATIAL_X] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_X] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_X] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_X];\
-		(out)[GEO_SPATIAL_Y] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_Y] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Y];\
-		(out)[GEO_SPATIAL_Z] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_Z] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Z];\
-	} while (0)
-
-/*******************************************************************//**
- * [public] Inlined cross-product of two 3D vectors.
- *
- * @param[in] u
- *      3D vector
- * @param[in] v
- *      3D vector
- * @param[out] out
- *      Cross-product of @a u and @a v
+/**
+ * @brief Second power of a number. Use for dynamic variables.
+ * @param[in] x  Arbitrary number
+ * @return[double]  Second power of @a x.
  */
-#define VECTOR_CROSS_VECTOR_3D(u, v, out)\
-	do {\
-		(out)[GEO_SPATIAL_X] = (u)[GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Z] - (u)[GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Y];\
-		(out)[GEO_SPATIAL_Y] = (u)[GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_X] - (u)[GEO_SPATIAL_X] * (v)[GEO_SPATIAL_Z];\
-		(out)[GEO_SPATIAL_Z] = (u)[GEO_SPATIAL_X] * (v)[GEO_SPATIAL_Y] - (u)[GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_X];\
-	} while (0)
+#define fl_square(x) ({double __y__ = (x); __y__ * __y__;})
 
-/*******************************************************************//**
- * [public] Scalar Euclidean product of two 3D vectors.
- *
- * @param[in] x
- *      3D vector
- * @param[in] y
- *      3D vector
- *
- * @return
- *      Scalar product of @a x and @a y.
+/**
+ * @brief Mean Earth gravity value in [m/s^2].
+ * Notice that, strictly speaking, the mean Earth gravity acceleration at surface
+ * level is negative; the positive value we measure with an accelerometer is the
+ * ground reaction which counters the gravity and prevents us from falling into the interior.
+ * Also notice that given the inaccuracy (i.e. bias & miscalibration) of mobile device
+ * acceleromenters at ~1 [m/s^2] the number of significant digits in the reference value below is way too many.
  */
-INLINE double fl_vector_3d_dot_product(const double x[], const double y[])
-{
-	return x[0] * y[0] + x[1] * y[1] + x[2] * y[2];
-}
+#define EARTH_GRAVITY 9.80665
 
-#if !defined(__FL_MATH_EXT_C__)
-	#undef INLINE
-#endif
+#define EARTH_RADIUS  6371000.0
 
 #ifdef __cplusplus
 }
 #endif
 
-#endif /* __FL_MATH_EXT_H__ */
+#endif /* __MATH_EXT_H__ */
diff --git a/lbs-server/src/fused/motion-detector.c b/lbs-server/src/fused/motion-detector.c
deleted file mode 100644
index f220867..0000000
--- a/lbs-server/src/fused/motion-detector.c
+++ /dev/null
@@ -1,109 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
-	/* custom logging threshold - keep undefined on the repo */
-	/* #define LOG_THRESHOLD   LOG_LEVEL_TRACE */
-#endif
-
-#define __FL_MOTION_DETECTOR_C__
-
-#define _USE_MATH_DEFINES
-#include <math.h>
-#include "math-ext.h"
-#include "motion-detector.h"
-#include "debug_util.h"
-
-#if (FL_MOTION_DETECTOR)
-
-void fl_moti_init(motion_changed_cb motion_changed, gpointer handler)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(motion_cb=%p, handler=%p)"), motion_changed, handler);
-	fl_aema1d_init(&__moti_al2, 1 << FL_MOTI_AEMA_PLATEAU_BITS);
-	__moti_immobile_time           = 0;
-	__moti_state                   = MOTI_UNDECIDED;
-	__moti_last_notification       = MOTI_UNDECIDED;
-	__moti_motion_changed = motion_changed;
-	__moti_handler                 = handler;
-}
-
-void fl_moti_exit(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	__moti_handler                 = NULL;
-	__moti_motion_changed = NULL;
-	__moti_last_notification       = MOTI_UNDECIDED;
-	__moti_state                   = MOTI_UNDECIDED;
-	__moti_immobile_time           = 0;
-	fl_aema1d_exit(&__moti_al2);
-}
-
-void fl_moti_reset(void)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0);
-	__moti_immobile_time     = 0;
-	__moti_state             = MOTI_UNDECIDED;
-	__moti_last_notification = MOTI_UNDECIDED;
-	fl_aema1d_reset(&__moti_al2);
-}
-
-PRIVATE void __moti_set_state(fl_motion_state state)
-{
-	LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%u->%u)"), __moti_state, state);
-	__moti_state = state;
-	if (__moti_motion_changed && __moti_last_notification != state) {
-		__moti_motion_changed(state, __moti_handler);
-		__moti_last_notification = state;
-	}
-}
-
-void fl_moti_process(fl_seconds t, fl_acceleration_vector_ref al)
-{
-	fl_real al2;
-
-	al2 = fl_aema1d_process(&__moti_al2, fl_vector_3d_length2(al));
-	if (al2 > SQUARE(FL_MOTI_MOTION_LEVEL)) {
-		switch (__moti_state) {
-		case MOTI_UNDECIDED:
-		case MOTI_IMMOBILE:
-		case MOTI_SLEEP:
-			__moti_set_state(MOTI_MOVING);
-			break;
-
-		default: /* MOTI_MOVING */
-			break;
-		}
-	} else if (al2 < SQUARE(FL_MOTI_NOISE_LEVEL)) {
-		switch (__moti_state) {
-		case MOTI_UNDECIDED:
-		case MOTI_MOVING:
-			__moti_set_state(MOTI_IMMOBILE);
-			__moti_immobile_time = t;
-			break;
-
-		case MOTI_IMMOBILE:
-			if (t - __moti_immobile_time > FL_MOTI_IMMOBILITY_INTERVAL)
-				__moti_set_state(MOTI_SLEEP);
-			break;
-
-		default:
-		/* MOTI_SLEEP: */
-			break;
-		}
-	}
-}
-
-#endif
diff --git a/lbs-server/src/fused/motion-detector.h b/lbs-server/src/fused/motion-detector.h
deleted file mode 100644
index 8d31ee1..0000000
--- a/lbs-server/src/fused/motion-detector.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    motion-detector.h
- * @brief   Accelerometer-based detector of significant motion.
- */
-
-#pragma once
-#ifndef __FL_MOTION_DETECTOR_H__
-#define __FL_MOTION_DETECTOR_H__
-
-#include <glib.h>
-#include "types.h"
-#include "parameters.h"
-#include "aema-estimator.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-typedef enum {
-	MOTI_UNDECIDED, /* initial */
-	MOTI_MOVING,    /* acceleration above MOTION_LEVEL  */
-	MOTI_IMMOBILE,  /* acceleration below NOISE_LEVEL   */
-	MOTI_SLEEP,     /* immobile for IMMOBILITY_INTERVAL */
-} fl_motion_state;
-
-/***************************************************************************//**
- * [public] Callback invoked on every change in motion state.
- *
- * @param[in] state
- *      New motion state.
- * @param[in] handler
- *      Arbitrary pointer to user data specified during the init() call.
- */
-typedef void (*motion_changed_cb)(fl_motion_state state, gpointer handler);
-
-#if (FL_MOTION_DETECTOR)
-
-/***************************************************************************//**
- * [public] Initializaton of the singleton unit (static constructor).
- *
- * @param[in] motion_changed
- *      Callback to be invoked when the detected state of motion changes.
- * @param[in] handler
- *      Arbitrary user parameter passed to the callback.
- */
-void fl_moti_init(motion_changed_cb motion_changed, gpointer handler);
-
-/***************************************************************************//**
- * [public] Cleanup of the singleton unit (static destructor).
- */
-void fl_moti_exit(void);
-
-/***************************************************************************//**
- * [public] Reset of the internal state back to initial one. This function is
- * used for repetitive testing and module soft restarts.
- */
-void fl_moti_reset(void);
-
-/***************************************************************************//**
- * [public] Processing of a single sample from the accelerometer. This function
- * implments a hysteresis in the 2D space of linear acceleration modulus |al|^2
- * vs motion state:
- *  - Crossing the FL_MOTI_MOTION_LEVEL from below changes state to MOVING;
- *  - Crossing the FL_MOTI_NOISE_LEVEL from above changes state to IMMOBILE;
- *  - Staying IMMOBILE for longer than FL_MOTI_IMMOBILITY_INTERVAL changes
- *    state to SLEEP.
- *
- * @param[in] t
- *      Time of the event in seconds.
- * @param[in] al
- *      3D vector of linear acceleration.
- */
-void fl_moti_process(fl_seconds t, fl_acceleration_vector_ref al);
-
-#if defined(__FL_MOTION_DETECTOR_C__)
-	#ifdef GTEST
-		#define PRIVATE
-	#else
-		#define PRIVATE \
-		static
-	#endif
-#else
-	#define PRIVATE \
-	extern
-#endif
-
-PRIVATE fl_aema1d_estimator __moti_al2;                     /**< Linear acceleration low-pass filter */
-PRIVATE fl_seconds __moti_immobile_time;           /**< Start time of immobility */
-PRIVATE fl_motion_state __moti_state;                   /**< Curent state */
-PRIVATE fl_motion_state __moti_last_notification;       /**< Last notified state */
-PRIVATE motion_changed_cb __moti_motion_changed; /**< User callback */
-PRIVATE gpointer __moti_handler;                 /**< User data */
-
-/***************************************************************************//**
- * [private] Change the state indicator and notify the user.
- *
- * @param[in] state
- *      New state value (does not have to be different than present one).
- */
-PRIVATE void __moti_set_state(fl_motion_state state);
-
-#if !defined(__FL_MOTION_DETECTOR_C__)
-	#undef PRIVATE
-#endif
-
-#else
-	#define fl_moti_init(motion_changed, handler)
-	#define fl_moti_exit()
-	#define fl_moti_reset()
-	#define fl_moti_process(t, al)
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_MOTION_DETECTOR_H__ */
diff --git a/lbs-server/src/fused/parameters.h b/lbs-server/src/fused/parameters.h
index 2f2c053..4513493 100644
--- a/lbs-server/src/fused/parameters.h
+++ b/lbs-server/src/fused/parameters.h
@@ -33,15 +33,21 @@
 #define FL_KALMAN_ANGULAR_VELOCITY     1
 /** 0 = off, 1 = Filtration of output location notifications */
 #define FL_MIN_NOTIFICATION_FILTRATION 1
-/** Minimal interval in miliseconds between consecutive output location notifications */
-#define FL_MIN_NOTIFICATION_INTERVAL   1000
+/** Minimal interval in seconds between consecutive output location notifications */
+#define FL_MIN_NOTIFICATION_INTERVAL   1.0
+/** Maximum time in seconds from last input location for sending output position to clients */
+#define FL_MAX_TIME_FROM_LAST_POS      300.0
+/** Max time in seconds from last gps when pedometer skip may occur (filtration pedometer false positive in driving case) */
+#define FL_MAX_PEDOMETER_SKIP_TIME     60.0
+/** Min speed in km/h from last gps when pedometer skip may occur (filtration pedometer false positive in driving case) */
+#define FL_MIN_PEDOMETER_SKIP_SPEED    10.0
 /** 0 = off, 1 = fused devel logs on */
 #define FL_ENABLE_DEVEL_LOG            0
 
 /* Free parameters */
 
 /** [ms] Sensor sampling interval */
-#define FL_SENSOR_SAMPLING_INTERVAL    100
+#define FL_SENSOR_SAMPLING_INTERVAL    10
 /** [m] Default standard deviation of measured position */
 #define FL_DEFAULT_POSITION_SIGMA      8.0
 /** [m/s] Default standard deviation of measured linear velocity */
@@ -49,14 +55,14 @@
 /** [rad/s] Default standard deviation of measured angular velocity */
 #define FL_DEFAULT_SPIN_SIGMA          0.2
 /** [m/s^2] Accelerometer noise level */
-#define FL_ACCEL_NOISE_LEVEL           ((fl_acceleration)0.25)
+#define FL_ACCEL_NOISE_LEVEL           0.25
 /** [Hz] Low-pass acceleration frequency threshold for estimation of gravity direction */
-#define FL_LP3D_CUTOFF_FREQUENCY       ((fl_hertz)1.0)
+#define FL_LP3D_CUTOFF_FREQUENCY       10
 /** [m/s^2] Upper level of IMMOBILE state */
 #define FL_MOTI_NOISE_LEVEL            FL_ACCEL_NOISE_LEVEL
 /** [m/s^2] Lower level of MOVING state */
 #define FL_MOTI_MOTION_LEVEL           (3*FL_ACCEL_NOISE_LEVEL)
-/** [s] Continuous immobility interval before transitioning to SLEEP state */
+/** [s] Continuous immobility interval before transition to SLEEP state */
 #define FL_MOTI_IMMOBILITY_INTERVAL    90
 /** Log-2 of the AEMA averaging constant used for motion detection. */
 #define FL_MOTI_AEMA_PLATEAU_BITS      4
diff --git a/lbs-server/src/fused/types.h b/lbs-server/src/fused/types.h
deleted file mode 100644
index 8c8f9e9..0000000
--- a/lbs-server/src/fused/types.h
+++ /dev/null
@@ -1,210 +0,0 @@
-/*
- *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-/**
- * @file    types.h
- * @brief   Core types definitions
- */
-
-#pragma once
-#ifndef __FL_TYPES_H__
-#define __FL_TYPES_H__
-
-#include "location-types.h"
-#include "location-position.h"
-#include "location-velocity.h"
-#include "location-accuracy.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-typedef       double     fl_real;         /**< Default precision for the floating-point data in fused location */
-typedef        float sensor_real;         /**< Default precision for the floating-point data in sensors */
-typedef      fl_real     fl_seconds;      /**< Time variables in SI units */
-typedef unsigned int     fl_timestamp;    /**< Time-stamps in [s] */
-typedef      fl_real     fl_hertz;        /**< Frequency variables in SI units */
-typedef      fl_real     fl_meters;       /**< Length variables in SI units */
-typedef      fl_real     fl_velocity;     /**< Linear velocity in SI units [m/s] */
-typedef      fl_real     fl_km_h;         /**< Linear velocity in SI-derived units [km/h] */
-typedef      fl_real     fl_acceleration; /**< Linear acceleration in SI units [m/s^2] */
-typedef  sensor_real sensor_acceleration; /**< Linear acceleration in SI units [m/s^2] supplied by <sensor.h> */
-typedef      fl_real     fl_radians;      /**< Angular position in SI units [rad] modulo 3.14159 */
-typedef  sensor_real sensor_radians;      /**< Angular position in SI units [rad] modulo 3.14159 supplied by <sensor.h> */
-typedef      fl_real     fl_degrees;      /**< Angular position modulo 360 */
-typedef      fl_real     fl_spin;         /**< Angular velocity in SI units [rad/s]  modulo 3.14159 */
-typedef  sensor_real sensor_spin;         /**< Angular velocity in SI units [rad/s]  modulo 3.14159 supplied by <sensor.h> */
-typedef      fl_real     fl_utesla;       /**< Magnetic field strength (B) in [uT] */
-typedef  sensor_real sensor_utesla;       /**< Magnetic field strength (B) in [uT] supplied by <sensor.h> */
-
-#define FL_UNDEFINED_TIME   ((fl_seconds)-1e+48)
-
-/** Labels for a planar 2D space*/
-typedef enum {
-	PLANAR_X,
-	PLANAR_Y,
-	/* always the last */
-	PLANAR_DIMENSION
-} fl_planar;
-
-/** Labels for an axial (or cylyndrical without phase) 2D space */
-typedef enum {
-	AXIAL_H,
-	AXIAL_V,
-	/* always the last */
-	AXIAL_DIMENSION
-} fl_axial;
-
-/** Labels for a geographic coordinate system (left-handed) */
-typedef enum {
-	GEO_SPATIAL_X,
-	GEO_SPATIAL_Y,
-	GEO_SPATIAL_Z,
-	/* always the last */
-	GEO_SPATIAL_DIMENSION
-} fl_geo_spatial;
-
-/** Labels for a device coordinate system (right-handed) */
-typedef enum {
-	DEV_SPATIAL_X			= GEO_SPATIAL_Y,
-	DEV_SPATIAL_Y			= GEO_SPATIAL_X,
-	DEV_SPATIAL_Z			= GEO_SPATIAL_Z,
-	DEV_SPATIAL_DIMENSION	= GEO_SPATIAL_DIMENSION
-} fl_dev_spatial;
-
-/** Labels for sensors */
-typedef enum {
-	ACCELEROMETER,
-	GYROSCOPE,
-	SENSOR_SOURCE_NUM
-} fl_sensory_source;
-
-/** Flag indicators for sensors' combination */
-typedef enum {
-	NO_SENSOR_FLAG		= 0,
-	ACCELEROMETER_FLAG	= (1 << ACCELEROMETER),
-	GYROSCOPE_FLAG		= (1 << GYROSCOPE),
-	/* always the last */
-	SUPPORTED_SENSORS_MASK	= (1 << SENSOR_SOURCE_NUM) - 1
-} fl_sensory_flags;
-
-/** Three Cartesian coordinates of a point on the Earth surface */
-typedef       fl_meters        fl_position_vector[GEO_SPATIAL_DIMENSION];
-typedef       fl_meters       *fl_position_vector_ref;
-typedef const fl_meters *const_fl_position_vector_ref;
-
-typedef       fl_velocity        fl_velocity_vector[GEO_SPATIAL_DIMENSION];
-typedef       fl_velocity       *fl_velocity_vector_ref;
-typedef const fl_velocity *const_fl_velocity_vector_ref;
-
-/** Three device-oriented axes of the acceleration */
-typedef       sensor_acceleration        sensor_acceleration_vector[GEO_SPATIAL_DIMENSION];
-typedef       sensor_acceleration       *sensor_acceleration_vector_ref;
-typedef const sensor_acceleration *const_sensor_acceleration_vector_ref;
-typedef           fl_acceleration            fl_acceleration_vector[GEO_SPATIAL_DIMENSION];
-typedef           fl_acceleration           *fl_acceleration_vector_ref;
-typedef const     fl_acceleration     *const_fl_acceleration_vector_ref;
-
-/** Three imaginary axes of the rotation quaternion */
-typedef       sensor_spin        sensor_spin_vector[GEO_SPATIAL_DIMENSION];
-typedef       sensor_spin       *sensor_spin_vector_ref;
-typedef const sensor_spin *const_sensor_spin_vector_ref;
-typedef           fl_spin            fl_spin_vector[GEO_SPATIAL_DIMENSION];
-typedef           fl_spin           *fl_spin_vector_ref;
-typedef     const fl_spin     *const_fl_spin_vector_ref;
-
-typedef       sensor_utesla        sensor_magnetic_vector[GEO_SPATIAL_DIMENSION];
-typedef       sensor_utesla       *sensor_magnetic_vector_ref;
-typedef const sensor_utesla *const_sensor_magnetic_vector_ref;
-typedef           fl_utesla            fl_magnetic_vector[GEO_SPATIAL_DIMENSION];
-typedef           fl_utesla           *fl_magnetic_vector_ref;
-typedef const     fl_utesla     *const_fl_magnetic_vector_ref;
-
-typedef const sensor_real *const_sensor_vector_3d_ref;
-
-typedef           fl_real        fl_vector_2d[AXIAL_DIMENSION];
-typedef           fl_real       *fl_vector_2d_ref;
-typedef const     fl_real *const_fl_vector_2d_ref;
-
-typedef           fl_real        fl_vector_3d[GEO_SPATIAL_DIMENSION];
-typedef           fl_real       *fl_vector_3d_ref;
-typedef const     fl_real *const_fl_vector_3d_ref;
-
-typedef             fl_real  fl_matrix_3d[GEO_SPATIAL_DIMENSION][GEO_SPATIAL_DIMENSION];
-typedef             fl_real(*fl_matrix_3d_ref)[GEO_SPATIAL_DIMENSION];
-typedef fl_real(*const const_fl_matrix_3d_ref)[GEO_SPATIAL_DIMENSION];
-
-/* Spatio-temporal position in Cartesian coordinates */
-typedef struct {
-	fl_position_vector  spatial;
-	fl_seconds          time;
-} fl_position_4d;
-
-/*! Position in spherical coordintes */
-typedef LocationPosition fl_spherical_position;
-/*! Velocity in coordinates of tangent space (tangent plane x ray coordinate) */
-typedef LocationVelocity fl_tangent_velocity;
-/*!  Standard deviations (1-sigma, or CEP 67%) */
-typedef struct {
-	LocationAccuracyLevel level;             /**< ovelapps with 'accuracy.level' */
-	fl_meters             of_horizontal_pos; /**< std. dev. of horizontal position in [m] */
-	fl_meters             of_altitude;       /**< std. dev. of vertical   position in [m] */
-	fl_velocity           of_speed;          /**< std. dev. of horizontal velocity in [m/s] */
-	fl_velocity           of_climb;          /**< std. dev. of vertical   velocity in [m/s] */
-} fl_sigma;
-
-/** Huge sigma value representing "almost flat" distribution. Unfortunately,
- *  we're forced by the API to use uncertainties (standard deviations, CEPs,
- *  or variances if squared) instead of the certainties (weights, or accuracies)
- *  which are their inverses and do not suffer this initialization problem.
- *  Even more peculiar is the custom of twisting the brain and calling
- *  uncertainty 'accuracy'. By this token zero represents the maximum, while
- *  infinity - the minimum. Beware.
- */
-#define FLAT_SIGMA      1e24
-#define FLAT_SIGMA2     (FLAT_SIGMA * FLAT_SIGMA)
-
-typedef union {
-	LocationAccuracy accuracy;
-	fl_sigma         sigma;
-} fl_uncertainty_union;
-
-/*! Complete location data */
-typedef struct {
-	fl_spherical_position pos;
-	fl_tangent_velocity   vel;
-	union {
-		LocationAccuracy  accuracy;
-		fl_sigma          sigma;
-	};
-} fl_location;
-
-/***************************************************************************//**
- * Callback invoked on every new predicted position.
- *
- * @param[in] position
- *      The structure holds current filter output: spatio-temporal vector of
- *      dimension (3+1) in Cartesian coordinate system.
- * @param[in] handler
- *      Arbitrary pointer to user data specified during the init() call.
- */
-typedef void (*predicted_updated_cb)(const fl_position_4d* position, gpointer handler);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FL_TYPES_H__ */
diff --git a/lbs-server/src/lbs_server.c b/lbs-server/src/lbs_server.c
index a8eb904..59c31b7 100644
--- a/lbs-server/src/lbs_server.c
+++ b/lbs-server/src/lbs_server.c
@@ -26,6 +26,7 @@
 #include <vconf.h>
 #include "setting.h"
 #include <lbs_dbus_server.h>
+#include <system_info.h>
 
 #include "gps_plugin_data_types.h"
 #include "lbs_server.h"
@@ -47,6 +48,9 @@
 #define MAX_BATCH_INTERVAL	255
 #define MAX_BATCH_PERIOD	60000
 
+// TODO blocking area test
+#define VIRTUAL_BLOCK_AREA	1
+
 typedef struct {
 	/* gps variables */
 	pos_data_t position;
@@ -79,10 +83,10 @@ typedef struct {
 
 	/* dynamic interval update */
 	GHashTable *gps_interval_table;
-	GHashTable *fused_interval_table;
 	guint *optimized_interval;
 	guint temp_minimum_interval;
 	gboolean is_changing_gps_interval;
+	gint current_location_source;
 #ifndef TIZEN_DEVICE
 	GHashTable *batch_interval_table;
 	guint *optimized_batch_array;
@@ -97,10 +101,12 @@ typedef struct {
 	GVariant *mock_accuracy;
 
 	/* fused variables */
+	GHashTable *fused_interval_table;
 	gint fused_high_count;
 	gint fused_balance_count;
 	gboolean is_fused_running;
-	gint current_location_source;
+	gboolean fused_feature;
+	gint fused_test_block;
 } lbs_server_s;
 
 #ifndef TIZEN_DEVICE
@@ -115,7 +121,7 @@ typedef enum {
 	LOCATION_SOURCE_NONE = 0,
 	LOCATION_SOURCE_GPS,
 	LOCATION_SOURCE_WPS,
-	LOCATION_SOURCE_BOTH,
+	LOCATION_SOURCE_ALL,
 } lbs_current_location_source;
 
 typedef enum {
@@ -233,6 +239,7 @@ static void nps_set_status(lbs_server_s *lbs_server, LbsStatus status)
 
 static void nps_update_position(lbs_server_s *lbs_server, NpsManagerPositionExt wps)
 {
+	LOG_NPS(DBG_LOW, "ENTER >>>");
 	if (!lbs_server) {
 		LOG_NPS(DBG_ERR, "lbs-server is NULL!!");
 		return;
@@ -256,8 +263,17 @@ static void nps_update_position(lbs_server_s *lbs_server, NpsManagerPositionExt
 	LOG_NPS(DBG_LOW, "[WPS] time: %d, latitude : %.6lf, longitude : %.6lf", lbs_server->wps.timestamp, lbs_server->wps.latitude, lbs_server->wps.longitude);
 
 #ifdef TIZEN_DEVICE
-	if (lbs_server->is_fused_running)
-		send_wps_position_to_fused_engine(wps.timestamp, wps.latitude, wps.longitude, wps.hor_accuracy, wps.ver_accuracy);
+	/*
+	 * For fused location
+	 */
+	if (lbs_server->fused_feature) {
+		if (lbs_server->is_fused_running) {
+			if (lbs_server->fused_test_block == 0)
+			send_wps_position_to_fused_engine(wps.timestamp, wps.latitude, wps.longitude, wps.hor_accuracy);
+			else
+				LOG_NPS(DBG_LOW, "[VIRTUAL] WPS disabled temporary");
+		}
+	}
 #endif
 
 	lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_NPS,
@@ -551,7 +567,7 @@ static void stop_tracking(lbs_server_s *lbs_server, lbs_server_method_e method,
 
 		break;
 	case LBS_SERVER_METHOD_NPS:
-		LOG_NPS(DBG_LOW, "_stop_tracking NPS");
+		LOG_NPS(DBG_LOW, "stop tracking NPS [client: %d]", lbs_server->nps_client_count);
 
 		client_count_updater(lbs_server, LBS_SERVER_METHOD_NPS, _LBS_CLIENT_REMOVE, fused_mode);
 
@@ -574,8 +590,9 @@ static void stop_tracking(lbs_server_s *lbs_server, lbs_server_method_e method,
 
 static gboolean _set_current_location_source(lbs_server_s *lbs_server, gint mode)
 {
+	LOG_GPS(DBG_LOW, "set current location source [%d]", mode);
 	if (lbs_server->current_location_source != mode) {
-		LOG_GPS(DBG_LOW, "location source changed[%d]", mode);
+		LOG_GPS(DBG_LOW, "location source changed[%d -> %d]", lbs_server->current_location_source, mode);
 		lbs_server->current_location_source = mode;
 		return TRUE;
 	}
@@ -584,7 +601,9 @@ static gboolean _set_current_location_source(lbs_server_s *lbs_server, gint mode
 
 static gboolean __get_fused_interval_control(lbs_server_s *lbs_server)
 {
-	/* Activate fused combine interval when all clients are fused */
+	/*
+	 * Activate fused combine interval when all clients are fused
+	 */
 	gint pure_gps_count = lbs_server->gps_client_count - lbs_server->fused_high_count;
 	if (pure_gps_count == 0 && lbs_server->fused_balance_count > 0)
 		return TRUE;
@@ -594,41 +613,86 @@ static gboolean __get_fused_interval_control(lbs_server_s *lbs_server)
 
 static gboolean select_location_source(lbs_server_s *lbs_server)
 {
-	/*********************************************************************************
-		 Define all condition to select optimized location source
-		gps, high ==>     1(gps0, high0) 2(gps3, high0) 3(gps3, high1) 4(gps3, high3)
-		wps, balance ==>  A(wps0, bal0)  B(wps3, bal0)  C(wps3, bal1)  D(wps3, bal3)
+	LOG_FUSED_FUNC;
 
-		LOCATION_SOURCE_GPS  : 2A, 2D, 3A, 3D, 4A, 4D
-		LOCATION_SOURCE_WPS  : 1B, 1C, 1D
-		LOCATION_SOURCE_BOTH : 2B, 2C, 3B, 2C, 4B, 4C
-		LOCATION_SOURCE_NONE : 1A
-	*********************************************************************************/
+	/*
+	 * For fused location
+	 */
+	if (lbs_server->fused_feature) {
+		/**
+		 * Define all condition to select optimized location source
+		 * gps, high ==>     1(gps0, high0) 2(gps3, high0) 3(gps3, high1) 4(gps3, high3)
+		 * wps, balance ==>  A(wps0, bal0)  B(wps3, bal0)  C(wps3, bal1)  D(wps3, bal3)
+
+		 * LOCATION_SOURCE_GPS  : 2A, 2D, 3A, 3D, 4A, 4D
+		 * LOCATION_SOURCE_WPS  : 1B, 1C, 1D
+		 * LOCATION_SOURCE_ALL  : 2B, 2C, 3B, 2C, 4B, 4C
+		 * LOCATION_SOURCE_NONE	: 1A
+	     **/
+
+		gint pure_wps_count = lbs_server->nps_client_count - lbs_server->fused_balance_count;
+
+		if (lbs_server->gps_client_count > 0) {		/* case 2, 3, 4 */
+
+			if (lbs_server->nps_client_count > 0) {
+				if (pure_wps_count > 0) {			/* 2B, 2C, 3B, 3C, 4B, 4C */
+					if (_set_current_location_source(lbs_server, LOCATION_SOURCE_ALL)) return TRUE;
+				} else {							/* 2D, 3D, 4D */
+					if (_set_current_location_source(lbs_server, LOCATION_SOURCE_GPS)) return TRUE;
+				}
+			} else {								/* 2A, 3A, 4A */
+				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_GPS)) return TRUE;
+			}
 
-	gint pure_wps_count = lbs_server->nps_client_count - lbs_server->fused_balance_count;
+		} else {									/* case 1*/
+			if (lbs_server->nps_client_count > 0) {	/* 1B, 1C, 1D */
+				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_WPS)) return TRUE;
+			} else {								/* 1A */
+				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_NONE)) return TRUE;
+			}
+		}
 
-	if (lbs_server->gps_client_count > 0) {		/* case 2, 3, 4 */
+	/*
+	 * Fused disabled
+	 */
+	} else {
 
-		if (lbs_server->nps_client_count > 0) {
-			if (pure_wps_count > 0) {			/* 2B, 2C, 3B, 3C, 4B, 4C */
-				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_BOTH)) return TRUE;
-			} else {							/* 2D, 3D, 4D */
+		/* [TRUE] Change location source */
+		if (lbs_server->gps_client_count > 0) {
+			if (lbs_server->nps_client_count > 0) {
+				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_ALL)) return TRUE;
+			} else {
 				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_GPS)) return TRUE;
 			}
-		} else {								/* 2A, 3A, 4A */
-			if (_set_current_location_source(lbs_server, LOCATION_SOURCE_GPS)) return TRUE;
-		}
-
-	} else {									/* case 1*/
-		if (lbs_server->nps_client_count > 0) {	/* 1B, 1C, 1D */
-			if (_set_current_location_source(lbs_server, LOCATION_SOURCE_WPS)) return TRUE;
-		} else {								/* 1A */
-			if (_set_current_location_source(lbs_server, LOCATION_SOURCE_NONE)) return TRUE;
+		} else {
+			if (lbs_server->nps_client_count > 0) {
+				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_WPS)) return TRUE;
+			} else {
+				if (_set_current_location_source(lbs_server, LOCATION_SOURCE_NONE)) return TRUE;
+			}
 		}
 	}
+
+	LOG_GPS(DBG_LOW, "update location source [FALSE], keep current [gps: %d, wps: %d]",
+					lbs_server->gps_client_count, lbs_server->nps_client_count);
+
+	/* [FALSE] Keep current location source */
 	return FALSE;
 }
 
+#if (VIRTUAL_BLOCK_AREA)
+static void _block_cb(keynode_t *key, void *user_data)
+{
+	LOG_GPS(DBG_LOW, "[VIRTUAL] VCONFKEY_LOCATION_SUPL_VERSION changed");
+	int value = 0;
+
+	setting_get_int(VCONFKEY_LOCATION_SUPL_VERSION, &value);
+
+	lbs_server_s *lbs_server = (lbs_server_s *)user_data;
+	lbs_server->fused_test_block = value;
+}
+#endif
+
 static gboolean location_source_selector(lbs_server_s *lbs_server, gint fused_mode)
 {
 	LOG_FUSED_FUNC;
@@ -637,31 +701,41 @@ static gboolean location_source_selector(lbs_server_s *lbs_server, gint fused_mo
 		return FALSE;
 	}
 
-	/* set fused running status */
-	if (fused_mode == LOCATION_FUSED_HIGH || fused_mode == LOCATION_FUSED_BALANCED) {
-		if (lbs_server->fused_high_count == 0 && lbs_server->fused_balance_count == 0) {
-			LOG_GPS(DBG_LOW, "[FUSED] Need to stop fused mode");
-			if (lbs_server->is_fused_running) {
-				g_mutex_lock(&lbs_server->mutex);
-				lbs_server->is_fused_running = FALSE;
-				g_mutex_unlock(&lbs_server->mutex);
-
-				location_fused_stop();
-			}
-		} else {
-			LOG_GPS(DBG_LOW, "[FUSED] Need to start fused mode");
-			if (lbs_server->is_fused_running == FALSE) {
-				g_mutex_lock(&lbs_server->mutex);
-				lbs_server->is_fused_running = TRUE;
-				g_mutex_unlock(&lbs_server->mutex);
+	/*
+	 * For fused location
+	 */
+	if (lbs_server->fused_feature) {
+		/* set fused running status */
+		if (fused_mode == LOCATION_FUSED_HIGH || fused_mode == LOCATION_FUSED_BALANCED) {
+			if (lbs_server->fused_high_count == 0 && lbs_server->fused_balance_count == 0) {
+				LOG_GPS(DBG_LOW, "[FUSED] Need to stop fused mode");
+				if (lbs_server->is_fused_running) {
+					g_mutex_lock(&lbs_server->mutex);
+					lbs_server->is_fused_running = FALSE;
+					g_mutex_unlock(&lbs_server->mutex);
+
+					location_fused_stop();
+				}
+			} else {
+				LOG_GPS(DBG_LOW, "[FUSED] Need to start fused mode");
+				if (lbs_server->is_fused_running == FALSE) {
+					g_mutex_lock(&lbs_server->mutex);
+					lbs_server->is_fused_running = TRUE;
+					g_mutex_unlock(&lbs_server->mutex);
 
-				location_fused_start();
+					location_fused_start();
+				}
 			}
 		}
 	}
 
-	if (select_location_source(lbs_server))
+	/*
+	 * common
+	 */
+	if (select_location_source(lbs_server)) {
+		LOG_GPS(DBG_LOW, "select location source [TRUE] --> source changed");
 		return TRUE;
+	}
 
 	return FALSE;
 }
@@ -757,8 +831,8 @@ static gboolean plugin_status_controller(lbs_server_s *lbs_server)
 		if (lbs_server->is_nps_running == FALSE)
 			ret = nps_plugin_start(lbs_server);
 
-	} else if (lbs_server->current_location_source == LOCATION_SOURCE_BOTH) {
-		LOG_FUSED(DBG_LOW, "-->> LOCATION_SOURCE_BOTH");
+	} else if (lbs_server->current_location_source == LOCATION_SOURCE_ALL) {
+		LOG_FUSED(DBG_LOW, "-->> LOCATION_SOURCE_ALL");
 		if (lbs_server->is_gps_running == FALSE)
 			ret = gps_plugin_start(lbs_server);
 
@@ -948,6 +1022,9 @@ static gboolean update_gps_tracking_interval(lbs_server_interval_manipulation_ty
 	return ret_val;
 }
 
+/*
+ * For fused location
+ */
 static gboolean update_fused_tracking_interval(lbs_server_interval_manipulation_type type, const gchar *client, int method, guint interval, guint prev_interval, gpointer userdata)
 {
 	LOG_FUSED(DBG_LOW, "[FUSED] >>> update fused_tracking_interval");
@@ -1078,8 +1155,9 @@ static void request_plugin_interval(lbs_server_s *lbs_server)
 				request_change_pos_update_interval_standalone_gps(lbs_server->now_gps_interval);
 			}
 		}
+
 	} else {
-		LOG_GPS(DBG_LOW, "request plugin[stop] --> [0]");
+		LOG_GPS(DBG_LOW, "request plugin[stop] --> [interval: 0]");
 		lbs_server->now_gps_interval = 0;
 	}
 }
@@ -1149,8 +1227,13 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat
 				LOG_GPS(DBG_LOW, "update gps_tracking_interval -> START");
 				update_gps_tracking_interval(LBS_SERVER_INTERVAL_ADD, client, method, interval, 0, lbs_server);
 
-				if (fused_mode != LOCATION_FUSED_NONE)
-					update_fused_tracking_interval(LBS_SERVER_INTERVAL_ADD, client, method, interval, 0, lbs_server);
+				/*
+				 * For fused location
+				 */
+				if (lbs_server->fused_feature) {
+					if (fused_mode != LOCATION_FUSED_NONE)
+						update_fused_tracking_interval(LBS_SERVER_INTERVAL_ADD, client, method, interval, 0, lbs_server);
+				}
 			}
 
 			if (app_id) {
@@ -1160,8 +1243,7 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat
 
 			start_tracking(lbs_server, method, fused_mode);
 
-		}
-		else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP")) {
+		} else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP")) {
 			while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) {
 				if (!g_strcmp0(key, "METHOD"))
 					method = g_variant_get_int32(value);
@@ -1178,8 +1260,13 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat
 				LOG_GPS(DBG_LOW, "update gps_tracking_interval -> STOP");
 				update_gps_tracking_interval(LBS_SERVER_INTERVAL_REMOVE, client, method, interval, 0, lbs_server);
 
-				if (fused_mode != LOCATION_FUSED_NONE)
-					update_fused_tracking_interval(LBS_SERVER_INTERVAL_REMOVE, client, method, interval, 0, lbs_server);
+				/*
+				 * For fused location
+				 */
+				if (lbs_server->fused_feature) {
+					if (fused_mode != LOCATION_FUSED_NONE)
+						update_fused_tracking_interval(LBS_SERVER_INTERVAL_REMOVE, client, method, interval, 0, lbs_server);
+				}
 			}
 
 			if (app_id) {
@@ -1187,7 +1274,17 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat
 				free(app_id);
 			}
 
-			stop_tracking(lbs_server, method, fused_mode);
+			if (LBS_SERVER_METHOD_GPS == method) {
+				if (lbs_server->gps_client_count > 0)
+					stop_tracking(lbs_server, method, fused_mode);
+				else
+					LOG_GPS(DBG_LOW, "[STOP] stop tracking is not called because [gps: 0]");
+			} else if (LBS_SERVER_METHOD_NPS == method) {
+				if (lbs_server->nps_client_count > 0)
+					stop_tracking(lbs_server, method, fused_mode);
+				else
+					LOG_GPS(DBG_LOW, "[STOP] stop tracking is not called because [wps: 0]");
+			}
 
 		}
 #ifndef TIZEN_DEVICE
@@ -1284,8 +1381,13 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat
 				LOG_GPS(DBG_LOW, "update gps_tracking_interval -> SET:OPT");
 				update_gps_tracking_interval(LBS_SERVER_INTERVAL_UPDATE, client, method, interval, prev_interval, lbs_server);
 
-				if (fused_mode != LOCATION_FUSED_NONE)
-					update_fused_tracking_interval(LBS_SERVER_INTERVAL_UPDATE, client, method, interval, prev_interval, lbs_server);
+				/*
+				 * For fused location
+				 */
+				if (lbs_server->fused_feature) {
+					if (fused_mode != LOCATION_FUSED_NONE)
+						update_fused_tracking_interval(LBS_SERVER_INTERVAL_UPDATE, client, method, interval, prev_interval, lbs_server);
+				}
 
 				if (lbs_server->is_changing_gps_interval) {
 					lbs_server->is_changing_gps_interval = FALSE;
@@ -1298,7 +1400,7 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat
 
 static gboolean gps_remove_all_clients(lbs_server_s *lbs_server)
 {
-	LOG_GPS(DBG_LOW, "remove_all_clients[%d] GPS", lbs_server->gps_client_count);
+	LOG_GPS(DBG_LOW, "remove_all_clients[%d] GPS, [gps client --> 0]", lbs_server->gps_client_count);
 	if (lbs_server->is_mock_running == LBS_SERVER_METHOD_GPS) {
 		mock_stop_tracking(lbs_server);
 		setting_ignore_key_changed(VCONFKEY_LOCATION_MOCK_ENABLED, __setting_mock_cb);
@@ -1335,39 +1437,27 @@ static void shutdown(gpointer userdata, gboolean *shutdown_arr)
 			nps_plugin_stop(lbs_server);
 		}
 	}
-
-#if 0 /* Not needed */
-	int enabled = 0;
-	setting_get_int(VCONFKEY_LOCATION_NETWORK_ENABLED, &enabled);
-	if (enabled == 0) {
-		if (lbs_server->loop != NULL)
-			g_main_loop_quit(lbs_server->loop);
-	} else {
-		if (vconf_notify_key_changed(VCONFKEY_LOCATION_NETWORK_ENABLED, _network_enabled_cb, lbs_server))
-			LOG_NPS(DBG_ERR, "fail to notify VCONFKEY_LOCATION_NETWORK_ENABLED");
-	}
-#endif
 }
 
 #ifdef TIZEN_DEVICE
-static void fused_update_position_cb(fl_location *location, gpointer user_data)
+/*
+ * For fused location
+ */
+static void fused_update_position_cb(gint timestamp, gdouble latitude, gdouble longitude, gdouble altitude,
+		gdouble speed, gdouble direction, gdouble climb, gdouble hor_accuracy, gdouble ver_accuracy, gpointer user_data)
 {
-//	LOG_FUSED_FUNC;
-	if (location == NULL || user_data == NULL) {
-		LOG_FUSED(LOG_ERROR, "parameter is NULL");
+	if (user_data == NULL) {
+		LOG_FUSED(LOG_ERROR, "user_data parameter is NULL");
 		return;
 	}
 	lbs_server_s *lbs_server = (lbs_server_s *)(user_data);
 	LbsPositionExtFields fields = (LBS_POSITION_EXT_FIELDS_LATITUDE | LBS_POSITION_EXT_FIELDS_LONGITUDE
 			| LBS_POSITION_EXT_FIELDS_ALTITUDE | LBS_POSITION_EXT_FIELDS_SPEED | LBS_POSITION_EXT_FIELDS_DIRECTION | LBS_POSITION_EXT_FIELDS_CLIMB);
 
-	GVariant *accuracy = g_variant_new("(idd)", LBS_ACCURACY_LEVEL_DETAILED,
-									location->accuracy.horizontal_accuracy, location->accuracy.vertical_accuracy);
+	GVariant *accuracy = g_variant_new("(idd)", LBS_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy);
 
 	lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_FUSED,
-				fields, location->pos.timestamp,
-				location->pos.latitude, location->pos.longitude, location->pos.altitude,
-				location->vel.speed, location->vel.direction, location->vel.climb, accuracy);
+				fields, timestamp, latitude, longitude, altitude, speed, direction, climb, accuracy);
 }
 #endif
 
@@ -1397,26 +1487,49 @@ static void gps_update_position_cb(pos_data_t *pos, gps_error_t error, void *use
 	gps_set_position(pos);
 
 #ifdef TIZEN_DEVICE
-	if (lbs_server->is_fused_running)
-		send_gps_position_to_fused_engine(pos->timestamp, pos->latitude, pos->longitude, pos->altitude,
-			pos->speed, pos->bearing, pos->hor_accuracy, pos->ver_accuracy);
+	/*
+	 * For fused location
+	 */
+	if (lbs_server->fused_feature) {
+		if (lbs_server->is_fused_running) {
+			if (lbs_server->fused_test_block == 0)
+			send_gps_position_to_fused_engine(pos->timestamp, pos->latitude, pos->longitude, pos->altitude,
+				pos->speed, pos->bearing, pos->hor_accuracy, pos->ver_accuracy);
+			else
+				LOG_GPS(DBG_LOW, "[VIRTUAL] GPS disalbed temporary");
+		}
+	}
 #endif
 
 	lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_GPS, fields, pos->timestamp,
 					pos->latitude, pos->longitude, pos->altitude, pos->speed, pos->bearing, 0.0, accuracy);
 }
 
-static void gps_update_batch_cb(batch_data_t *batch, void *user_data)
+static void gps_update_batch_cb(batch_data_t *bat, void *user_data)
 {
 	lbs_server_s *lbs_server = (lbs_server_s *)(user_data);
-	memcpy(&lbs_server->batch, batch, sizeof(batch_data_t));
+
+	int idx;
+	GVariant *batch_info = NULL;
+	GVariantBuilder *batch_builder = NULL;
+	memcpy(&lbs_server->batch, bat, sizeof(batch_data_t));
 
 	if (lbs_server->status != LBS_STATUS_AVAILABLE) {
 		lbs_server_emit_status_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_GPS, LBS_STATUS_AVAILABLE);
 		lbs_server->status = LBS_STATUS_AVAILABLE;
 	}
 
-	lbs_server_emit_batch_changed(lbs_server->lbs_dbus_server, batch->num_of_location);
+	batch_builder = g_variant_builder_new(G_VARIANT_TYPE("a(iddddddd)"));
+	for (idx = 0; idx < bat->num_of_location; idx++) {
+		g_variant_builder_add(batch_builder, "(iddddddd)", bat->data[idx].timestamp,
+					bat->data[idx].latitude, bat->data[idx].longitude, bat->data[idx].altitude,
+					bat->data[idx].speed, bat->data[idx].bearing,
+					bat->data[idx].hor_accuracy, bat->data[idx].ver_accuracy);
+	}
+
+	batch_info = g_variant_builder_end(batch_builder);
+
+	lbs_server_emit_batch_changed(lbs_server->lbs_dbus_server, bat->num_of_location, batch_info);
 }
 
 static void gps_update_satellite_cb(sv_data_t *sv, void *user_data)
@@ -1537,6 +1650,13 @@ static void lbs_server_init(lbs_server_s *lbs_server)
 	lbs_server->fused_balance_count = 0;
 	lbs_server->current_location_source = LOCATION_SOURCE_NONE;
 
+	lbs_server->fused_feature = FALSE;
+	const char *FUSED_FEATURE = "http://tizen.org/feature/location.fused";
+	bool is_fused_supported = false;
+	system_info_get_platform_bool(FUSED_FEATURE, &is_fused_supported);
+	if (is_fused_supported)
+		lbs_server->fused_feature = TRUE;
+
 	nps_init(lbs_server);
 
 	/* create resource for dynamic-interval */
@@ -1557,8 +1677,17 @@ static void lbs_server_init(lbs_server_s *lbs_server)
 #endif
 
 #ifdef TIZEN_DEVICE
-	lbs_server->fused_interval_table = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, NULL);
-	location_fused_init(fused_update_position_cb, lbs_server);
+	/*
+	 * For fused location
+	 */
+	if (lbs_server->fused_feature) {
+		lbs_server->fused_interval_table = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, NULL);
+		location_fused_init(fused_update_position_cb, lbs_server);
+	#if (VIRTUAL_BLOCK_AREA)
+		LOG_GPS(DBG_LOW, "[VIRTUAL] temporary vconf added");
+		vconf_notify_key_changed(VCONFKEY_LOCATION_SUPL_VERSION, _block_cb, lbs_server);
+	#endif
+	}
 #endif
 }
 
@@ -1608,7 +1737,7 @@ static void nps_get_last_position(lbs_server_s *lbs_server)
 		if (++index == MAX_NPS_LOC_ITEM) break;
 		last_location[index] = (char *)strtok_r(NULL, ";", &last);
 	}
-	LOG_NPS(DBG_LOW, "[%d] %lf, %lf", lbs_server->last_pos.timestamp, lbs_server->last_pos.latitude, lbs_server->last_pos.longitude);
+	LOG_NPS(DBG_LOW, "[WPS get_last: %d] %lf, %lf", lbs_server->last_pos.timestamp, lbs_server->last_pos.latitude, lbs_server->last_pos.longitude);
 }
 
 static void nps_init(lbs_server_s *lbs_server)
@@ -1769,8 +1898,18 @@ int main(int argc, char **argv)
 	LOG_GPS(DBG_LOW, "lbs_server deamon Stop....");
 
 #ifdef TIZEN_DEVICE
-	location_fused_deinit();
-	g_hash_table_destroy(lbs_server->fused_interval_table);
+	/*
+	 * For fused location
+	 */
+	if (lbs_server->fused_feature) {
+		location_fused_deinit();
+
+	#if (VIRTUAL_BLOCK_AREA)
+		LOG_GPS(DBG_LOW, "[VIRTUAL] temporary vconf removed");
+		vconf_ignore_key_changed(VCONFKEY_LOCATION_SUPL_VERSION, _block_cb);
+	#endif
+		g_hash_table_destroy(lbs_server->fused_interval_table);
+	}
 #endif
 
 	gps_deinit_log();
@@ -1955,24 +2094,28 @@ static void client_count_updater(lbs_server_s *lbs_server, lbs_server_method_e m
 	switch (method) {
 		case LBS_SERVER_METHOD_GPS: {
 			g_mutex_lock(&lbs_server->mutex);
-			if (type == _LBS_CLIENT_ADD)
+			if (type == _LBS_CLIENT_ADD) {
 				lbs_server->gps_client_count++;
-			else if (type == _LBS_CLIENT_REMOVE)
-				lbs_server->gps_client_count--;
-			else if (type == _LBS_CLIENT_REMOVE_ALL)
+			} else if (type == _LBS_CLIENT_REMOVE) {
+				if (lbs_server->gps_client_count > 0)
+					lbs_server->gps_client_count--;
+			} else if (type == _LBS_CLIENT_REMOVE_ALL) {
 				lbs_server->gps_client_count = 0;
+			}
 
 			g_mutex_unlock(&lbs_server->mutex);
 			break;
 		}
 		case LBS_SERVER_METHOD_NPS: {
 			g_mutex_lock(&lbs_server->mutex);
-			if (type == _LBS_CLIENT_ADD)
+			if (type == _LBS_CLIENT_ADD) {
 				lbs_server->nps_client_count++;
-			else if (type == _LBS_CLIENT_REMOVE)
-				lbs_server->nps_client_count--;
-			else if (type == _LBS_CLIENT_REMOVE_ALL)
+			} else if (type == _LBS_CLIENT_REMOVE) {
+				if (lbs_server->nps_client_count > 0)
+					lbs_server->nps_client_count--;
+			} else if (type == _LBS_CLIENT_REMOVE_ALL) {
 				lbs_server->nps_client_count = 0;
+			}
 
 			g_mutex_unlock(&lbs_server->mutex);
 			break;
@@ -1983,30 +2126,39 @@ static void client_count_updater(lbs_server_s *lbs_server, lbs_server_method_e m
 		}
 	}
 
-	if (type == _LBS_CLIENT_ADD) {
-		if (fused_mode == LOCATION_FUSED_HIGH)
-			lbs_server->fused_high_count++;
-		else if (fused_mode == LOCATION_FUSED_BALANCED)
-			lbs_server->fused_balance_count++;
-	} else if (type == _LBS_CLIENT_REMOVE) {
-		if (fused_mode == LOCATION_FUSED_HIGH)
-			lbs_server->fused_high_count--;
-		else if (fused_mode == LOCATION_FUSED_BALANCED)
-			lbs_server->fused_balance_count--;
-	} else if (type == _LBS_CLIENT_REMOVE_ALL) {
-		lbs_server->fused_high_count = 0;
-		lbs_server->fused_balance_count = 0;
-	}
+	/*
+	 * For fused location
+	 */
+	if (lbs_server->fused_feature) {
+
+		if (type == _LBS_CLIENT_ADD) {
+			if (fused_mode == LOCATION_FUSED_HIGH)
+				lbs_server->fused_high_count++;
+			else if (fused_mode == LOCATION_FUSED_BALANCED)
+				lbs_server->fused_balance_count++;
+		} else if (type == _LBS_CLIENT_REMOVE) {
+			if (fused_mode == LOCATION_FUSED_HIGH)
+				lbs_server->fused_high_count--;
+			else if (fused_mode == LOCATION_FUSED_BALANCED)
+				lbs_server->fused_balance_count--;
+		} else if (type == _LBS_CLIENT_REMOVE_ALL) {
+			lbs_server->fused_high_count = 0;
+			lbs_server->fused_balance_count = 0;
+		}
 
-	if (lbs_server->fused_high_count < 0) {
-		LOG_GPS(DBG_ERR, "fused_high count is negative value");
-		lbs_server->fused_high_count = 0;
-	}
+		if (lbs_server->fused_high_count < 0) {
+			LOG_GPS(DBG_ERR, "fused_high count is negative value");
+			lbs_server->fused_high_count = 0;
+		}
 
-	if (lbs_server->fused_balance_count < 0) {
-		LOG_GPS(DBG_ERR, "fused_balance count is negative value");
-		lbs_server->fused_balance_count = 0;
+		if (lbs_server->fused_balance_count < 0) {
+			LOG_GPS(DBG_ERR, "fused_balance count is negative value");
+			lbs_server->fused_balance_count = 0;
+		}
 	}
+
+	LOG_GPS(DBG_LOW, ">>> client count updater [method: %d, type: %d, gps: %d, wps: %d]",
+					method, type, lbs_server->gps_client_count, lbs_server->nps_client_count);
 }
 
 
diff --git a/module/gps_module.c b/module/gps_module.c
index 499d560..fed1691 100644
--- a/module/gps_module.c
+++ b/module/gps_module.c
@@ -182,15 +182,39 @@ static void position_callback(GVariant *param, void *user_data)
 
 static void batch_callback(GVariant *param, void *user_data)
 {
-	MOD_LOGD("batch_callback");
 	GpsManagerData *module = (GpsManagerData *)user_data;
 	g_return_if_fail(module);
 	g_return_if_fail(module->batch_cb);
 
-	int num_of_location = 0;
-	g_variant_get(param, "(i)", &num_of_location);
+	guint idx;
+	int num_of_location = 0, timestamp = 0;
+	double lat = 0.0, lon = 0.0, alt = 0.0, speed = 0.0, direction = 0.0, horizontal = 0.0, vertical = 0.0;
+
+	LocationBatch *batch = NULL;
+	GVariant *batch_data = NULL;
+	GVariantIter *batch_iter = NULL;
+
+	g_variant_get(param, "(i@a(iddddddd))", &num_of_location, &batch_data);
+	g_variant_get(batch_data, "a(iddddddd)", &batch_iter);
+	MOD_LOGD("batch_callback - batch size [%d]", num_of_location);
+
+	batch = location_batch_new(num_of_location);
+	batch->num_of_location = num_of_location;
+
+	GVariant *tmp_var = NULL;
+	for (idx = 0; idx < num_of_location; idx++) {
+		tmp_var = g_variant_iter_next_value(batch_iter);
+		g_variant_get(tmp_var, "(iddddddd)", &timestamp, &lat, &lon, &alt, &speed, &direction, &horizontal, &vertical);
+
+		MOD_LOGD("idx[%d] %d, %lf, %lf, %lf ", idx, timestamp, lat, lon, alt);
+		location_set_batch_details(batch, idx, lat, lon, alt, speed, direction, horizontal, vertical, timestamp);
+		g_variant_unref(tmp_var);
+	}
 
-	module->batch_cb(TRUE, num_of_location, module->userdata);
+	module->batch_cb(TRUE, num_of_location, batch, module->userdata);
+	location_batch_free(batch);
+	g_variant_iter_free(batch_iter);
+	g_variant_unref(batch_data);
 }
 
 static void on_signal_batch_callback(const gchar *sig, GVariant *param, gpointer user_data)
diff --git a/packaging/lbs-server.spec b/packaging/lbs-server.spec
index 31853ba..efea4a1 100644
--- a/packaging/lbs-server.spec
+++ b/packaging/lbs-server.spec
@@ -1,6 +1,6 @@
 Name:    lbs-server
 Summary: LBS Server for Tizen
-Version: 1.2.6
+Version: 1.3.0
 Release: 1
 Group:   Location/Service
 License: Apache-2.0
@@ -18,6 +18,7 @@ BuildRequires: pkgconfig(lbs-location)
 BuildRequires: pkgconfig(lbs-dbus)
 BuildRequires: pkgconfig(gio-unix-2.0)
 BuildRequires: pkgconfig(capi-network-wifi)
+BuildRequires: pkgconfig(capi-system-info)
 BuildRequires: pkgconfig(gio-2.0)
 BuildRequires: pkgconfig(vconf-internal-keys)
 BuildRequires: pkgconfig(gthread-2.0)