#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
*/
typedef struct {
int num_of_location; /**< Number of batch data */
+ pos_data_t *data;
} batch_data_t;
/**
#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)
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,
{
// 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)
{
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) {
#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) */
--- /dev/null
+/*
+ * 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);
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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];
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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;
+ }
+ }
+ }
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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 "Conversions.h"
+
+double conversions_radians_to_positive_degrees(double radians)
+{
+ 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);
+ else if (d >= 360)
+ do {
+ d -= 360;
+ } while (d >= 360);
+ return d;
+}
+
+double conversions_radians_to_balanced_degrees(double 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);
+ else if (d >= 180)
+ do {
+ d -= 360;
+ } while (d >= 180);
+ return d;
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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);
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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);
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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]);
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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);
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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];
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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);
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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));
+}
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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_ */
+++ /dev/null
-/*
- * 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);
-}
+++ /dev/null
-/*
- * 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__ */
+++ /dev/null
-/*
- * 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];
-}
+++ /dev/null
-/*
- * 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__ */
+++ /dev/null
-/*
- * 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.
- */
-
-#define __FL_CONVERSIONS_C__
-
-#include "conversions.h"
-
-fl_degrees fl_radians_to_positive_degrees(fl_radians radians)
-{
- fl_degrees d;
-
- d = (180 / M_PI) * radians;
- /* When the value only ocassionally enters adjacent branches this is more
- efficient a way than division modulo. */
- if (d < 0)
- do { d += 360; } while (d < 0);
- else if (d >= 360)
- do { d -= 360; } while (d >= 360);
- return d;
-}
-
-fl_degrees fl_radians_to_balanced_degrees(fl_radians radians)
-{
- fl_degrees d;
-
- 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);
- else if (d >= 180)
- do { d -= 360; } while (d >= 180);
- return d;
-}
+++ /dev/null
-/*
- * 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_ */
+++ /dev/null
-/*
- * 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__ */
+++ /dev/null
-/*
- * 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);
-}
+++ /dev/null
-/*
- * 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__ */
+++ /dev/null
-/*
- * 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;
-}
+++ /dev/null
-/*
- * 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__ */
+++ /dev/null
-/*
- * 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);
- }
-}
+++ /dev/null
-/*
- * 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__ */
+++ /dev/null
-/*
- * 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];
-}
+++ /dev/null
-/*
- * 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__ */
+++ /dev/null
-/*
- * 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);
-}
-
*/
#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>
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 */
#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__ */
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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__ */
#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 */
/** [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
+++ /dev/null
-/*
- * 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__ */
#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"
#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;
/* 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;
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
LOCATION_SOURCE_NONE = 0,
LOCATION_SOURCE_GPS,
LOCATION_SOURCE_WPS,
- LOCATION_SOURCE_BOTH,
+ LOCATION_SOURCE_ALL,
} lbs_current_location_source;
typedef enum {
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;
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,
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);
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;
}
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;
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;
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;
}
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);
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");
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;
}
}
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) {
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);
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) {
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
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;
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);
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
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)
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 */
#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
}
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)
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();
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;
}
}
- 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);
}
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)", ×tamp, &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)
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
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)
projects / platform / core / location / lbs-server.git / commitdiff