From: kj7.sung Date: Thu, 31 Aug 2017 08:32:46 +0000 (+0900) Subject: Fused Pedestrian mode X-Git-Tag: submit/tizen_4.0/20170920.092211~1 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=1f20e53df49c634e1a1584bb63649364c3553985;p=platform%2Fcore%2Flocation%2Flbs-server.git Fused Pedestrian mode Change-Id: I840da7d5431d776ef13338da3a8dd8ace30ebca1 Signed-off-by: kj7.sung --- diff --git a/lbs-server/src/fused.c b/lbs-server/src/fused.c index b6c2f6c..cdf3ea1 100644 --- a/lbs-server/src/fused.c +++ b/lbs-server/src/fused.c @@ -19,190 +19,199 @@ #define __LOCATION_FUSED_C__ #define _USE_MATH_DEFINES +#include #include #include #include -#include "fused/types.h" -#include "fused/conversions.h" -#include "fused/kalman-filter.h" +#include +#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(); + double params[4]; + vconf_get_dbl(VCONFKEY_LOCATION_MANUAL_HORIZONTAL_ACCURACY, ¶ms[0]); + vconf_get_dbl(VCONFKEY_LOCATION_MANUAL_LATITUDE, ¶ms[1]); + vconf_get_dbl(VCONFKEY_LOCATION_MANUAL_LONGITUDE, ¶ms[2]); + vconf_get_dbl(VCONFKEY_LOCATION_MANUAL_ALTITUDE, ¶ms[3]); + LOG_FUSED(LOG_DEBUG, "gyro_bias update_timestamp = %.10f, x = %.10f, y = %.10f, z = %.10f", + params[0], params[1], params[2], params[3]); + fusion_engine_set_calibration(&fused.fusion_engine, params); + 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); + + double params[4]; + fusion_engine_get_calibration(&fused.fusion_engine, params); + LOG_FUSED(LOG_DEBUG, "params[0] = %.10f, params[1] = %.10f, params[2] = %.10f, params[3] = %.10f", + params[0], params[1], params[2], params[3]); + vconf_set_dbl(VCONFKEY_LOCATION_MANUAL_HORIZONTAL_ACCURACY, params[0]); + vconf_set_dbl(VCONFKEY_LOCATION_MANUAL_LATITUDE, params[1]); + vconf_set_dbl(VCONFKEY_LOCATION_MANUAL_LONGITUDE, params[2]); + vconf_set_dbl(VCONFKEY_LOCATION_MANUAL_ALTITUDE, params[3]); } void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server) @@ -212,31 +221,40 @@ void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server) LOG_FUSED(LOG_ERROR, "location_fused_init failed !!!"); return; } - - fused.attached_sensors = NO_SENSOR_FLAG; - fused.started_sensors = NO_SENSOR_FLAG; + fused.sensors[ACCELEROMETER].type = SENSOR_ACCELEROMETER; + fused.sensors[GYROSCOPE].type = SENSOR_GYROSCOPE; + fused.sensors[PEDOMETER].type = SENSOR_HUMAN_PEDOMETER; fused.fused_pos_cb = _fused_pos_cb; fused.lbs_server_handle = lbs_server; - - sensor_func_create(); - fused_engine_init(on_motion_state, on_engine_position, lbs_server); + fused.status.started = FALSE; + fused.status.paused = FALSE; + fused.last_gps_speed = -1.0; + fused.last_gps_timestamp = 0; + fused.last_wps_timestamp = 0; + sensors_clear(); + sensors_create(); + + time_t now = time(NULL); + unsigned long long boot_time_ns = get_current_boot_time(); + fused.utc_offset_sec = (double)now - (double)boot_time_ns / 1000000000.0; + LOG_FUSED(LOG_DEBUG, "now = %lu, boot_time_ns = %llu, utc_offset_sec = %.10f", now, boot_time_ns, fused.utc_offset_sec); + fusion_engine_init(&fused.fusion_engine, on_motion_state, fused.utc_offset_sec); } void location_fused_deinit() { LOG_FUSED_FUNC; - - // stop sensors - if (fused.status.started) { - fused.status.started = FALSE; - fused_engine_stop(); - sensor_func_stop(fused.started_sensors); - } - fused_engine_exit(); - sensor_func_destroy(); - - fused.fused_pos_cb = NULL; + if (fused.status.started) + location_fused_stop(); + sensors_destroy(); + fusion_engine_exit(&fused.fusion_engine); + fused.fused_pos_cb = NULL; fused.lbs_server_handle = NULL; + fused.status.started = FALSE; + fused.status.paused = FALSE; + fused.last_gps_speed = -1.0; + fused.last_gps_timestamp = 0; + fused.last_wps_timestamp = 0; } void send_gps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double altitude, @@ -244,60 +262,118 @@ void send_gps_position_to_fused_engine(time_t timestamp, double latitude, double { // LOG_FUSED_FUNC; - LocationPosition *pos = location_position_new(timestamp, latitude, longitude, altitude, LOCATION_STATUS_3D_FIX); - LocationVelocity *vel = location_velocity_new(timestamp, speed, bearing, 0.0); - LocationAccuracy *accuracy = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy); + if (!fused.status.started) + return; + + fused.last_gps_timestamp = timestamp; + fused.last_gps_speed = speed; - static fl_uncertainty_union __u; - __u.accuracy = *accuracy; - __u.sigma.of_speed = __u.sigma.of_horizontal_pos * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA; - __u.sigma.of_climb = __u.sigma.of_altitude * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA; - const fl_sigma* sigma = &__u.sigma; - fused_engine_process_position_2x3d_event(pos, vel, sigma); + bool is_new_position = fusion_engine_position_3d_event(&fused.fusion_engine, + (double)timestamp, latitude, longitude, altitude, hor_accuracy, 100.0, speed / 3.6, + hor_accuracy * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA, bearing, 0.0, + 100.0 * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA); - location_position_free(pos); - location_velocity_free(vel); - location_accuracy_free(accuracy); + if (is_new_position) + on_engine_new_position(); } -void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy, double ver_accuracy) +void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy) { - LOG_FUSED_FUNC; - LocationPosition *pos = location_position_new(timestamp, latitude, longitude, 0.0, LOCATION_STATUS_2D_FIX); - LocationAccuracy *accuracy = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy); - - static fl_uncertainty_union __u; - __u.accuracy = *accuracy; - __u.sigma.of_speed = -1; - __u.sigma.of_climb = -1; - const fl_sigma* sigma = &__u.sigma; - fused_engine_process_position_2d_event(pos, sigma); - - location_position_free(pos); - location_accuracy_free(accuracy); +// LOG_FUSED_FUNC; + + if (!fused.status.started) + return; + + fused.last_wps_timestamp = timestamp; + + bool is_new_position = fusion_engine_position_2d_event(&fused.fusion_engine, + (double)timestamp, latitude, longitude, hor_accuracy); + + if (is_new_position) + on_engine_new_position(); } -static void on_engine_position(const fl_position_4d* position, gpointer user_data) +static void on_engine_new_position() { // LOG_FUSED_FUNC; - fused_engine_get_location(&fused.last_location.pos, &fused.last_location.vel, &fused.last_location.sigma); + time_t now = time(NULL); + double seconds_from_last_gps = difftime(now, fused.last_gps_timestamp); + double seconds_from_last_wps = difftime(now, fused.last_wps_timestamp); + double seconds_from_last_position = fmin(seconds_from_last_gps, seconds_from_last_wps); + LOG_FUSED(LOG_DEBUG, "max time %.0f/%.0f[s]", seconds_from_last_position, FL_MAX_TIME_FROM_LAST_POS); + if (seconds_from_last_position > FL_MAX_TIME_FROM_LAST_POS) { + LOG_FUSED(LOG_DEBUG, "max time %.0f/%.0f[s] have passed -> skip output location", + seconds_from_last_position, FL_MAX_TIME_FROM_LAST_POS); + return; + } + fusion_engine_get_wgs_location(&fused.fusion_engine, &fused.last_location); if (fused.fused_pos_cb) - fused.fused_pos_cb(&fused.last_location, user_data); + fused.fused_pos_cb((gint)fused.last_location.time, + (gdouble)fused.last_location.latitude, + (gdouble)fused.last_location.longitude, + (gdouble)fused.last_location.altitude, + (gdouble)fused.last_location.speed, + (gdouble)fused.last_location.direction, + (gdouble)fused.last_location.climb, + (gdouble)fused.last_location.hor_dev, + (gdouble)fused.last_location.ver_dev, + fused.lbs_server_handle); else LOG_FUSED(LOG_ERROR, "fused_pos_cb == NULL"); } static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id) { +// LOG_FUSED_FUNC; + fl_sensor_source sensor_type = PTR2ENUM(sensor_id, fl_sensor_source); if (event) { - fused_engine_process_sensory_event((fl_seconds)((double)(long long)event->timestamp / 1000000.0), - PTR2ENUM(sensor_id, fl_sensory_source), event->values); - } else { - LOG_FUSED(LOG_ERROR, "(handle=%p, event=%p, sensor_id=%d): UNSUPPORTED", - handle, event, PTR2ENUM(sensor_id, fl_sensory_source)); - } + bool is_new_position = false; + switch (sensor_type) { + case ACCELEROMETER: + is_new_position = fusion_engine_acc_event(&fused.fusion_engine, + (double)(long long)event->timestamp / 1000000.0, + event->values[DEV_SPATIAL_X], + event->values[DEV_SPATIAL_Y], + event->values[DEV_SPATIAL_Z]); + break; + case GYROSCOPE: + is_new_position = fusion_engine_gyro_event(&fused.fusion_engine, + (double)(long long)event->timestamp / 1000000.0, + conversions_degrees_to_radians(event->values[DEV_SPATIAL_X]), + conversions_degrees_to_radians(event->values[DEV_SPATIAL_Y]), + conversions_degrees_to_radians(event->values[DEV_SPATIAL_Z])); + break; + case PEDOMETER: + LOG_FUSED(LOG_DEBUG, "[PEDOMETER] timestamp[%llu] accuracy[%d] Step[%.0f] Step[%.0f] [%.1f] Dist[%.3f m] [%.1f] Speed[%.5f m/s] [%.1f] [%.1f]", + event->timestamp, event->accuracy, + event->values[0], event->values[1], event->values[2], event->values[3], + event->values[4], event->values[5], event->values[6], event->values[7]); + + if (fused.last_gps_timestamp > 0 && fused.last_gps_speed > 0) { + double seconds_from_last_gps = (double)(long long)event->timestamp / 1000000.0 + + fused.utc_offset_sec - (double)fused.last_gps_timestamp; + if (seconds_from_last_gps < FL_MAX_PEDOMETER_SKIP_TIME && fused.last_gps_speed >= FL_MIN_PEDOMETER_SKIP_SPEED) { + LOG_FUSED(LOG_DEBUG, "seconds_from_last_gps = %.1f/%.1f[s] && last_gps_speed = %.2f/%.1f[km/h] -> PEDOMETER SKIP", + seconds_from_last_gps, FL_MAX_PEDOMETER_SKIP_TIME, fused.last_gps_speed, FL_MIN_PEDOMETER_SKIP_SPEED); + break; + } + } + + float speed = event->values[5]; + if (fabs(speed) >= 0.001) + is_new_position = fusion_engine_speed_event(&fused.fusion_engine, + (double)(long long)event->timestamp / 1000000.0, event->values[5], 1.0); + break; + default: + LOG_FUSED(LOG_ERROR, "unsupported sensor type[%d]", sensor_type); + break; + } + if (is_new_position) + on_engine_new_position(); + } else + LOG_FUSED(LOG_ERROR, "event == NULL"); } #if (FL_MOTION_DETECTOR) @@ -305,28 +381,23 @@ static void location_fused_pause() { LOG_FUSED_FUNC; if (fused.status.paused == FALSE) { - sensor_func_stop(fused.started_sensors & ~ACCELEROMETER_FLAG); + sensor_stop(GYROSCOPE); + sensor_stop(PEDOMETER); } + fused.status.paused = TRUE; } -static LocationError location_fused_resume() +static void location_fused_resume() { LOG_FUSED_FUNC; - LocationError ret; - if (fused.status.paused) { - ret = sensor_func_start(GYROSCOPE_FLAG); - if (ret == LOCATION_ERROR_NONE) - sensor_func_stop(GYROSCOPE_FLAG); - } else { - ret = LOCATION_ERROR_NOT_AVAILABLE; + sensor_start(GYROSCOPE); + sensor_start(PEDOMETER); } - - LOG_FUSED(LOG_DEBUG, "0x%X", ret); - return ret; + fused.status.paused = FALSE; } -static void on_motion_state(fl_motion_state state, gpointer data) +static void on_motion_state(MotionDetectorState state) { LOG_FUSED(LOG_DEBUG, "(state=%u)", state); if (fused.status.started) { diff --git a/lbs-server/src/fused.h b/lbs-server/src/fused.h index b34a7e1..df7dcf5 100644 --- a/lbs-server/src/fused.h +++ b/lbs-server/src/fused.h @@ -23,159 +23,94 @@ #ifndef __LOCATION_FUSED_H__ #define __LOCATION_FUSED_H__ -#include "gps_plugin_data_types.h" -#include "fused/types.h" -#include "lbs_dbus_server.h" +#if (FL_MOTION_DETECTOR) +#include "fused/MotionDetector.h" +#endif #include -/***************************************************************************//** - * This callback is called with position data. - * - * @param[in] location Location data - * @param[in] user_data User defined data - */ -typedef void (*fused_updated_cb)(fl_location *location, gpointer user_data); +/** Labels for standard sensors */ +typedef enum { + ACCELEROMETER = 0, + GYROSCOPE, + PEDOMETER, + SENSOR_SOURCE_COUNT +} fl_sensor_source; -/***************************************************************************//** - * TODO: TBD +/** + * @brief This callback is called with position data. */ -void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server); +typedef void (*fused_updated_cb)(gint timestamp, gdouble latitude, gdouble longitude, gdouble altitude, + gdouble speed, gdouble direction, gdouble climb, gdouble hor_accuracy, gdouble ver_accuracy, gpointer user_data); -/***************************************************************************//** - * TODO: TBD - */ +void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server); void location_fused_deinit(); - -/***************************************************************************//** - * TODO: TBD - */ void location_fused_start(); - -/***************************************************************************//** - * TODO: TBD - */ void location_fused_stop(); -/***************************************************************************//** - * [callback] Pass the GPS location data to the engine for processing. - * - * @param[in] timestamp timestamp - * @param[in] latitude latitude - * @param[in] longitude longitude - * @param[in] altitude altitude - * @param[in] speed speed - * @param[in] bearing bearing - * @param[in] hor_accuracy hor_accuracy - * @param[in] ver_accuracy ver_accuracy +/** + * @brief [callback] Pass the GPS location data to the engine for processing. + * @param[in] timestamp timestamp + * @param[in] latitude latitude + * @param[in] longitude longitude + * @param[in] altitude altitude + * @param[in] speed speed + * @param[in] bearing bearing + * @param[in] hor_accuracy hor_accuracy + * @param[in] ver_accuracy ver_accuracy */ void send_gps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double altitude, double speed, double bearing, double hor_accuracy, double ver_accuracy); -/***************************************************************************//** - * [callback] Pass the WPS location data to the engine for processing. - * - * @param[in] timestamp timestamp - * @param[in] latitude latitude - * @param[in] longitude longitude - * @param[in] hor_accuracy hor_accuracy - * @param[in] ver_accuracy ver_accuracy +/** + * @brief [callback] Pass the WPS location data to the engine for processing. + * @param[in] timestamp timestamp + * @param[in] latitude latitude + * @param[in] longitude longitude + * @param[in] hor_accuracy hor_accuracy + * @param[in] ver_accuracy ver_accuracy */ -void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, - double hor_accuracy, double ver_accuracy); +void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy); #if defined(__LOCATION_FUSED_C__) -/***************************************************************************//** - * [private] Detect sensors used for dead-reckoning and power-saving. - * - * @return - * Conjunction of the detected and used sensors as bit-set. - */ -static fl_sensory_flags sensor_func_detect(); +static void sensors_create(); -/***************************************************************************//** - * [private] Detect and attach sensors used for dead-reckoning and power-saving. - */ -static void sensor_func_create(); +const char* sensor_error_to_str(int error); -/***************************************************************************//** - * [private] Detach used sensors. - */ -static void sensor_func_destroy(); +static void sensors_clear(); -/***************************************************************************//** - * [private] Start selected subset of sensors. - * - * @param[in] sensors - * Bit-set indicator of the sensors to be started. - * - * @return - * - LOCATION_ERROR_NONE (zero) upon success; - * - LOCATION_ERROR_NOT_SUPPORTED if the sensors are not attached; - * - LOCATION_ERROR_CONFIGURATION if the sensors could not be started. - */ -static LocationError sensor_func_start(fl_sensory_flags sensors); +static void sensors_reset_listeners(); -/***************************************************************************//** - * [private] Stop selected subset of sensors. - * - * @param[in] sensors - * Bit-set indicator of the sensors to be stopped. - */ -static void sensor_func_stop(fl_sensory_flags sensors); +static void sensors_destroy(); -/***************************************************************************//** - * [private, callback] Pass the sensory data to the engine. - * - * @param[in] handle - * Sensor handle - * @param[in] event - * VConf key name to watch for changes - * @param[in] sensor_id - * TODO: TBD - */ -static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id); +static void sensor_start(fl_sensor_source sensor); -/***************************************************************************//** - * [private, callback] Invoked upon change of position reported by the engine. - * There is no distinction between predicted and fused one; it can be either of - * the two. Upon checking user-supplied conditions the notification is issued. - * - * @param[in] position - * New spatio-temporal (4D) position. - */ -static void on_engine_position(const fl_position_4d *position, gpointer user_data); +static void sensor_stop(fl_sensor_source sensor); -#if (FL_MOTION_DETECTOR) +static void on_engine_new_position(); -/***************************************************************************//** - * [private] Enters power-saving mode by unsubscribing from location source, and - * and unused sensor(s)). +/** + * @brief Pass the sensory data to the engine. + * @param[in] handle sensor handle + * @param[in] event vconf key name to watch for changes + * @param[in] sensor_id sensor id */ +static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id); + +#if (FL_MOTION_DETECTOR) +/** Enters power-saving mode by unsubscribing from location source, and and unused sensor(s)) */ static void location_fused_pause(); -/***************************************************************************//** - * [private] Resumes normal-power operation mode by subscribing to location - * source, and and used sensor(s). - * - * @return - * - LOCATION_ERROR_NONE (zero) upon success; - * - error code otherwise. - */ -static LocationError location_fused_resume(); +/** Resumes normal-power operation mode by subscribing to location source, and and used sensor(s) */ +static void location_fused_resume(); -/***************************************************************************//** - * [private, callback] Receive notifications about motion state changes, and - * pause/resume accordingly. - * - * @param[in] state - * New motion state +/** + * @brief Receive notifications about motion state changes, and pause/resume accordingly. + * @param[in] state New motion state */ -static void on_motion_state(fl_motion_state state, gpointer data); +static void on_motion_state(MotionDetectorState state); #else /* (FL_MOTION_DETECTOR) */ -/***************************************************************************//** - * [private] Compilation stub. - */ +/** Compilation stub */ #define on_motion_state NULL #endif /* (FL_MOTION_DETECTOR) */ diff --git a/lbs-server/src/fused/AccelerometerFilter.c b/lbs-server/src/fused/AccelerometerFilter.c new file mode 100644 index 0000000..eb2b7df --- /dev/null +++ b/lbs-server/src/fused/AccelerometerFilter.c @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "AccelerometerFilter.h" +#include "Conversions.h" +#include "math-ext.h" +#include "debug_util.h" + +#define _USE_MATH_DEFINES +#include + +#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, =%5.2f"), time, af_len, g2); + + if (af_len > FL_ACCEL_NOISE_LEVEL && g2 > 1) { + const double norm_out = 1.0 / af_len; + vector_3d_mul_number(af, norm_out, gu); + *w2gu = self->gravity_estimator.base.decay_rate / (fl_square(gu[X]) + fl_square(gu[Y]) + fl_square(gu[Z] - 1) + GEO_DIMENSION * DEFAULT_ACCELERATION_SIGMA2); + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, decay=%.16g, gu=(%.16g, %.16g, %.16g), af_len=%.16g, overlap=%.16g, g2=%.16g"), + time, self->gravity_estimator.base.decay_rate, gu[X], gu[Y], gu[Z], af_len, overlap, g2); + } else { + vector_3d_set(gu, 0, 0, 1); /* default */ + *w2gu = self->gravity_estimator.base.decay_rate / (GEO_DIMENSION * DEFAULT_ACCELERATION_SIGMA2); + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, decay=%.16g, gu=(%.16g, %.16g, %.16g), af_len=%.16g, overlap=%.16g, g2=%.16g"), + time, self->gravity_estimator.base.decay_rate, gu[X], gu[Y], gu[Z], af_len, overlap, g2); + } + + Vector_3d al; /* Linear acceleration in global frame. */ + const double g = sqrt(g2); + if (g > 1) + vector_3d_linear_combination(EARTH_GRAVITY / g, acc, -EARTH_GRAVITY, gu, al); + else + vector_3d_copy(al, acc); + + motion_detector_process(&self->motion_detector, time, al); +} diff --git a/lbs-server/src/fused/AccelerometerFilter.h b/lbs-server/src/fused/AccelerometerFilter.h new file mode 100644 index 0000000..47fabfd --- /dev/null +++ b/lbs-server/src/fused/AccelerometerFilter.h @@ -0,0 +1,81 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file AccelerometerFilter.h + * @brief Accelerometer calibration (scale and sampling frequency), and + * low-pass 3D filtering. + */ + +#pragma once +#ifndef __ACCELEROMETER_FILTER_H__ +#define __ACCELEROMETER_FILTER_H__ + +#include "FrequencyEstimator.h" +#include "Butterworth3dFilter.h" +#include "MotionDetector.h" +#include "GravityEstimator.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** Accelerometer calibration (scale and sampling frequency), and low-pass 3D filtering */ +typedef struct { + FrequencyEstimator frequency_estimator; + Butterworth3dFilter lp_3d_filter; + GravityEstimator gravity_estimator; + MotionDetector motion_detector; +} AccelerometerFilter; + +/** + * @brief Initialization of the AccelerometerFilter object (constructor). + * @param[in] on_motion_state_changed Callback to be invoked when the detected state of motion changes. + */ +void accelerometer_filter_init(AccelerometerFilter* self, MotionDetectorStateChangeListener on_motion_state_changed); + +void accelerometer_filter_exit(AccelerometerFilter* self); + +/** + * @brief Reset of the internal state back to initial one. + * This function is used for repetitive testing and module soft restarts. + * @param[in] av Stationary acceleration state to be set (typically {0, 0, g}). + */ +void accelerometer_filter_reset(AccelerometerFilter* self, const_Vector_3d_ref av); + +/** + * @brief Processing of a single sample from the accelerometer. This function performs: + * - estimation of the sampling frequency, + * - estimation of the gravity direction via low-pass 3D filter (cf. LP3D unit), + * - estimation of the gravity value (cf. GRES unit) and scale correction, + * - estimation of the linear acceleration i.e. with subtracted gravity component, + * - motion state detection (cf. MOTI unit) + * @param[in] time Time of the event in seconds. + * @param[in] acc Measured 3D acceleration vector in global frame. + * @param[out] gu Unit vector along gravity direction oriented upwards. + * @param[out] w2gu Estimated inverse covariance (squared weight) of a @gu. + */ +void accelerometer_filter_process(AccelerometerFilter* self, + double time, const_Vector_3d_ref acc, Vector_3d_ref gu, double *w2gu); + +/** @return double */ +#define accelerometer_filter_get_update_rate(self) gravity_estimator_get_update_rate(&(self)->__gravity_estimator) + +#ifdef __cplusplus +} +#endif + +#endif /* __ACCELEROMETER_FILTER_H__ */ diff --git a/lbs-server/src/fused/Butterworth3dFilter.c b/lbs-server/src/fused/Butterworth3dFilter.c new file mode 100644 index 0000000..064d417 --- /dev/null +++ b/lbs-server/src/fused/Butterworth3dFilter.c @@ -0,0 +1,104 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#include "Butterworth3dFilter.h" +#include "math-ext.h" +#include "debug_util.h" + +#define LP3D_FORMAT "% 6.2f" + +void butterworth_3d_filter_init(Butterworth3dFilter* self, double f) +{ + LOG_FUSED_DEV(DBG_LOW, INDENT("()")); + self->__cut_off_frequency = f; + self->__lp3d_G = 0; + self->__lp3d_B1 = -2; + self->__lp3d_B2 = 1; + butterworth_3d_filter_reset(self); + LOG_FUSED_DEV(DBG_LOW, UNINDENT("(): OK")); +} + +void butterworth_3d_filter_exit(Butterworth3dFilter* self)\ +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()")); +} + +void butterworth_3d_filter_set_sampling_frequency(Butterworth3dFilter* self, double f) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("frequency() f=%.16e, cut_off=%.16e"), f, self->__cut_off_frequency); + + const double omegaC = tan(M_PI * self->__cut_off_frequency / f); + const double omegaC2 = fl_square(omegaC); + const double _B0 = 1.0 / (1 + 2 * omegaC * cos(M_PI_4) + omegaC2); + self->__lp3d_B1 = _B0 * 2*(omegaC2 - 1); + self->__lp3d_B2 = _B0 * (1 - 2 * omegaC * cos(M_PI_4) + omegaC2); + self->__lp3d_G = _B0 * omegaC2; + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%g Hz): OK"), f); +} + +void butterworth_3d_filter_reset(Butterworth3dFilter* self) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()")); + vector_3d_clear_array(self->__lp3d_u); + vector_3d_clear_array(self->__lp3d_v); +} + +void butterworth_3d_filter_reset_to(Butterworth3dFilter* self, const_Vector_3d_ref stationary) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_FORMAT ", " LP3D_FORMAT ", " LP3D_FORMAT "})"), stationary[0], stationary[1], stationary[2]); + unsigned i = GEO_DIMENSION; + do { + --i; + /* up-conversion */ + self->__lp3d_u[TIME_SHIFT_1][i] = stationary[i]; + self->__lp3d_u[TIME_SHIFT_2][i] = stationary[i]; + self->__lp3d_v[TIME_SHIFT_1][i] = stationary[i]; + self->__lp3d_v[TIME_SHIFT_2][i] = stationary[i]; + } while (i); +} + +const_Vector_3d_ref butterworth_3d_filter_process(Butterworth3dFilter* self, const_Vector_3d_ref u) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_FORMAT ", " LP3D_FORMAT ", " LP3D_FORMAT "})"), u[0], u[1], u[2]); + unsigned i = GEO_DIMENSION; + do { + --i; +#define u1 self->__lp3d_u[TIME_SHIFT_1][i] +#define u2 self->__lp3d_u[TIME_SHIFT_2][i] +#define v1 self->__lp3d_v[TIME_SHIFT_1][i] +#define v2 self->__lp3d_v[TIME_SHIFT_2][i] + const double u0 = u[i]; + const double v0 = self->__lp3d_G * (u0 + 2 * u1 + u2) - self->__lp3d_B1 * v1 - self->__lp3d_B2 * v2; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("process() i=%i, __lp3d_G=%.16e, __lp3d_B1=%.16e, u0=%.16e, u1=%.16e, u2=%.16e, v0=%.16e, v1=%.16e, v2=%.16e"), + i, self->__lp3d_G, self->__lp3d_B1, u0, u1, u2, v0, v1, v2); + + u2 = u1; + u1 = u0; + v2 = v1; + v1 = v0; +#undef u1 +#undef u2 +#undef v1 +#undef v2 + } while (i); + return self->__lp3d_v[TIME_SHIFT_1]; +} diff --git a/lbs-server/src/fused/Butterworth3dFilter.h b/lbs-server/src/fused/Butterworth3dFilter.h new file mode 100644 index 0000000..5d9b555 --- /dev/null +++ b/lbs-server/src/fused/Butterworth3dFilter.h @@ -0,0 +1,80 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file Butterworth3dFilter.h + * @brief Low-pass 3D 2nd order Butterworth filter. + */ + +#pragma once +#ifndef __BUTTERWORTH_3D_FILTER_H__ +#define __BUTTERWORTH_3D_FILTER_H__ + +#include "Vector.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef enum { + TIME_SHIFT_1, /* t - 1 */ + TIME_SHIFT_2, /* t - 2 */ + TIME_SHIFT_COUNT +} TimeShift; + +typedef struct { + /* Filter coefficients. */ + double __cut_off_frequency; + double __lp3d_G; /* omega^2/B_0 */ + double __lp3d_B1; /* B_1/B_0 */ + double __lp3d_B2; /* B_2/B_0 */ + + /* Memory of past states. */ + Vector_3d __lp3d_u[TIME_SHIFT_COUNT]; + Vector_3d __lp3d_v[TIME_SHIFT_COUNT]; +} Butterworth3dFilter; + +void butterworth_3d_filter_init(Butterworth3dFilter* self, double f); + +void butterworth_3d_filter_exit(Butterworth3dFilter* self); + +void butterworth_3d_filter_reset(Butterworth3dFilter* self); + +/** + * @brief Adjust internal parameters to the given sampling frequency and reset the the past state memory. + * @param[in] f The sampling frequency in [Hz]. + */ +void butterworth_3d_filter_set_sampling_frequency(Butterworth3dFilter* self, double f); + +/** + * @brief Reset to the given stationary state (input = output). + * @param[in] stationary 3D vector of acceleration measured in tangent coordinate frame. + */ +void butterworth_3d_filter_reset_to(Butterworth3dFilter* self, const_Vector_3d_ref stationary); + +/** + * @brief Process single input sample through the 3D Butterworth filter. + * @param[in] u 3D acceleration input vector. + * @return 3D acceleration output vector. + */ +const_Vector_3d_ref butterworth_3d_filter_process(Butterworth3dFilter* self, const_Vector_3d_ref u); + +#ifdef __cplusplus +} +#endif + +#endif /* __BUTTERWORTH_3D_FILTER_H__ */ diff --git a/lbs-server/src/fused/CalibrationFilter.c b/lbs-server/src/fused/CalibrationFilter.c new file mode 100644 index 0000000..54c85be --- /dev/null +++ b/lbs-server/src/fused/CalibrationFilter.c @@ -0,0 +1,142 @@ +/* + * 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" + +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; + +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); +} + +void calibration_filter_get_calibration(CalibrationFilter* self, double* params) +{ + params[0] = self->cal_prec; + /* params[0] = self->calibration_time + self->utc_offset; */ + params[1] = self->bias[0]; + params[2] = self->bias[1]; + params[3] = self->bias[2]; +} + +void calibration_filter_set_calibration(CalibrationFilter* self, double* params) +{ + self->cal_prec = params[0]; + /* self->calibration_time = params[0] - self->utc_offset; */ + self->bias[0] = params[1]; + self->bias[1] = params[2]; + self->bias[2] = params[3]; +} diff --git a/lbs-server/src/fused/CalibrationFilter.h b/lbs-server/src/fused/CalibrationFilter.h new file mode 100644 index 0000000..1159b4d --- /dev/null +++ b/lbs-server/src/fused/CalibrationFilter.h @@ -0,0 +1,51 @@ +/* + * 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 + +#define CALIBRATION_LENGTH 4 + +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 void calibration_filter_get_calibration(CalibrationFilter* self, double* params); +extern void calibration_filter_set_calibration(CalibrationFilter* self, double* params); + +#endif /* __CALIBRATIONFILTER_H_ */ diff --git a/lbs-server/src/fused/Conversions.c b/lbs-server/src/fused/Conversions.c new file mode 100644 index 0000000..affa501 --- /dev/null +++ b/lbs-server/src/fused/Conversions.c @@ -0,0 +1,49 @@ +/* + * 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; +} diff --git a/lbs-server/src/fused/Conversions.h b/lbs-server/src/fused/Conversions.h new file mode 100644 index 0000000..42aaa66 --- /dev/null +++ b/lbs-server/src/fused/Conversions.h @@ -0,0 +1,73 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file Conversions.h + * @brief Units conversion functions + */ + +#pragma once +#ifndef __CONVERSIONS_H__ +#define __CONVERSIONS_H__ + +#include "math-ext.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define FL_UNDEFINED_TIME -1e+48 +#define KNOWN_TIME(t) (t > FL_UNDEFINED_TIME) + +/** + * @brief Convert degrees to radians. + * @param[in] degrees Value in degrees. + * @return Corresponding value in radians. + */ +#define conversions_degrees_to_radians(degrees) ((M_PI / 180) * (degrees)) + +/** + * @brief Convert radians to degrees. + * @param[in] radians Value in radians. + * @return Corresponding value in degrees. + */ +#define conversions_radians_to_degrees(radians) ((180 / M_PI) * (radians)) + +/** + * @brief Convert radians to degrees in the [0,360) range + * @param[in] radians Arbitrary value in radians. + * @return Value in degrees projected onto the [0,360) branch. + */ +double conversions_radians_to_positive_degrees(double radians); + +/** + * @brief Convert radians to degrees in the [-180,180) range. + * @param[in] radians Arbitrary value in radians. + * @return Value in degrees projected onto the [-180,180) branch. + */ +double conversions_radians_to_balanced_degrees(double radians); + +/** Convert radians to latitude degrees [-90,90] */ +#define conversions_radians_to_latitude conversions_radians_to_balanced_degrees + +/** Convert radians to longitude degrees [0, 360) */ +#define conversions_radians_to_longitude conversions_radians_to_positive_degrees + +#ifdef __cplusplus +} +#endif + +#endif /* __CONVERSIONS_H_ */ diff --git a/lbs-server/src/fused/FrequencyEstimator.c b/lbs-server/src/fused/FrequencyEstimator.c new file mode 100644 index 0000000..863de85 --- /dev/null +++ b/lbs-server/src/fused/FrequencyEstimator.c @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "FrequencyEstimator.h" +#include "Conversions.h" +#include "debug_util.h" + +#define FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD (1.0 * 1.0) + +void frequency_estimator_init(FrequencyEstimator* self, double initial_frequency) +{ + self->frequency = initial_frequency; + self->frequency_update_time = FL_UNDEFINED_TIME; + self->samples = 0; + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("frequency_estimator_process() last_t=%.16e, last_f=%.16e"), self->frequency_update_time, self->frequency); +} + +bool frequency_estimator_process(FrequencyEstimator* self, double time) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16e, last_t=%.16e, f=%.16e, c=%i"), time, self->frequency_update_time, self->frequency, self->samples); + + bool frequency_updated = false; + if (KNOWN_TIME(self->frequency_update_time)) { + ++(self->samples); + const double dt = time - self->frequency_update_time; + + if (dt > 0) { + const double f = self->samples / dt; + const double df = fabs(f - self->frequency); + if (dt * df >= FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD) { + self->samples = 0; + self->frequency = f; + self->frequency_update_time = time; + frequency_updated = true; + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("last_t=%.16e, f=%.16e"), time, self->frequency); + } + } + + } else { + self->frequency_update_time = time; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("last_t=%.16e, f=%.16e"), time, self->frequency); + } + + return frequency_updated; +} diff --git a/lbs-server/src/fused/FrequencyEstimator.h b/lbs-server/src/fused/FrequencyEstimator.h new file mode 100644 index 0000000..ea5b809 --- /dev/null +++ b/lbs-server/src/fused/FrequencyEstimator.h @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file FrequencyEstimator.h + * @brief Estimates frequency of arbitrary timed events. + */ + +#pragma once +#ifndef __FREQUENCYESTIMATOR_H_ +#define __FREQUENCYESTIMATOR_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef struct { + double frequency; /* Current estimated frequency. */ + double frequency_update_time; /* Time of last frequency update. */ + unsigned samples; /* Number of samples from last frequency update. */ +} FrequencyEstimator; + +/** + * @brief Initialization of the FrequencyEstimator object (constructor). + * @param[in] initial_frequency Initial frequency. + */ +extern void frequency_estimator_init(FrequencyEstimator* self, double initial_frequency); + +/** (double) Returns current frequency. */ +#define frequency_estimator_get_frequency(self) ((self)->frequency) + +/** + * @brief Called when new event happens. + * @param[in] time Time of event. + * @return Is new frequency calculated. + */ +extern bool frequency_estimator_process(FrequencyEstimator* self, double time); + +#ifdef __cplusplus +} +#endif + +#endif /* __FREQUENCYESTIMATOR_H_ */ diff --git a/lbs-server/src/fused/FusionEngine.c b/lbs-server/src/fused/FusionEngine.c new file mode 100644 index 0000000..31763d3 --- /dev/null +++ b/lbs-server/src/fused/FusionEngine.c @@ -0,0 +1,344 @@ +/* + * 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); + /* + double t = time_offset_correct_time(&self->__timestamp_offset, time); + */ + location_filter_update_speed(&self->kalf, /*t*/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, /*t*/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; +} + +void fusion_engine_get_orientation(FusionEngine* self, double* o) +{ + orientation_get_orientation(&self->__orientation, o); +} + +void fusion_engine_set_enable_gyro_filter(FusionEngine* self, bool enable) +{ + self->__enable_gyro_filter = enable; +} + +void fusion_engine_set_enable_static_calibration(FusionEngine* self, bool enable) +{ + self->__enable_static_calibration = enable; +} + +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)); +} + +void fusion_engine_get_calibration(FusionEngine* self, double* params) +{ + calibration_filter_get_calibration(&(self->__calibration_filter), params); +} + +void fusion_engine_set_calibration(FusionEngine* self, double* params) +{ + calibration_filter_set_calibration(&(self->__calibration_filter), params); +} diff --git a/lbs-server/src/fused/FusionEngine.h b/lbs-server/src/fused/FusionEngine.h new file mode 100644 index 0000000..a94b9a7 --- /dev/null +++ b/lbs-server/src/fused/FusionEngine.h @@ -0,0 +1,248 @@ +/* + * 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 +#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 + +#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] sigma_of_altitude 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 sigma_of_altitude, + 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); + +/** + * @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); + +/** + * @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); + +/** + * @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); + +/** + * @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. + * @param[out] params Gyroscope calibration parameters. Must be length of minimum 4. + */ +extern void fusion_engine_get_calibration(FusionEngine* self, double* params); + +/** + * @brief Sets new gyroscope calibration parameters. + * @param[in] params New gyroscope calibration parameters. Must be length of minimum 4. + */ +extern void fusion_engine_set_calibration(FusionEngine* self, double* params); + +#ifdef __cplusplus +} +#endif + +#endif /* __FUSION_ENGINE_H__ */ diff --git a/lbs-server/src/fused/GravityEstimator.h b/lbs-server/src/fused/GravityEstimator.h new file mode 100644 index 0000000..101d6f3 --- /dev/null +++ b/lbs-server/src/fused/GravityEstimator.h @@ -0,0 +1,73 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file GravityEstimator.h + * @brief Estimator of the gravity value used for accelerometer scale calibration. + */ + +#pragma once +#ifndef __GRAVITY_ESTIMATOR_H__ +#define __GRAVITY_ESTIMATOR_H__ + +#include "MovingAverageFilters.h" +#include "parameters.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef MovingAverage1dFilter GravityEstimator; + +#define gravity_estimator_init(self) moving_average_1d_filter_init(self, (1 << FL_GRES_AEMA_PLATEAU_BITS)) + +#define gravity_estimator_exit(self) moving_average_1d_filter_exit(self) + +/** + * @brief Bring the internal state back to initial one. This function is used for repetitive testing and module soft restarts. + */ +#define gravity_estimator_reset(self) moving_average_1d_filter_reset(self) + +/** + * @brief Processing of a single data sample. + * @param[in] g2 Modulus squared of the measured acceleration projected onto z-axis. + * @return[double] Estimated value of the Earth gravity. + */ +#define gravity_estimator_process(self, g2) moving_average_1d_filter_process(self, g2) + +/** + * @brief Bring the filter to a stationary state (EMA mode, input = output). This method is used for testing. + * @param[in] g2 Modulus squared value of Earth gravity measured by the accelerometer. + */ +#define gravity_estimator_set_estimate(self, g2) moving_average_1d_filter_set_estimate(self, g2) + +/** + * @brief Fetch the current gravity estimate. + * @param[in] self Object pointer + * @return[double] Output value after last processed sample. + */ + +/** Retrieve current estimate */ +#define gravity_estimator_get_estimate(self) moving_average_1d_filter_get_estimate(self) + +#define gravity_estimator_get_update_rate(self) moving_average_get_update_rate(&(self)->base) + +#ifdef __cplusplus +} +#endif + +#endif /* __GRAVITY_ESTIMATOR_H__ */ diff --git a/lbs-server/src/fused/GyroscopeFilter.c b/lbs-server/src/fused/GyroscopeFilter.c new file mode 100644 index 0000000..376f9d4 --- /dev/null +++ b/lbs-server/src/fused/GyroscopeFilter.c @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#include "GyroscopeFilter.h" +#include "GravityEstimator.h" +#include "math-ext.h" +#include "debug_util.h" + +#define _USE_MATH_DEFINES +#include + +#define GYRO_PLATEAU_EVIDENCE (1 << FL_GYRO_AEMA_PLATEAU_BITS) +#define GYRO_SCALE2_I (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_I))) +#define GYRO_SCALE2_F (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_F))) + +void gyroscope_filter_init(GyroscopeFilter* self) +{ + LOG_FUSED_DEV(DBG_LOW, INDENT("()")); + gyroscope_filter_reset(self); + LOG_FUSED_DEV(DBG_LOW, UNINDENT("()")); +} + +void gyroscope_filter_exit(GyroscopeFilter* self) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()")); +} + +void gyroscope_filter_reset(GyroscopeFilter* self) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()")); + waema3d_estimator_reset(self, (SQUARE(FL_GYRO_SPIN_SIGMA_I / FL_GYRO_SPIN_SIGMA_F) - 1) / SQUARE(GYRO_PLATEAU_EVIDENCE), GYRO_PLATEAU_EVIDENCE, GYRO_SCALE2_I); +} + +const_Vector_3d_ref gyroscope_filter_process(GyroscopeFilter* self, double time, const_Vector_3d_ref wm) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs)"), time); + return waema3d_estimator_filter(self, wm); +} diff --git a/lbs-server/src/fused/GyroscopeFilter.h b/lbs-server/src/fused/GyroscopeFilter.h new file mode 100644 index 0000000..86fbf2d --- /dev/null +++ b/lbs-server/src/fused/GyroscopeFilter.h @@ -0,0 +1,61 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file GyroscopeFilter.h + * @brief Correction of the gyroscope bias. + */ + +#pragma once +#ifndef __GYROSCOPE_FILTER_H__ +#define __GYROSCOPE_FILTER_H__ + +#include "Waema3dEstimator.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef Waema3dEstimator GyroscopeFilter; + + +void gyroscope_filter_init(GyroscopeFilter* self); +void gyroscope_filter_exit(GyroscopeFilter* self); +void gyroscope_filter_reset(GyroscopeFilter* self); + +/** + * @brief [TEST] Bring the filter to a stationary state (EMA mode, input = output). + * @param[in] w0 3D vector of angular velocity measured by the gyroscope in inertial coordinate frame. + */ +void gyroscope_filter_set(GyroscopeFilter* self, const_Vector_3d_ref w0); + +/** Processing of a vector data sample. This method performs: + * - estimation of the gyroscope bias with the weighted AEMA filter; + * the individual samples are weighted by the distance from average using Gaussian + * curve with monotonically-decreasing sigma parameter; + * - correction of the measurement by the estimated bias. + * @param[in] time Event time in seconds. + * @param[in] wm 3D vector of measured angular velocity. + * @return 3D vector of bias-corrected angular velocity. + */ +const_Vector_3d_ref gyroscope_filter_process(GyroscopeFilter* self, double time, const_Vector_3d_ref wm); + +#ifdef __cplusplus +} +#endif + +#endif /* __GYROSCOPE_FILTER_H__ */ diff --git a/lbs-server/src/fused/LocationFilter.c b/lbs-server/src/fused/LocationFilter.c new file mode 100644 index 0000000..b6b063b --- /dev/null +++ b/lbs-server/src/fused/LocationFilter.c @@ -0,0 +1,479 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "LocationFilter.h" +#include "Matrix.h" +#include "Conversions.h" +#include "debug_util.h" + +#include + +#define TIME_FORMAT "%.9fs" + +#define NEAR_ZERO 1.0e-200 + +static bool location_filter_update_heading(_fl_kalf_state_m* self) +{ + const double vx = self->v[X]; + const double vy = self->v[Y]; + if (fabs(vx) > 0.001 || fabs(vy) > 0.001) { + self->heading = atan2(vy, vx); + return true; + } + return false; +} + +void location_filter_init(LocationFilter* self, double x, double y, double z, double var_p, double var_v, double var_a) +{ + self->default_var_a = var_a; + self->last_correction_time = FL_UNDEFINED_TIME; + memset(&self->prior, 0, sizeof(self->prior)); + memset(&self->posterior, 0, sizeof(self->posterior)); + self->__z_rot_speed = 0; + self->prior.x.p[X] = self->posterior.x.p[X] = x; + self->prior.x.p[Y] = self->posterior.x.p[Y] = y; + self->prior.x.p[Z] = self->posterior.x.p[Z] = z; + unsigned i; + for (i = GEO_DIMENSION; i;) { + --i; + /* diagonal terms */ + self->prior.s2x[i][POS][POS] = self->posterior.s2x[i][POS][POS] = var_p; + self->prior.s2x[i][VEL][VEL] = self->posterior.s2x[i][VEL][VEL] = var_v; + self->prior.s2x[i][ACC][ACC] = self->posterior.s2x[i][ACC][ACC] = var_a; + } + location_filter_update_heading(&self->posterior.x); + self->prior.x.heading = self->posterior.x.heading; + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]); +} + +/** + * @brief Check for and correct the numerical errors which cause the covariance matrix + * to cease being positive-definite. This operation has to be performed regularly, + * or otherwise magic ensues, and every strange result becomes possible. + * @param[in, out] src The state object to be rectified. + * This is either the predicted (prior), or estimated (posterior) one. + */ +static void __rectify_state(_fl_kalf_sigma_cube s2x) +{ + unsigned i; + for (i = GEO_DIMENSION; i--;) { + Matrix_3d_ref s2xi = s2x[i]; + /* diagonal elements */ +#define CHECK(u) \ + if (s2xi[u][u] < 0) { \ + /*LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): src.s2x[%u][" #u "][" #u "] = %g < 0"), src, i, s2xi[u][u]);*/ \ + s2xi[u][u] = 0; \ + } + CHECK(POS); + CHECK(VEL); + CHECK(ACC); +#undef CHECK + + /* partial determinants */ + double uuvv, uvvu; +#define CHECK(u, v) \ + uuvv = s2xi[u][u] * s2xi[v][v]; \ + uvvu = s2xi[u][v] * s2xi[v][u]; \ + if (uuvv < uvvu) { \ + /*LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): det(src.s2x[%u], " #u ", " #v ") = %g < 0"), src, i, uuvv - uvvu);*/ \ + s2xi[u][v] = \ + s2xi[v][u] *= sqrt(uuvv / uvvu); \ + } + CHECK(POS, VEL); + CHECK(POS, ACC); + CHECK(ACC, VEL); +#undef CHECK + } +} + +/** + * @brief Copy of the state (3x3D position & covariance) object. + * @param[out] dst Destination state object + * @param[out] src Source state object + */ +static void __copy_state(_fl_kalf_state *dst, _fl_kalf_state *src) +{ + __rectify_state(src->s2x); + memcpy(dst, src, sizeof(*dst)); +} + +void location_filter_update_state(LocationFilter* self) +{ + memcpy(&self->prior.x, &self->posterior.x, sizeof(self->prior.x)); +} + +/** + * @brief Calculates prior from posterior, i.e. extrapolates position, + * velocity, and the covariance-matrices up to the given time. + * We assume rotation is at constant angular rate (__z_rot_speed) and speed (norm of velocity) is also constant. + * Therefore motion equations are: + * Vx = vx cos(wt) - vy sin(wt) + * Vy = vy cos(wt) + vx sin(wt) + * Vz = vz + * X = x + vx sin(wt)/w + vy(cos(wt) - 1)/w (by integrating) + * Y = y + vy sin(wt)/w - vx(cos(wt) - 1)/w + * Z = z + vz t + * Ax = -vy w (differentiating at t=0) + * Ay = vx w + * Az = 0 + * And transition matrix is: + * | 1 0 0 sin(wt)/w (cos(wt) - 1)/w 0 0 0 0| + * | 0 1 0 -(cos(wt) - 1)/w sin(wt)/w 0 0 0 0| + * | 0 0 1 0 0 t 0 0 0| + * | 0 0 0 cos(wt) -sin(wt) 0 0 0 0| + * | 0 0 0 sin(wt) -cos(wt) 0 0 0 0| + * | 0 0 0 0 0 1 0 0 0| + * | 0 0 0 0 -w 0 0 0 0| + * | 0 0 0 w 0 0 0 0 0| + * | 0 0 0 0 0 0 0 0 0| + * so for small wt: + * | 1 0 0 t wt^2/2 0 0 0 0| + * | 0 1 0 -wt^2/2 t 0 0 0 0| + * | 0 0 1 0 0 t 0 0 0| + * | 0 0 0 1 - w^2t^2/2 -t 0 0 0 0| + * | 0 0 0 t -1 + w^2t^2/2 0 0 0 0| + * | 0 0 0 0 0 1 0 0 0| + * | 0 0 0 0 -w 0 0 0 0| + * | 0 0 0 w 0 0 0 0 0| + * | 0 0 0 0 0 0 0 0 0| + * We assume covariance matrix can be represented as + * 3 independent covariance matrixes, one for each coordinate, + * and assume transition matrix used for covariance update + * for each coordinate is: + * | 1 t t^2/2 | + * | 0 1 t | + * | 0 0 1 | + * Process noise matrix ??? + * + * @param[in] time Internal time + */ +static void location_filter_predict(LocationFilter* self, double time) +{ + if (!KNOWN_TIME(self->last_correction_time)) { + /* no prior data */ + return; + } + double dt = time - self->last_correction_time; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT "), dt=" TIME_FORMAT), time, dt); + if (dt <= 0) { + return; + } + const _fl_kalf_state_m* post = &self->posterior.x; + _fl_kalf_state_m* prior = &self->prior.x; + double z_rot_speed = self->__z_rot_speed; + double rot = z_rot_speed * dt; + if (fabs(rot) > NEAR_ZERO) { + double cos_rot = cos(rot); + double sin_rot = sin(rot); + double inv_z_rot_speed = 1.0 / z_rot_speed; + double cos_rot_inv = (cos_rot - 1) * inv_z_rot_speed; + double sin_rot_inv = sin_rot * inv_z_rot_speed; + + prior->p[X] = post->p[X] + post->v[X] * sin_rot_inv + post->v[Y] * cos_rot_inv; + prior->p[Y] = post->p[Y] + post->v[Y] * sin_rot_inv - post->v[X] * cos_rot_inv; + prior->v[X] = post->v[X] * cos_rot - post->v[Y] * sin_rot; + prior->v[Y] = post->v[Y] * cos_rot + post->v[X] * sin_rot; + if (!location_filter_update_heading(prior)) { + prior->heading = post->heading + rot; + } + } else { + prior->p[X] = post->p[X] + dt * post->v[X]; + prior->p[Y] = post->p[Y] + dt * post->v[Y]; + prior->v[X] = post->v[X]; + prior->v[Y] = post->v[Y]; + prior->heading = post->heading; + } + prior->p[Z] = post->p[Z] + dt * post->v[Z]; + prior->v[Z] = post->v[Z]; + + prior->a[X] = -prior->v[Y] * z_rot_speed; + prior->a[Y] = prior->v[X] * z_rot_speed; + prior->a[Z] = 0; + + const_fl_kalf_sigma_cube_ref post_s = self->posterior.s2x; + _fl_kalf_sigma_cube_ref prior_s = self->prior.s2x; + unsigned i; + for (i = GEO_DIMENSION; i;) { + --i; + + const_Matrix_3d_ref post_si = post_s[i]; + Matrix_3d_ref prior_si = prior_s[i]; + + prior_si[ACC][ACC] = post_si[ACC][ACC]; + prior_si[ACC][VEL] = post_si[ACC][VEL] + post_si[ACC][ACC] * dt; + prior_si[ACC][POS] = post_si[ACC][POS] + + (post_si[ACC][VEL] + post_si[ACC][ACC] * dt / 2) * dt; + + prior_si[VEL][ACC] = prior_si[ACC][VEL]; /* by symmetry */ + prior_si[VEL][VEL] = post_si[VEL][VEL] + + (post_si[VEL][ACC] * 2 + post_si[ACC][ACC] * dt) * dt; + prior_si[VEL][POS] = post_si[VEL][POS] + + (post_si[VEL][VEL] + post_si[POS][ACC] + + (post_si[VEL][ACC] * 3 + + post_si[ACC][ACC] * dt) / 2 * dt) * dt; + + prior_si[POS][ACC] = prior_si[ACC][POS]; /* by symmetry */ + prior_si[POS][VEL] = prior_si[VEL][POS]; /* by symmetry */ + prior_si[POS][POS] = post_si[POS][POS] + + (post_si[POS][VEL] * 2 + + (post_si[VEL][VEL] + post_si[ACC][POS] + + (post_si[ACC][VEL] + + post_si[ACC][ACC] * dt / 4) * dt) + * dt) * dt; + } + /* process noise */ + prior_s[X][ACC][ACC] += fl_square(prior->a[X] - post->a[X]); + prior_s[Y][ACC][ACC] += fl_square(prior->a[Y] - post->a[Y]); + __rectify_state(prior_s); + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]); +} + +/** + * @brief Update of the 2D position + * Observation matrix for each coordinate is: + * | 1 0 0| + * + * @param[in] time Internal time + * @param[in] pos Measured position + * @param[in] pos_ac Measured position deviation (squared standard deviation) + */ +void location_filter_kalman_p(LocationFilter* self, double time, const_Vector_3d_ref pos, const_Vector_3d_ref pos_ac) +{ + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=%g, x0=%g, x1=%g, x2=%g, s0=%g, s1=%g, s2=%g)"), + time, pos[0], pos[1], pos[2], pos_ac[0], pos_ac[1], pos_ac[2]); + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("input (t=%g, p0=%g, p1=%g, p2=%g, v0=%g, v1=%g, v2=%g)"), + time, self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2], self->prior.x.v[0], self->prior.x.v[1], self->prior.x.v[2]); + + location_filter_predict(self, time); + unsigned i; + /* @note WPS-specific: restrict the update to horizontal position */ + for (i = PLANAR_DIMENSION; i;) { + --i; + const double s2mi = pos_ac[i]; + Matrix_3d_ref post_si = self->posterior.s2x[i]; + const_Matrix_3d_ref prior_si = self->prior.s2x[i]; + const_Vector_3d_ref s2pis = prior_si[POS]; + const double s2piss = s2pis[POS]; + const double innovation_covariance = s2mi + s2piss; + if (innovation_covariance > 0) { + const double inv_inn_cov = 1.0 / innovation_covariance; + const double innovation = pos[i] - self->prior.x.p[i]; + + /* reduced Joseph form in special 3x1 case: subtract scaled projection */ +#define SET_FIELD(a, b) post_si[a][b] = inv_inn_cov * ((prior_si[a][b] * s2piss - s2pis[a] * s2pis[b]) + prior_si[a][b] * s2mi) +#define CPY_FIELD(a, b) post_si[a][b] = post_si[b][a] + SET_FIELD(POS, POS); + SET_FIELD(POS, VEL); + SET_FIELD(POS, ACC); + CPY_FIELD(VEL, POS); + SET_FIELD(VEL, VEL); + SET_FIELD(VEL, ACC); + CPY_FIELD(ACC, POS); + CPY_FIELD(ACC, VEL); + SET_FIELD(ACC, ACC); +#undef CPY_FIELD +#undef SET_FIELD + + self->posterior.x.p[i] = self->prior.x.p[i] + innovation * inv_inn_cov * s2pis[POS]; + self->posterior.x.v[i] = self->prior.x.v[i] + innovation * inv_inn_cov * s2pis[VEL]; + self->posterior.x.a[i] = self->prior.x.a[i] + innovation * inv_inn_cov * s2pis[ACC]; + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(i=%i, t=%g, prev=%g, dx=%g, inv_s2k=%g, s2pis[POS]=%g, cur=%g)"), + i, time, self->prior.x.p[i], innovation, inv_inn_cov, s2piss, self->posterior.x.p[i]); + } + } + /* the third coordinate is of `special care': we want to preserve it in the tangent frame, along with its sigma */ + self->posterior.x.p[Z] = pos[Z]; + location_filter_update_heading(&self->posterior.x); + __copy_state(&self->prior, &self->posterior); + self->last_correction_time = time; + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]); +} + +/** + * @brief Update of the 3D position & velocity + * Observation matrix for each coordinate is: + * | 1 0 0| + * | 0 1 0| + * + * @param[in] time Internal time + * @param[in] pm Measured position + * @param[in] vm Measured velocity + * @param[in] s2pm Measured position deviation (squared standard deviation) + * @param[in] s2vm Measured velocity deviation (squared standard deviation) + */ +void location_filter_kalman_pv(LocationFilter* self, double time, + const_Vector_3d_ref pm, const_Vector_3d_ref vm, const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm) +{ + location_filter_predict(self, time); + unsigned i; + for (i = GEO_DIMENSION; i;) { + --i; + + /* shortcuts */ + const_Matrix_3d_ref prior_cov = self->prior.s2x[i]; + + /* Sk = Sp + Sm */ + Matrix_2d innovation_covariance; + innovation_covariance[POS][POS] = prior_cov[POS][POS] + s2pm[i]; + innovation_covariance[POS][VEL] = /* == prior_cov[POS][VEL] */ + innovation_covariance[VEL][POS] = prior_cov[VEL][POS]; + innovation_covariance[VEL][VEL] = prior_cov[VEL][VEL] + s2vm[i]; + + Matrix_2d inv_inn_cov; + if (matrix_2d_invert(innovation_covariance, inv_inn_cov)) { + Matrix_2d k0; + matrix_3d_multiply_matrix_2d(prior_cov, inv_inn_cov, k0); /* K = Sp.Si */ + /* 1 - K = Sm.Si, use positive form */ + Matrix_2d k1; + k1[POS][POS] = s2pm[i] * inv_inn_cov[POS][POS]; + k1[POS][VEL] = s2pm[i] * inv_inn_cov[POS][VEL]; + k1[VEL][POS] = s2vm[i] * inv_inn_cov[VEL][POS]; + k1[VEL][VEL] = s2vm[i] * inv_inn_cov[VEL][VEL]; + /* state update */ + self->posterior.x.p[i] = k0[POS][POS] * pm[i] + + k0[POS][VEL] * vm[i] + + k1[POS][POS] * self->prior.x.p[i] + + k1[POS][VEL] * self->prior.x.v[i]; + + self->posterior.x.v[i] = k0[VEL][POS] * pm[i] + + k0[VEL][VEL] * vm[i] + + k1[VEL][POS] * self->prior.x.p[i] + + k1[VEL][VEL] * self->prior.x.v[i]; + + /* covariance update Se = K1,K1.Sp + K,K.Sm */ + Matrix_3d_ref s2ei = self->posterior.s2x[i]; + +#define SET_S2E(j, k) s2ei[j][k] = \ + k1[j][POS] * k1[k][POS] * prior_cov[POS][POS] + k0[j][POS] * k0[k][POS] * s2pm[i] \ + + k1[j][POS] * k1[k][VEL] * prior_cov[POS][VEL] \ + + k1[j][VEL] * k1[k][POS] * prior_cov[VEL][POS] \ + + k1[j][VEL] * k1[k][VEL] * prior_cov[VEL][VEL] + k0[j][VEL] * k0[k][VEL] * s2vm[i] + + SET_S2E(POS, POS); + SET_S2E(POS, VEL); + SET_S2E(VEL, POS); + SET_S2E(VEL, VEL); +#undef SET_S2E + } + /* upon arrival of new position-velocity pair (a maximally uncertain) the pair (p, v) decorrelates from a: */ + self->posterior.s2x[i][POS][ACC] = 0; + self->posterior.s2x[i][VEL][ACC] = 0; + self->posterior.s2x[i][ACC][POS] = 0; + self->posterior.s2x[i][ACC][VEL] = 0; + self->posterior.s2x[i][ACC][ACC] = self->prior.s2x[i][ACC][ACC]; + } + location_filter_update_heading(&self->posterior.x); + __copy_state(&self->prior, &self->posterior); + self->last_correction_time = time; + + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]); +} + +/** + * @brief Update of the 2D position using speed measurement + * Observation matrix is: + * | 0 vx/v 0 0 vy/v 0 0 0 0| + * Where v = sqrt(vx^2 + vy^2) + * + * @param[in] time Internal time + * @param[in] speed Measured horizontal speed + * @param[in] sigma_of_speed Measured horizontal speed deviation (squared standard deviation) + */ +void location_filter_update_speed(LocationFilter* self, double time, double speed, double sigma_of_speed) +{ + location_filter_predict(self, time); + const double vx = self->prior.x.v[X]; + const double vy = self->prior.x.v[Y]; + const double v = sqrt(vx * vx + vy * vy); + double ch; + double sh; + if (v != 0) { + ch = vx/v; + sh = vy/v; + } else { + const double heading = self->prior.x.heading; + ch = cos(heading); + sh = sin(heading); + } + const double ch2 = ch * ch; + const double sh2 = sh * sh; + const double innovation_covariance = + ch2 * self->prior.s2x[X][VEL][VEL] + sh2 * self->prior.s2x[Y][VEL][VEL] + SQUARE(sigma_of_speed); + const double innovation = speed - v; + /* Update state estimate */ + const double vxk = ch * innovation / innovation_covariance; + self->posterior.x.p[X] = self->prior.x.p[X] + self->prior.s2x[X][POS][VEL] * vxk; + self->posterior.x.v[X] = self->prior.x.v[X] + self->prior.s2x[X][VEL][VEL] * vxk; + self->posterior.x.a[X] = self->prior.x.a[X] + self->prior.s2x[X][ACC][VEL] * vxk; + + const double vyk = sh * innovation / innovation_covariance; + self->posterior.x.p[Y] = self->prior.x.p[Y] + self->prior.s2x[Y][POS][VEL] * vyk; + self->posterior.x.v[Y] = self->prior.x.v[Y] + self->prior.s2x[Y][VEL][VEL] * vyk; + self->posterior.x.a[Y] = self->prior.x.a[Y] + self->prior.s2x[Y][ACC][VEL] * vyk; + + self->posterior.x.p[Z] = self->prior.x.p[Z]; + self->posterior.x.v[Z] = self->prior.x.v[Z]; + self->posterior.x.a[Z] = self->prior.x.a[Z]; + + /* Update covariance estimate */ + Vector_2d vv; + vv[0] = ch2 / innovation_covariance; + vv[1] = sh2 / innovation_covariance; + unsigned i; + for (i = PLANAR_DIMENSION; i;) { + --i; + const_Matrix_3d_ref prior_cov = self->prior.s2x[i]; + Matrix_3d_ref post_cov = self->posterior.s2x[i]; + unsigned s1; + for (s1 = STATE_DIMENSION; s1;) { + s1--; + unsigned s2; + for (s2 = s1 + 1; s2;) { + s2--; + post_cov[s2][s1] = post_cov[s1][s2] = + prior_cov[s1][s2] - prior_cov[s1][VEL] * prior_cov[VEL][s2] * vv[i]; + } + } + } + matrix_3d_copy(self->posterior.s2x[Z], self->prior.s2x[Z]); + location_filter_update_heading(&self->posterior.x); + __copy_state(&self->prior, &self->posterior); + self->last_correction_time = time; +} + +void location_filter_clear_acc(LocationFilter* self, double time) +{ + unsigned i; + for (i = GEO_DIMENSION; i--;) { + self->posterior.x.a[i] = 0; + self->posterior.s2x[i][ACC][ACC] = self->default_var_a; + } + + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]); + + location_filter_predict(self, time); +} + +void location_filter_update_acc(LocationFilter* self, double time) +{ + location_filter_predict(self, time); + self->posterior.s2x[X][ACC][ACC] = fl_square(self->prior.x.a[X] - self->posterior.x.a[X]); + self->posterior.s2x[Y][ACC][ACC] = fl_square(self->prior.x.a[Y] - self->posterior.x.a[Y]); + __copy_state(&self->posterior, &self->prior); + self->last_correction_time = time; + + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]); +} diff --git a/lbs-server/src/fused/LocationFilter.h b/lbs-server/src/fused/LocationFilter.h new file mode 100644 index 0000000..82b0711 --- /dev/null +++ b/lbs-server/src/fused/LocationFilter.h @@ -0,0 +1,135 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file LocationFilter.h + * @brief Implementation of a 4D (2D position + 2D velocity) Kalman filter + */ + +#pragma once +#ifndef __LOCATION_FILTER_H_ +#define __LOCATION_FILTER_H_ + +#include "parameters.h" +#include "Vector.h" +#include "Matrix.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + POS, /* position [m] */ + VEL, /* velocity [m/s] */ + ACC, /* acceleration [m/s^2] */ + STATE_DIMENSION +} _fl_kalf_state_component; + +typedef struct { + Vector_3d p; /* Position */ + Vector_3d v; /* Velocity */ + Vector_3d a; /* Acceleration */ + double heading; /* Heading */ +} _fl_kalf_state_m; + +typedef Matrix_3d _fl_kalf_sigma_cube[GEO_DIMENSION]; +typedef Matrix_3d(*_fl_kalf_sigma_cube_ref); +typedef const Matrix_3d* const_fl_kalf_sigma_cube_ref; + +typedef struct { + _fl_kalf_state_m x; /* state vector */ + _fl_kalf_sigma_cube s2x; /* covariance matrix */ +} _fl_kalf_state; + +typedef struct { + double default_var_a; + _fl_kalf_state prior; /* predicted (a priori) state */ + _fl_kalf_state posterior; /* corrected (a posteriori) state */ + double last_correction_time; /* update of posterior */ + double __z_rot_speed; /* reference angular z-velocity (horizontal plane) */ +} LocationFilter; + +/** + * @brief Initialization of the filter (constructor). + * @param[in] x X coordinate of the initial position. + * @param[in] y Y coordinate of the initial position. + * @param[in] z Z coordinate of the initial position. + * @param[in] var_p Initial variance of position. + * @param[in] var_v Initial variance of speed. + * @param[in] var_a Initial variance of acceleration. + */ +extern void location_filter_init(LocationFilter* self, + double x, double y, double z, double var_p, double var_v, double var_a); + +/** + * @brief Sets z rotation speed. + * @param[in] z_rot_speed Z rotation speed in [rad/sec]. + */ +#define location_filter_set_z_rot_speed(self, z_rot_speed) ((self)->__z_rot_speed = (z_rot_speed)) + +extern void location_filter_update_state(LocationFilter* self); + +/** + * @brief Planar (2D) Kalman blending of position. + * @param[in] time Internal time. + * @param[in] pm Measured position. + * @param[in] s2pm Position variances. + */ +extern void location_filter_kalman_p(LocationFilter* self, + double time, const_Vector_3d_ref pm, const_Vector_3d_ref s2pm); + +/** + * @brief Spatial (3D) Kalman blending of position and velocity. + * @param[in] time Internal time. + * @param[in] pm Measured position. + * @param[in] vm Measured velocity. + * @param[in] s2pm Position variances. + * @param[in] s2vm Velocity variances. + */ +extern void location_filter_kalman_pv(LocationFilter* self, double time, + const_Vector_3d_ref pm, const_Vector_3d_ref vm, const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm); + +/** + * @brief Kalman blending of speed. + * @param[in] time Internal time. + * @param[in] speed Measured speed. + * @param[in] sigma_of_speed Variance of speed. + */ +extern void location_filter_update_speed(LocationFilter* self, double time, double speed, double sigma_of_speed); + +extern void location_filter_clear_acc(LocationFilter* self, double time); +extern void location_filter_update_acc(LocationFilter* self, double time); + +/** (Vector_3d_ref) Returns current position estimate */ +#define location_filter_get_position(self) ((self)->posterior.x.p) + +/** (Vector_3d_ref) Returns current velocity estimate */ +#define location_filter_get_velocity(self) ((self)->posterior.x.v) + +/** (const_fl_kalf_sigma_cube_ref) */ +#define location_filter_get_predicted_covariance(self) ((self)->prior.s2x) + +/** (const_Vector_3d_ref) Returns predicted position estimate */ +#define location_filter_get_predicted_position(self) ((self)->prior.x.p) + +/** (const_Vector_3d_ref) Returns predicted velocity estimate */ +#define location_filter_get_predicted_velocity(self) ((self)->prior.x.v) + +#ifdef __cplusplus +} +#endif + +#endif /* __LOCATION_FILTER_H_ */ diff --git a/lbs-server/src/fused/Matrix.c b/lbs-server/src/fused/Matrix.c new file mode 100644 index 0000000..d7f5be2 --- /dev/null +++ b/lbs-server/src/fused/Matrix.c @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "Matrix.h" +#include "math-ext.h" + +void matrix_3d_transpose(Matrix_3d_ref self) +{ + int i, j; + + for (i = GEO_DIMENSION - 1; i >= 0; i--) { + for (j = i - 1; j >= 0; j--) { + double v = self[i][j]; + self[i][j] = self[j][i]; + self[j][i] = v; + } + } +} + +void matrix_3d_multiply_matrix_3d(const_Matrix_3d_ref m1, const_Matrix_3d_ref m2, Matrix_3d_ref out) +{ + int i, j; + + for (i = GEO_DIMENSION - 1; i >= 0; i--) { + for (j = GEO_DIMENSION - 1; j >= 0; j--) { + out[i][j] = m1[i][X] * m2[X][j] + m1[i][Y] * m2[Y][j] + m1[i][Z] * m2[Z][j]; + } + } +} + +void matrix_3d_multiply_vector_3d(const_Matrix_3d_ref m, const_Vector_3d_ref v, Vector_3d_ref out) +{ + out[X] = vector_3d_dot_product(m[X], v); + out[Y] = vector_3d_dot_product(m[Y], v); + out[Z] = vector_3d_dot_product(m[Z], v); +} + +void vector_3d_multiply_matrix_3d(const_Vector_3d_ref v, const_Matrix_3d_ref m, Vector_3d_ref out) +{ + out[X] = m[X][X] * v[X] + m[Y][X] * v[Y] + m[Z][X] * v[Z]; + out[Y] = m[X][Y] * v[X] + m[Y][Y] * v[Y] + m[Z][Y] * v[Z]; + out[Z] = m[X][Z] * v[X] + m[Y][Z] * v[Y] + m[Z][Z] * v[Z]; +} + +void matrix_3d_from_vector_3d(const_Vector_3d_ref p, Matrix_3d_ref out, + double* lat, double* lon, double* sin_lat, double* cos_lat) +{ + double cos_rx, sin_rx; + double cos_ry, sin_ry; + double yz2 = fl_square(p[Y]) + fl_square(p[Z]); + double r2s = sqrt(yz2 + fl_square(p[X])); + if (r2s > 0) { + double _r = 1.0 / r2s; + double yz = sqrt(yz2); + + *lat = atan2(p[X], yz); + *sin_lat = sin_rx = _r * p[X]; + *cos_lat = cos_rx = _r * yz; + if (yz > 0) { + double _yz = 1.0 / yz; + + *lon = atan2(p[Y], p[Z]); + sin_ry = _yz * p[Y]; + cos_ry = _yz * p[Z]; + } else { + *lon = 0; + sin_ry = 0; + cos_ry = 1; + } + } else { + *lat = 0; + *lon = 0; + *sin_lat = sin_rx = 0; + *cos_lat = cos_rx = 1; + sin_ry = 0; + cos_ry = 1; + } + out[X][X] = cos_rx; + out[Y][X] = 0; + out[Z][X] = sin_rx; + + out[X][Y] = -sin_ry * sin_rx; + out[Y][Y] = cos_ry; + out[Z][Y] = sin_ry * cos_rx; + + out[X][Z] = -cos_ry * sin_rx; + out[Y][Z] = -sin_ry; + out[Z][Z] = cos_ry * cos_rx; +} + +void matrix_2d_multiply_matrix_2d(const_Matrix_2d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out) +{ + int i, j; + + for (i = PLANAR_DIMENSION - 1; i >= 0; i--) { + for (j = PLANAR_DIMENSION - 1; j >= 0; j--) { + out[i][j] = m1[i][PLANAR_X] * m2[PLANAR_X][j] + m1[i][PLANAR_Y] * m2[PLANAR_Y][j]; + } + } +} + +void matrix_3d_multiply_matrix_2d(const_Matrix_3d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out) +{ + int i, j; + + for (i = PLANAR_DIMENSION - 1; i >= 0; i--) { + for (j = PLANAR_DIMENSION - 1; j >= 0; j--) { + out[i][j] = m1[i][PLANAR_X] * m2[PLANAR_X][j] + m1[i][PLANAR_Y] * m2[PLANAR_Y][j]; + } + } +} + +void matrix_2d_multiply_vector_2d(const_Matrix_2d_ref m, const_Vector_2d_ref v, Vector_2d_ref out) +{ + out[PLANAR_X] = vector_2d_dot_product(m[PLANAR_X], v); + out[PLANAR_Y] = vector_2d_dot_product(m[PLANAR_Y], v); +} + +void vector_2d_multiply_matrix_2d(const_Vector_2d_ref v, const_Matrix_2d_ref m, Vector_2d_ref out) +{ + out[PLANAR_X] = m[PLANAR_X][PLANAR_X] * v[PLANAR_X] + m[PLANAR_Y][PLANAR_X] * v[PLANAR_Y]; + out[PLANAR_Y] = m[PLANAR_X][PLANAR_Y] * v[PLANAR_X] + m[PLANAR_Y][PLANAR_Y] * v[PLANAR_Y]; +} + +double matrix_2d_det(const_Matrix_2d_ref self) +{ + return self[PLANAR_X][PLANAR_X] * self[PLANAR_Y][PLANAR_Y] - self[PLANAR_Y][PLANAR_X] * self[PLANAR_X][PLANAR_Y]; +} + +bool matrix_2d_invert(const_Matrix_2d_ref self, Matrix_2d_ref out) +{ + const double det = matrix_2d_det(self); + if (det == 0) { + return false; + } + + const double denom = 1.0 / det; + out[PLANAR_X][PLANAR_X] = self[PLANAR_Y][PLANAR_Y] * denom; + out[PLANAR_X][PLANAR_Y] = -self[PLANAR_Y][PLANAR_X] * denom; + out[PLANAR_Y][PLANAR_X] = -self[PLANAR_X][PLANAR_Y] * denom; + out[PLANAR_Y][PLANAR_Y] = self[PLANAR_X][PLANAR_X] * denom; + return true; +} diff --git a/lbs-server/src/fused/Matrix.h b/lbs-server/src/fused/Matrix.h new file mode 100644 index 0000000..b86d911 --- /dev/null +++ b/lbs-server/src/fused/Matrix.h @@ -0,0 +1,169 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file Matrix.h + * @brief matrix + */ + +#pragma once +#ifndef __MATRIX_H_ +#define __MATRIX_H_ + +#include "Vector.h" +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef double Matrix_3d[GEO_DIMENSION][GEO_DIMENSION]; +typedef double(*Matrix_3d_ref)[GEO_DIMENSION]; +typedef const double(*const_Matrix_3d_ref)[GEO_DIMENSION]; + +typedef double Matrix_2d[PLANAR_DIMENSION][PLANAR_DIMENSION]; +typedef double(*Matrix_2d_ref)[PLANAR_DIMENSION]; +typedef const double(*const_Matrix_2d_ref)[PLANAR_DIMENSION]; + +/** + * @brief Clears content of provided 3D matrix. + * @param[in] self 3D matrix to clear. + */ +#define matrix_3d_clear(self) memset(self, 0, sizeof(Matrix_3d)) + +/** + * @brief Copy content of one 3D matrix to another one. + * @param[in] dst Destination 3D matrix. + * @param[in] src Source 3D matrix. + */ +#define matrix_3d_copy(dst, src) memcpy(dst, src, sizeof(Matrix_3d)) + +/** + * @brief Transposition of provided 3D matrix. + * @param[in] self 3D matrix to transpose. + */ +extern void matrix_3d_transpose(Matrix_3d_ref self); + +/** + * @brief Multiplies two 3D matrixes. + * @param[in] m1 First 3D matrix. + * @param[in] m2 Second 3D matrix. + * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2 + */ +extern void matrix_3d_multiply_matrix_3d(const_Matrix_3d_ref m1, const_Matrix_3d_ref m2, Matrix_3d_ref out); + +/** + * @brief Multiplies 3D matrix with 3D vector. + * @param[in] m 3D matrix. + * @param[in] v 3D vector. + * @param[out] out Multiplication of provided matrix and vector, i.e. out = m * v + */ +extern void matrix_3d_multiply_vector_3d(const_Matrix_3d_ref m, const_Vector_3d_ref v, Vector_3d_ref out); + +/** + * @brief Multiplies 3D vector with 3D matrix. + * @param[in] v 3D vector. + * @param[in] m 3D matrix. + * @param[out] out Multiplication of provided vector and matrix, i.e. out = v * m + */ +extern void vector_3d_multiply_matrix_3d(const_Vector_3d_ref v, const_Matrix_3d_ref m, Vector_3d_ref out); + +extern void matrix_3d_from_vector_3d(const_Vector_3d_ref p, Matrix_3d_ref out, + double *lat, double *lon, double* sin_lat, double* cos_lat); + +/** + * @brief Inlined left-action of a linear operator on a 3D covector. + * @param[in] m 3x3D matrix. + * @param[in] v 3D vector. + * @param[out] out Left-action of the linear operator @a m on the vector @a v, i.e. out = m * v + */ +#define MATRIX_DOT_VECTOR_3D(m, v, out)\ + do {\ + (out)[X] = (m)[X][X] * (v)[X] + (m)[X][Y] * (v)[Y] + (m)[X][Z] * (v)[Z];\ + (out)[Y] = (m)[Y][X] * (v)[X] + (m)[Y][Y] * (v)[Y] + (m)[Y][Z] * (v)[Z];\ + (out)[Z] = (m)[Z][X] * (v)[X] + (m)[Z][Y] * (v)[Y] + (m)[Z][Z] * (v)[Z];\ + } while (0) + +/** + * @brief Inlined right-action of a linear operator on a 3D. + * In Euclidean metric this is the same as left action of the transposed operator on the vector. + * @param[in] v 3D vector. + * @param[in] m 3x3D matrix. + * @param[out] out Right-action of the linear operator @a m on the covector @a v, i.e. out = v * m + */ +#define VECTOR_DOT_MATRIX_3D(v, m, out)\ + do {\ + (out)[X] = (v)[X] * (m)[X][X] + (v)[Y] * (m)[Y][X] + (v)[Z] * (m)[Z][X];\ + (out)[Y] = (v)[X] * (m)[X][Y] + (v)[Y] * (m)[Y][Y] + (v)[Z] * (m)[Z][Y];\ + (out)[Z] = (v)[X] * (m)[X][Z] + (v)[Y] * (m)[Y][Z] + (v)[Z] * (m)[Z][Z];\ + } while (0) + +/** + * @brief Multiplies two 2D matrixes. + * @param[in] m1 First 2D matrix. + * @param[in] m2 Second 2D matrix. + * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2 + */ +extern void matrix_2d_multiply_matrix_2d(const_Matrix_2d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out); + +/** + * @brief Multiplies submatrix of 3D matrix with 2D matrixes. + * @param[in] m1 3D matrix. For multiplication 2D matrix consisting of first 2 rows and first 2 columns is taken. + * @param[in] m2 2D matrix. + * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2 + */ +extern void matrix_3d_multiply_matrix_2d(const_Matrix_3d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out); + +/** + * @brief Multiplies 2D matrix with 2D vector. + * @param[in] m 2D matrix. + * @param[in] v 2D vector. + * @param[out] out Multiplication of provided matrix and vector, i.e. out = m * v + */ +extern void matrix_2d_multiply_vector_2d(const_Matrix_2d_ref m, const_Vector_2d_ref v, Vector_2d_ref out); + +/** + * @brief Multiplies 2D vector with 2D matrix. + * @param[in] v 2D vector. + * @param[in] m 2D matrix. + * @param[out] out Multiplication of provided vector and matrix, i.e. out = v * m + */ +extern void vector_2d_multiply_matrix_2d(const_Vector_2d_ref v, const_Matrix_2d_ref m, Vector_2d_ref out); + +/** + * @brief Determinant of 2D matrix. + * @param[in] m 2D matrix. + * @return Determinant of the matrix. + */ +extern double matrix_2d_det(const_Matrix_2d_ref self); + +/** + * @brief Inverts 2D matrix. + * @param[in] m 2D matrix. + * @param[out] out Inverted matrix. Not modified if result of a function is false (determinant is 0). + * @return + * - true - If inverted matrix was calculated. + * - false - If matrix can not be inverted (determinant is 0). + */ +extern bool matrix_2d_invert(const_Matrix_2d_ref m, Matrix_2d_ref out); + +#ifdef __cplusplus +} +#endif + +#endif /* __MATRIX_H_ */ diff --git a/lbs-server/src/fused/MotionDetector.c b/lbs-server/src/fused/MotionDetector.c new file mode 100644 index 0000000..d018ab4 --- /dev/null +++ b/lbs-server/src/fused/MotionDetector.c @@ -0,0 +1,108 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#include "MotionDetector.h" +#include "math-ext.h" +#include "debug_util.h" + +#define _USE_MATH_DEFINES +#include + +#if (FL_MOTION_DETECTOR) + +void motion_detector_init(MotionDetector* self, MotionDetectorStateChangeListener on_motion_state_changed) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(motion_cb=%p)"), on_motion_state_changed); + Aema1dEstimator_init(&self->moti_al2, 1 << FL_MOTI_AEMA_PLATEAU_BITS); + self->moti_immobile_time = 0; + self->moti_state = MOTI_UNDECIDED; + self->moti_last_notification = MOTI_UNDECIDED; + self->moti_on_motion_state_changed = on_motion_state_changed; +} + +void motion_detector_exit(MotionDetector* self) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()")); + self->moti_on_motion_state_changed = NULL; + self->moti_last_notification = MOTI_UNDECIDED; + self->moti_state = MOTI_UNDECIDED; + self->moti_immobile_time = 0; + Aema1dEstimator_exit(&self->moti_al2); +} + +void motion_detector_reset(MotionDetector* self) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()")); + self->moti_immobile_time = 0; + self->moti_state = MOTI_UNDECIDED; + self->moti_last_notification = MOTI_UNDECIDED; + Aema1dEstimator_reset(&self->moti_al2); +} + +/** + * @brief Change the state indicator and notify the user. + * @param[in] state New state value (does not have to be different than present one). + */ +static void motion_detector_set_state(MotionDetector* self, MotionDetectorState state) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%u->%u)"), self->moti_state, state); + self->moti_state = state; + if (self->moti_on_motion_state_changed && self->moti_last_notification != state) { + self->moti_on_motion_state_changed(state); + self->moti_last_notification = state; + } +} + +void motion_detector_process(MotionDetector* self, double t, const_Vector_3d_ref al) +{ + double al2 = Aema1dEstimator_process(&self->moti_al2, vector_3d_length2(al)); + if (al2 > SQUARE(FL_MOTI_MOTION_LEVEL)) { + switch (self->moti_state) { + case MOTI_UNDECIDED: + case MOTI_IMMOBILE: + case MOTI_SLEEP: + MotionDetector_set_state(self, MOTI_MOVING); + break; + + default: /* MOTI_MOVING */ + break; + } + } else if (al2 < SQUARE(FL_MOTI_NOISE_LEVEL)) { + switch (self->moti_state) { + case MOTI_UNDECIDED: + case MOTI_MOVING: + MotionDetector_set_state(self, MOTI_IMMOBILE); + self->moti_immobile_time = t; + break; + + case MOTI_IMMOBILE: + if (t - self->moti_immobile_time > FL_MOTI_IMMOBILITY_INTERVAL) + MotionDetector_set_state(self, MOTI_SLEEP); + break; + + default: + /* MOTI_SLEEP: */ + break; + } + } +} + +#endif diff --git a/lbs-server/src/fused/MotionDetector.h b/lbs-server/src/fused/MotionDetector.h new file mode 100644 index 0000000..a6990ef --- /dev/null +++ b/lbs-server/src/fused/MotionDetector.h @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file MotionDetector.h + * @brief Accelerometer-based detector of significant motion. + */ + +#pragma once +#ifndef __MOTION_DETECTOR_H__ +#define __MOTION_DETECTOR_H__ + +#include "MovingAverageFilters.h" +#include "Vector.h" +#include "parameters.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + MOTI_UNDECIDED, /* initial */ + MOTI_MOVING, /* acceleration above MOTION_LEVEL */ + MOTI_IMMOBILE, /* acceleration below NOISE_LEVEL */ + MOTI_SLEEP, /* immobile for IMMOBILITY_INTERVAL */ +} MotionDetectorState; + +/** + * @brief Callback invoked on every change in motion state. + * @param[in] state New motion state. + * @param[in] handler Arbitrary pointer to user data specified during the init() call. + */ +typedef void (*MotionDetectorStateChangeListener)(MotionDetectorState state); + +typedef struct { + MovingAverage1dFilter moti_al2; /**< Linear acceleration low-pass filter */ + double moti_immobile_time; /**< Start time of immobility */ + MotionDetectorState moti_state; /**< Current state */ + MotionDetectorState moti_last_notification; /**< Last notified state */ + MotionDetectorStateChangeListener moti_on_motion_state_changed; /**< User callback */ +} MotionDetector; + +#if (FL_MOTION_DETECTOR) + +/** + * @brief Initialization of the singleton unit (static constructor). + * @param[in] on_motion_state_changed Callback to be invoked when the detected state of motion changes. + * @param[in] handler Arbitrary user parameter passed to the callback. + */ +void motion_detector_init(MotionDetector* self, MotionDetectorStateChangeListener on_motion_state_changed); + +/** Cleanup of the singleton unit (static destructor) */ +void motion_detector_exit(MotionDetector* self); + +/** + * @brief Reset of the internal state back to initial one. This function is + * used for repetitive testing and module soft restarts. + */ +void motion_detector_reset(MotionDetector* self); + +/** + * @biref Processing of a single sample from the accelerometer. This function + * implments a hysteresis in the 2D space of linear acceleration modulus |al|^2 + * vs motion state: + * - Crossing the FL_MOTI_MOTION_LEVEL from below changes state to MOVING; + * - Crossing the FL_MOTI_NOISE_LEVEL from above changes state to IMMOBILE; + * - Staying IMMOBILE for longer than FL_MOTI_IMMOBILITY_INTERVAL changes state to SLEEP. + * + * @param[in] time Time of the event in seconds. + * @param[in] al 3D vector of linear acceleration. + */ +void motion_detector_process(MotionDetector* self, double time, const_Vector_3d_ref al); + +#else + #define motion_detector_init(self, on_motion_state_changed) + #define motion_detector_exit(self) + #define motion_detector_reset(self) + #define motion_detector_process(self, t, al) +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __MOTION_DETECTOR_H__ */ diff --git a/lbs-server/src/fused/MovingAverage.c b/lbs-server/src/fused/MovingAverage.c new file mode 100644 index 0000000..cbdb8ed --- /dev/null +++ b/lbs-server/src/fused/MovingAverage.c @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "MovingAverage.h" +#include "debug_util.h" + +#include + +void moving_average_init(MovingAverage* self, unsigned plateau_samples) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](bits=%u)"), self, plateau_samples); + self->plateau_samples = plateau_samples; + moving_average_reset(self); +} + +void moving_average_exit(MovingAverage *self) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self); +} + +void moving_average_reset(MovingAverage* self) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self); + self->samples = 0; + self->update_rate = 1; + self->decay_rate = 0; +} + +void moving_average_process(MovingAverage *self) +{ + if (self->samples < self->plateau_samples) { + self->samples++; + self->update_rate = 1.0 / self->samples; + self->decay_rate = 1.0 - self->update_rate; + } +} + +void moving_average_set_estimate(MovingAverage* self) +{ + self->samples = self->plateau_samples; + self->update_rate = 1.0 / self->plateau_samples; + self->decay_rate = 1.0 - self->update_rate; +} diff --git a/lbs-server/src/fused/MovingAverage.h b/lbs-server/src/fused/MovingAverage.h new file mode 100644 index 0000000..ab52fc4 --- /dev/null +++ b/lbs-server/src/fused/MovingAverage.h @@ -0,0 +1,75 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file MovingAverage.h + * @brief Adaptive Exponential Moving Average (1st order) filter + */ + +#pragma once +#ifndef __MOVING_AVERAGE_H__ +#define __MOVING_AVERAGE_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + unsigned plateau_samples; /* number of samples to collect before EMA kicks-in */ + unsigned samples; /* min(number of samples collected, plateau_samples) */ + double update_rate; /* (1 / samples) */ + double decay_rate; /* (1 - update_rate) */ +} MovingAverage; + +/** + * @brief Initialization of the object (class constructor). + * @param[in] self Object pointer + * @param[in] plateau_samples Number of samples to collect before switching to standard EMA mode. + */ +extern void moving_average_init(MovingAverage *self, unsigned plateau_samples); + +/** Cleanup of the base class (destructor) */ +extern void moving_average_exit(MovingAverage *self); + +/** + * @brief Reset the AEMA object back to initial state. + * This function is used for repetitive testing and module soft restarts. + */ +extern void moving_average_reset(MovingAverage *self); + +/** + * @brief Processing of a data sample. This is the common part of the + * derived processing methods, which implements the AEMA update/decay rate. + */ +extern void moving_average_process(MovingAverage *self); + +/** Bring the base filter to a stationary state (EMA mode). This method is used for testing */ +extern void moving_average_set_estimate(MovingAverage* self); + +/** (double) */ +#define moving_average_get_update_rate(self) ((self)->update_rate) + +/** (double) */ +#define moving_average_get_decay_rate(self) ((self)->decay_rate) + +/** void moving_average_filter(MovingAverage* self, double* f, double v) */ +#define moving_average_filter(self, f, v) ((f) = (self)->decay_rate * (f) + (self)->update_rate * (v)) + +#ifdef __cplusplus +} +#endif + +#endif /* __MOVING_AVERAGE_H__ */ diff --git a/lbs-server/src/fused/MovingAverageFilters.c b/lbs-server/src/fused/MovingAverageFilters.c new file mode 100644 index 0000000..a5d9df2 --- /dev/null +++ b/lbs-server/src/fused/MovingAverageFilters.c @@ -0,0 +1,127 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "MovingAverageFilters.h" +#include "debug_util.h" + +/* Labels for an axial (or cylindrical without phase) 2D space. */ +typedef enum { + AXIAL_H, + AXIAL_V, + AXIAL_DIMENSION +} fl_axial; + +void moving_average_1d_filter_init(MovingAverage1dFilter* self, unsigned plateau_samples) +{ + moving_average_init(&self->base, plateau_samples); + self->value = 0; +} + +void moving_average_1d_filter_exit(MovingAverage1dFilter *self) +{ + moving_average_exit(&self->base); +} + +void moving_average_1d_filter_reset(MovingAverage1dFilter* self) +{ + moving_average_reset(&self->base); + self->value = 0; +} + +double moving_average_1d_filter_process(MovingAverage1dFilter* self, double value) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value); + moving_average_process(&self->base); + moving_average_filter(&self->base, self->value, value); + return self->value; +} + +void moving_average_1d_filter_set_estimate(MovingAverage1dFilter* self, double value) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value); + moving_average_set_estimate(&self->base); + self->value = value; +} + +void moving_average_2d_filter_init(MovingAverage2dFilter* self, unsigned plateau_samples) +{ + moving_average_init(&self->base, plateau_samples); + self->value[AXIAL_H] = 0; + self->value[AXIAL_V] = 0; +} + +void moving_average_2d_filter_exit(MovingAverage2dFilter *self) +{ + moving_average_exit(&self->base); +} + +void moving_average_2d_filter_reset(MovingAverage2dFilter* self) +{ + moving_average_reset(&self->base); + self->value[PLANAR_X] = 0; + self->value[PLANAR_Y] = 0; +} + +const_Vector_2d_ref moving_average_2d_filter_process(MovingAverage2dFilter* self, double value_H, double value_V) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V); + moving_average_process(&self->base); + moving_average_filter(&self->base, self->value[AXIAL_H], value_H); + moving_average_filter(&self->base, self->value[AXIAL_V], value_V); + return self->value; +} + +void moving_average_2d_filter_set_estimate(MovingAverage2dFilter* self, double value_H, double value_V) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V); + moving_average_set_estimate(&self->base); + self->value[AXIAL_H] = value_H; + self->value[AXIAL_V] = value_V; +} + +void moving_average_3d_filter_init(MovingAverage3dFilter* self, unsigned plateau_samples) +{ + moving_average_init(&self->base, plateau_samples); + vector_3d_clear(self->value); +} + +void moving_average_3d_filter_exit(MovingAverage3dFilter *self) +{ + moving_average_exit(&self->base); +} + +void moving_average_3d_filter_reset(MovingAverage3dFilter* self) +{ + moving_average_reset(&self->base); + vector_3d_clear(self->value); +} + +const_Vector_3d_ref moving_average_3d_filter_process(MovingAverage3dFilter* self, const_Vector_3d_ref value) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[X], value[Y], value[Z]); + moving_average_process(&self->base); + moving_average_filter(&self->base, self->value[X], value[X]); + moving_average_filter(&self->base, self->value[Y], value[Y]); + moving_average_filter(&self->base, self->value[Z], value[Z]); + return self->value; +} + +void moving_average_3d_filter_set_estimate(MovingAverage3dFilter* self, const_Vector_3d_ref value) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[X], value[Y], value[Z]); + moving_average_set_estimate(&self->base); + vector_3d_copy(self->value, value); +} diff --git a/lbs-server/src/fused/MovingAverageFilters.h b/lbs-server/src/fused/MovingAverageFilters.h new file mode 100644 index 0000000..7794fe0 --- /dev/null +++ b/lbs-server/src/fused/MovingAverageFilters.h @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file MovingAverageFilters.h + * @brief Moving Average (1st order) filters + */ + +#pragma once +#ifndef __MOVING_AVERAGE_1D_FILTER_H__ +#define __MOVING_AVERAGE_1D_FILTER_H__ + +#include "MovingAverage.h" +#include "Vector.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +/** Moving Average (1st order) 1D filter */ +typedef struct { + MovingAverage base; /* base object */ + double value; /* filter output (estimated value) */ +} MovingAverage1dFilter; + +/** + * @brief Initialization of the 1D AEMA object (class constructor). + * @param[in] self Object pointer + * @param[in] plateau_samples Number of samples to collect before switching to standard EMA mode. + */ +void moving_average_1d_filter_init(MovingAverage1dFilter *self, unsigned plateau_samples); +void moving_average_1d_filter_exit(MovingAverage1dFilter *self); +void moving_average_1d_filter_reset(MovingAverage1dFilter *self); + +/** + * @brief Processing of a single data sample. + * @param[in] self Object pointer + * @param[in] value Input value to the filter. + * @return Output value of the filter after processing. + * This is the same as returned by fl_aema1d_get_estimate(self). + */ +double moving_average_1d_filter_process(MovingAverage1dFilter *self, double value); + +/** + * @brief [TEST] Bring the filter to a stationary state (EMA mode, input = output). + * @param[in] self Object pointer + * @param[in] value I/o value of the filter. + */ +void moving_average_1d_filter_set_estimate(MovingAverage1dFilter *self, double value); + +/** + * @brief Fetch the current filter output. + * @param[in] self Object pointer + * @return Output value after last processed sample. + */ +#define moving_average_1d_filter_get_estimate(self) ((self)->value) + + +/** Moving Average (1st order) 2D filter */ +typedef struct { + MovingAverage base; /* base object */ + Vector_2d value; /* filter output (estimated value) */ +} MovingAverage2dFilter; + +/** + * @brief Initialization of the 2D AEMA object (class constructor). + * @param[in] self Object pointer + * @param[in] plateau_samples Number of samples to collect before switching to standard EMA mode. + */ +void moving_average_2d_filter_init(MovingAverage2dFilter *self, unsigned plateau_samples); +void moving_average_2d_filter_exit(MovingAverage2dFilter *self); +void moving_average_2d_filter_reset(MovingAverage2dFilter *self); + +/** + * @brief Processing of a double data sample. + * @param[in] self Object pointer + * @param[in] value_H First (horizontal) input value to the filter. + * @param[in] value_V Second (vertical) input value to the filter. + * @return Output value of the filter after processing. + * This is the same as returned by fl_aema2d_get_estimate(self). + */ +const_Vector_2d_ref moving_average_2d_filter_process(MovingAverage2dFilter *self, double value_X, double value_Y); + +/** + * @brief [TEST] Bring the filter to a stationary state (EMA mode, input = output). + * @param[in] self Object pointer + * @param[in] value_H First (horizontal) i/o value of the filter. + * @param[in] value_V Second (vertical) i/o value of the filter. + */ +void moving_average_2d_filter_set_estimate(MovingAverage2dFilter *self, double value_X, double value_Y); + +/** + * @brief (const_Vector_2d_ref) Fetch the current filter output + * @param[in] self Object pointer + * @return Output 2D vector after last processed sample. + */ +#define moving_average_2d_filter_get_estimate(self) ((self)->value) + + +/** Moving Average (1st order) 3D filter */ +typedef struct { + MovingAverage base; /* base object */ + Vector_3d value; /* filter output (estimated value) */ +} MovingAverage3dFilter; + +/** + * @brief Initialization of the 3D AEMA object (class constructor). + * @param[in] self Object pointer + * @param[in] plateau_samples Number of samples to collect before switching to standard EMA mode. + */ +void moving_average_3d_filter_init(MovingAverage3dFilter *self, unsigned plateau_samples); +void moving_average_3d_filter_exit(MovingAverage3dFilter *self); +void moving_average_3d_filter_reset(MovingAverage3dFilter *self); + +/** + * @brief Processing of a triple data sample. + * @param[in] self Object pointer + * @param[in] value Input 3D vector value to the filter. + * @return Output value of the filter after processing. + * This is the same as returned by fl_aema3d_get_estimate(self). + */ +const_Vector_3d_ref moving_average_3d_filter_process(MovingAverage3dFilter *self, const_Vector_3d_ref value); + +/** + * @brief [TEST]Bring the filter to a stationary state (EMA mode, input = output). + * @param[in] self Object pointer + * @param[in] value I/o 3D vector value of the filter. + */ +void moving_average_3d_filter_set_estimate(MovingAverage3dFilter *self, const_Vector_3d_ref value); + +/** + * (const_Vector_3d_ref) Fetch the current filter output. + * @param[in] self Object pointer + * @return Output 3D vector after last processed sample. + */ +#define moving_average_3d_filter_get_estimate(self) ((self)->value) + +#ifdef __cplusplus +} +#endif + +#endif /* __MOVING_AVERAGE_1D_FILTER_H__ */ diff --git a/lbs-server/src/fused/Orientation.c b/lbs-server/src/fused/Orientation.c new file mode 100644 index 0000000..44a80c4 --- /dev/null +++ b/lbs-server/src/fused/Orientation.c @@ -0,0 +1,388 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "Orientation.h" +#include "Conversions.h" +#include "debug_util.h" +#include "parameters.h" + +#define DEFAULT_ROTATION_SIGMA2 SQUARE(1.0 / 256) /* [(rad/s)^2] */ +#define TIME_FORMAT "%.3fs" + +#if (FL_ENABLE_DEVEL_LOG) +static const Vector_3d gravity = {0, 0, 1}; +#endif + +void orientation_init(Orientation* self, + MotionDetectorStateChangeListener on_motion_state_changed, double rot_var) +{ + accelerometer_filter_init(&self->acc_filter, on_motion_state_changed); + rotation_filter_init(&self->rotation_filter, rot_var); + orientation_reset(self); +} + +void orientation_exit(Orientation* self) +{ + accelerometer_filter_exit(&self->acc_filter); +} + +void orientation_reset(Orientation* self) +{ + matrix_3d_clear(&self->rotation); + unsigned i; + for (i = GEO_DIMENSION; i;) { + --i; + self->rotation[i][i] = 1; + } + self->last_angular_velocity_time = FL_UNDEFINED_TIME; + self->weight2 = M_1_PI / 3; +#if (FL_KALMAN_ANGULAR_VELOCITY) + rotation_filter_reset(&self->rotation_filter); +#else + self->z_rot_speed = 0; +#endif +} + +bool orientation_new_angular_velocity(Orientation* self, double time, const_Vector_3d_ref angular_velocity, double s2wm) +{ + bool result = false; + if (KNOWN_TIME(self->last_angular_velocity_time)) { + const double dt = time - self->last_angular_velocity_time; + + if (dt > 0) { + Vector_3d mean_av; + vector_3d_linear_combination(0.5, self->last_angular_velocity, 0.5, angular_velocity, mean_av); + const double z_rot_speed = vector_3d_dot_product(self->rotation[Z], mean_av); + +#if (FL_KALMAN_ANGULAR_VELOCITY) + rotation_filter_new_rot_speed(&self->rotation_filter, time, z_rot_speed, SQUARE(FL_DEFAULT_SPIN_SIGMA)); +#else + self->z_rot_speed = z_rot_speed; +#endif + result = true; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, dt=%.16e, wz=%.16e, wze=%.16e, m0=%.16e, m1=%.16e, m2=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"), + time, dt, z_rot_speed, orientation_get_z_rot_speed(self), + mean_av[0], mean_av[1], mean_av[2], self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]); + + if (vector_3d_length(mean_av) > 0) { + /* Integrate over dt: We use the fact that for any scalar t the quaternions commute. + * [exp(w), exp(tw)] = 0 + */ + Quaternion qwt; + quaternion_from_rotation_vector(qwt, mean_av, dt); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e"), + time, qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z]); + + Vector_3d v; + vector_3d_cross_vector_3d(self->last_angular_velocity, angular_velocity, v); + const double dt2 = dt * dt / 24; + qwt[QTR_X] += dt2 * v[X]; + qwt[QTR_Y] += dt2 * v[Y]; + qwt[QTR_Z] += dt2 * v[Z]; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, v0=%.16e, v1=%.16e, v2=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e, ql=%.16e"), + time, v[0], v[1], v[2], qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z], quaternion_norm(qwt)); + + quaternion_normalize(qwt); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e"), + time, qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z]); + + Matrix_3d dsr; + quaternion_to_rotation_matrix(qwt, dsr); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, r00=%.16e, r01=%.16e, r02=%.16e, r10=%.16e, r11=%.16e, r12=%.16e, r20=%.16e, r21=%.16e, r22=%.16e"), + time, dsr[0][0], dsr[0][1], dsr[0][2], dsr[1][0], dsr[1][1], dsr[1][2], dsr[2][0], dsr[2][1], dsr[2][2]); + + Matrix_3d new_rot; + matrix_3d_multiply_matrix_3d(self->rotation, dsr, new_rot); + matrix_3d_copy(self->rotation, new_rot); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"), time, self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]); + } + } + } + vector_3d_copy(self->last_angular_velocity, angular_velocity); + self->last_angular_velocity_time = time; + return result; +} + +/** + * @brief Extrapolate the system rotation (__sp) to the given time assuming constant angular velocity. + * @param[in] t Internal time + */ +static void orientation_predict_rw(Orientation* self, double time) +{ + if (KNOWN_TIME(self->last_angular_velocity_time)) { + const double dt = time - self->last_angular_velocity_time; + + if (dt > 0) { + const double w = vector_3d_length(self->last_angular_velocity); + + if (w > 0) { + Quaternion qwt; + quaternion_from_rotation_vector(qwt, self->last_angular_velocity, dt); + Matrix_3d dsr; + quaternion_to_rotation_matrix(qwt, dsr); + Matrix_3d new_rot; + matrix_3d_multiply_matrix_3d(self->rotation, dsr, new_rot); + matrix_3d_copy(self->rotation, new_rot); + } + } + } +} + +void orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acc) +{ + orientation_predict_rw(self, time); + Vector_3d gacc; /* Acceleration in global (tangent) frame */ + matrix_3d_multiply_vector_3d(self->rotation, acc, gacc); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.16e, acc0=%.16e, acc1=%.16e, acc2=%.16e, gacc0=%.16e, gacc1=%.16e, gacc2=%.16e, angle=%.16e"), + time, acc[0], acc[1], acc[2], gacc[0], gacc[1], gacc[2], vector_3d_angle(gacc, gravity)); + Vector_3d gu; + vector_3d_copy(gu, gacc); + +#if (FL_ENABLE_DEVEL_LOG) + Vector_3d gn; /* Gravity in device frame */ + /* rotate back to device frame */ + VECTOR_DOT_MATRIX_3D(gu, self->rotation, gn); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.16e, gn0=%.16e, gn1=%.16e, gn2=%.16e"), + time, gn[0], gn[1], gn[2]); +#endif + /* update rotation matrix */ + Matrix_3d new_rot; + double accLen = vector_3d_length(gu); + if (accLen > 0) { + Quaternion q; + q[QTR_0] = accLen + gu[Z]; + q[QTR_X] = gu[Y]; + q[QTR_Y] = -gu[X]; + q[QTR_Z] = 0; + quaternion_normalize(q); + Vector_3d rot_vect; + quaternion_get_rotation_vector(q, rot_vect); + quaternion_from_rotation_vector(q, rot_vect, 0.01); + Matrix_3d m; + quaternion_to_rotation_matrix(q, m); + matrix_3d_transpose(m); + matrix_3d_multiply_matrix_3d(m, self->rotation, new_rot); + matrix_3d_copy(self->rotation, new_rot); + } + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), time=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"), + time, self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]); +} + +void orientation_get_orientation(Orientation* self, double* o) +{ + Quaternion q; + quaternion_from_rotation_matrix(q, self->rotation); + if (q[QTR_0] < 0) { + o[0] = q[QTR_X]; + o[1] = q[QTR_Y]; + o[2] = q[QTR_Z]; + } else { + o[0] = -q[QTR_X]; + o[1] = -q[QTR_Y]; + o[2] = -q[QTR_Z]; + } +} + +void quaternion_normalize(Quaternion_ref self) +{ + double qwtn = quaternion_norm(self); + if (qwtn > 0) { + double norm_qwt = 1.0 / qwtn; + + self[QTR_0] *= norm_qwt; + self[QTR_X] *= norm_qwt; + self[QTR_Y] *= norm_qwt; + self[QTR_Z] *= norm_qwt; + } +} + +double quaternion_get_rotation_angle(const_Quaternion_ref self) +{ + return 2 * acos(fabs(self[QTR_0])); +} + +void quaternion_from_rotation_vector(Quaternion_ref self, const_Vector_3d_ref rot_vector, double scale) +{ + double w = vector_3d_length(rot_vector); + if (w > 0) { + double wt = w * scale * 0.5; + double swt = sin(wt) / w; + self[QTR_0] = cos(wt); + self[QTR_X] = swt * rot_vector[X]; + self[QTR_Y] = swt * rot_vector[Y]; + self[QTR_Z] = swt * rot_vector[Z]; + } else { + self[QTR_0] = 1; + self[QTR_X] = 0; + self[QTR_Y] = 0; + self[QTR_Z] = 0; + } +} + +void quaternion_get_rotation_vector(const_Quaternion_ref self, Vector_3d_ref rot) +{ + double length = sqrt(SQUARE(self[QTR_X]) + SQUARE(self[QTR_Y]) + SQUARE(self[QTR_Z])); + + if (length == 0) { + vector_3d_clear(rot); + } else { + double l = self[QTR_0] >= 0 + ? quaternion_get_rotation_angle(self) / length + : -quaternion_get_rotation_angle(self) / length; + + vector_3d_set(rot, self[QTR_X] * l, self[QTR_Y] * l, self[QTR_Z] * l); + } +} + +void quaternion_to_rotation_matrix(const_Quaternion_ref self, Matrix_3d_ref rot_matrix) +{ + double xx = 2 * self[QTR_X] * self[QTR_X]; + double xy = 2 * self[QTR_X] * self[QTR_Y]; + double xz = 2 * self[QTR_X] * self[QTR_Z]; + double xw = 2 * self[QTR_X] * self[QTR_0]; + double yy = 2 * self[QTR_Y] * self[QTR_Y]; + double yz = 2 * self[QTR_Y] * self[QTR_Z]; + double yw = 2 * self[QTR_Y] * self[QTR_0]; + double zz = 2 * self[QTR_Z] * self[QTR_Z]; + double zw = 2 * self[QTR_Z] * self[QTR_0]; + rot_matrix[X][X] = 1 - yy - zz; + rot_matrix[X][Y] = xy + zw; + rot_matrix[X][Z] = xz - yw; + + rot_matrix[Y][X] = xy - zw; + rot_matrix[Y][Y] = 1 - xx - zz; + rot_matrix[Y][Z] = yz + xw; + + rot_matrix[Z][X] = xz + yw; + rot_matrix[Z][Y] = yz - xw; + rot_matrix[Z][Z] = 1 - xx - yy; +} + +void quaternion_from_rotation_matrix(Quaternion_ref self, const_Matrix_3d_ref m) +{ + double qw; + double qx; + double qy; + double qz; + double tr = m[0][0] + m[1][1] + m[2][2]; + + if (tr > 0) { + double s = sqrt(tr + 1.0) * 2; /* s = 4 * qw */ + qw = 0.25 * s; + qx = (m[2][1] - m[1][2]) / s; + qy = (m[0][2] - m[2][0]) / s; + qz = (m[1][0] - m[0][1]) / s; + } else if ((m[0][0] > m[1][1]) & (m[0][0] > m[2][2])) { + double s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2; /* s = 4 * qx */ + qw = (m[2][1] - m[1][2]) / s; + qx = 0.25 * s; + qy = (m[0][1] + m[1][0]) / s; + qz = (m[0][2] + m[2][0]) / s; + } else if (m[1][1] > m[2][2]) { + double s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2; /* s = 4 * qy */ + qw = (m[0][2] - m[2][0]) / s; + qx = (m[0][1] + m[1][0]) / s; + qy = 0.25 * s; + qz = (m[1][2] + m[2][1]) / s; + } else { + double s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2; /* s = 4 * qz */ + qw = (m[1][0] - m[0][1]) / s; + qx = (m[0][2] + m[2][0]) / s; + qy = (m[1][2] + m[2][1]) / s; + qz = 0.25 * s; + } + + double n = sqrt(qw * qw + qx * qx + qy * qy + qz * qz); + if (qw >= 0) { + self[QTR_0] = qw / n; + self[QTR_X] = -qx / n; + self[QTR_Y] = -qy / n; + self[QTR_Z] = -qz / n; + } else { + self[QTR_0] = -qw / n; + self[QTR_X] = qx / n; + self[QTR_Y] = qy / n; + self[QTR_Z] = qz / n; + } +} + +void rotation_filter_init(RotationFilter* self, double rot_var) +{ + self->def_rot_var = rot_var; + rotation_filter_reset(self); +} + +void rotation_filter_reset(RotationFilter* self) +{ + self->rot_speed = 0; + self->rot_speed_variance = self->def_rot_var; + self->rot_speed_time = FL_UNDEFINED_TIME; + self->last_azimuth = 0; + self->last_azimuth_time = FL_UNDEFINED_TIME; +} + +void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot_speed, double rot_speed_variance) +{ + if (KNOWN_TIME(self->rot_speed_time)) { + const double dt = time - self->rot_speed_time; + + /* propagate/diffuse sigma^2 */ + const double s2wzp = self->rot_speed_variance + dt * SQUARE(FL_DEFAULT_SPIN_SIGMA); + const double s2wzk = s2wzp + rot_speed_variance; + if (s2wzk > 0) { + const double inv_s2k = 1.0 / s2wzk; + LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=" TIME_FORMAT ", w=%.4g, s2w=%.4g): __wze:%g->%g, __s2wze:%g->%g"), + time, rot_speed, rot_speed_variance, self->rot_speed, + inv_s2k * (s2wzp * rot_speed + rot_speed_variance * self->rot_speed), + self->rot_speed_variance, inv_s2k * s2wzp * rot_speed_variance * 2); + + self->rot_speed = inv_s2k * (s2wzp * rot_speed + rot_speed_variance * self->rot_speed); + self->rot_speed_variance = inv_s2k * s2wzp * rot_speed_variance * 2; + } else { + self->rot_speed = rot_speed; + self->rot_speed_variance = rot_speed_variance; + } + } else { + self->rot_speed = rot_speed; + self->rot_speed_variance = rot_speed_variance; + } + + self->rot_speed_time = time; +} + +void rotation_filter_new_azimuth(RotationFilter* self, double time, double azimuth, double vhe, double speed) +{ + if (speed > 0 && KNOWN_TIME(self->last_azimuth_time)) { + const double dt = time - self->last_azimuth_time; + + if (speed * vhe * dt > 0) { + /* circular difference */ + double db = azimuth - self->last_azimuth; + if (db > M_PI) { + db -= M_2PI; + } else if (db < -M_PI) { + db += M_2PI; + } + const double rot_speed = db / dt; + const double rot_speed_variance = 4 * fl_square(atan2(2 * sqrt(speed * vhe), FL_DEFAULT_VELOCITY_SIGMA)); + rotation_filter_new_rot_speed(self, time, rot_speed, rot_speed_variance); + } + } + + self->last_azimuth = azimuth; + self->last_azimuth_time = time; +} diff --git a/lbs-server/src/fused/Orientation.h b/lbs-server/src/fused/Orientation.h new file mode 100644 index 0000000..5d33972 --- /dev/null +++ b/lbs-server/src/fused/Orientation.h @@ -0,0 +1,177 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file Orientation.h + * @brief Orientation filter. + */ + +#pragma once +#ifndef __ORIENTATION_H_ +#define __ORIENTATION_H_ + +#include +#include "Vector.h" +#include "Matrix.h" +#include "math-ext.h" +#include "AccelerometerFilter.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef struct { + double def_rot_var; /* Initial variance of z rotation speed. */ + double rot_speed; /* Angular z rotation speed (horizontal plane). */ + double rot_speed_variance; /* Z rotation speed variance. */ + double rot_speed_time; /* Time of last z rotation speed update. */ + double last_azimuth; /* Last reported azimuth (North bearing). */ + double last_azimuth_time; /* Time of last azimuth update. */ +} RotationFilter; + +typedef struct { + Matrix_3d rotation; /* Current rotation matrix */ + double weight2; /* Square weight (inverse covariance) vector of matrix rows */ + Vector_3d last_angular_velocity; /* Last reported angular velocity. */ + double last_angular_velocity_time; /* Time when last angular velocity was reported. */ + AccelerometerFilter acc_filter; /* Accelerometer filter. */ +#if (FL_KALMAN_ANGULAR_VELOCITY) + RotationFilter rotation_filter; /* Filters z-rotation speed. */ +#else + double z_rot_speed; /* Angular z rotation speed (horizontal plane). */ +#endif +} Orientation; + +extern void orientation_init(Orientation* self, + MotionDetectorStateChangeListener on_motion_state_changed, double var_rot); + +extern void orientation_exit(Orientation* self); +extern void orientation_reset(Orientation* self); + +#if (FL_KALMAN_ANGULAR_VELOCITY) + +/** (double) */ +#define orientation_get_z_rot_speed(self) rotation_filter_get_rot_speed(&(self)->rotation_filter) + +#define orientation_new_azimuth(self, time, azimuth, vhe, speed) rotation_filter_new_azimuth(&(self)->rotation_filter, time, azimuth, vhe, speed) + +#else + +/** (double) */ +#define orientation_get_z_rot_speed(self) ((self)->z_rot_speed) + +#endif + +/** + * @brief Update system rotation matrix given a measurement of the device angular velocity. + * @param[in] t Internal time of the measurement + * @param[in] wm 3D vector of angular velocity [rad/s] in device coordinate frame + * @param[in] s2wm Estimated variance of the measuremnt + */ +extern bool orientation_new_angular_velocity(Orientation* self, + double time, const_Vector_3d_ref angular_velocity, double s2wm); + +extern void orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acceleration); + +extern void orientation_get_orientation(Orientation* self, double* o); + +/** Labels for quaternion components. */ +typedef enum { + QTR_X, + QTR_Y, + QTR_Z, + QTR_0, + QUATERNION_DIMENSION +} _quaternion_component; + +typedef double Quaternion[QUATERNION_DIMENSION]; +typedef double* Quaternion_ref; +typedef const double* const_Quaternion_ref; + +/** + * @brief Returns quaternion norm. + * @param[in] self Input quaternion. + * @return (double) Norm of quaternion. + */ +#define quaternion_norm(self) sqrt(SQUARE(self[QTR_0]) + SQUARE(self[QTR_X]) + SQUARE(self[QTR_Y]) + SQUARE(self[QTR_Z])) + +/** Normalizes quaternion */ +extern void quaternion_normalize(Quaternion_ref self); + +/** + * @brief Returns rotation angle assuming quaternion represents rotation. + * @param[in] self Input quaternion. Must be normed. + */ +extern double quaternion_get_rotation_angle(const_Quaternion_ref self); + +/** + * @brief Calculates quaternion representing rotation multiplied by some number. + * @param[out] self Output quaternion. + * @param[in] rot_vector Rotation vector. + * @param[in] scale Scale to multiply rotation vector. + */ +extern void quaternion_from_rotation_vector(Quaternion_ref self, const_Vector_3d_ref rot_vector, double scale); + +/** + * @brief Returns rotation vector assuming quaternion represents rotation. + * @param[in] self Input quaternion. Must be normed. + * @param[out] rot Rotation vector representing same rotation as input quaternion. + */ +extern void quaternion_get_rotation_vector(const_Quaternion_ref self, Vector_3d_ref rot); + +/** + * @brief Maps a quaternion into system rotation matrix assuming quaternion represents rotation. + * @param[in] self Input quaternion. Must be normed. + * @param[out] rot_matrix Corresponding system rotation 3D matrix + */ +extern void quaternion_to_rotation_matrix(const_Quaternion_ref self, Matrix_3d_ref rot_matrix); + +/** + * @brief Creates quaternion representing rotation from given rotation matrix. + * @param m 3D rotation matrix. + * @return Normed quaternion representing rotation with nonnegative scalar part. + */ +extern void quaternion_from_rotation_matrix(Quaternion_ref self, const_Matrix_3d_ref m); + +/** + * @brief Initialization of the filter (constructor). + * @param[in] self Object pointer + * @param[in] rot_var Default rotation variance. + */ +extern void rotation_filter_init(RotationFilter* self, double rot_var); + +/** Resets filter to initial state */ +extern void rotation_filter_reset(RotationFilter* self); + +/** + * @brief Axial (1D) Kalman blending of angular velocity along one axis (turn) + * @param[in] time Internal time + * @param[in] rot_speed Angular velocity projection on the axis. + * @param[in] rot_speed_variance Corresponding variance. + */ +extern void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot_speed, double rot_speed_variance); + +extern void rotation_filter_new_azimuth(RotationFilter* self, double time, double azimuth, double vhe, double speed); + +/** (double) Returns current rotation speed along axis */ +#define rotation_filter_get_rot_speed(self) ((self)->rot_speed) + +#ifdef __cplusplus +} +#endif + +#endif /* __ORIENTATION_H_ */ diff --git a/lbs-server/src/fused/PeaceDetector.c b/lbs-server/src/fused/PeaceDetector.c new file mode 100644 index 0000000..a0c2c96 --- /dev/null +++ b/lbs-server/src/fused/PeaceDetector.c @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "PeaceDetector.h" +#include "Conversions.h" + +void peace_detector_init(PeaceDetector *self, double stable_level, long stable_number) +{ + self->stable_level = stable_level; + self->stable_number = stable_number; + peace_detector_reset(self); +} + +void peace_detector_reset(PeaceDetector *self) +{ + self->stable_counter = self->stable_number; + self->last_timestamp = FL_UNDEFINED_TIME; +} + +bool peace_detector_is_stable(PeaceDetector *self) +{ + return self->stable_counter <= 0; +} + +bool peace_detector_new_data(PeaceDetector *self, double timestamp, const_Vector_3d_ref data) +{ + if ((!KNOWN_TIME(self->last_timestamp)) || self->last_timestamp > timestamp) { + self->stable_counter = self->stable_number; + } else { + const double length = vector_3d_distance(self->previous_measurement, data); + + if (length > self->stable_level) { + self->stable_counter = self->stable_counter < 0 ? 0 : self->stable_number; + } else { + self->stable_counter = self->stable_counter > 1 ? self->stable_counter - 1 : -1; + } + } + self->last_timestamp = timestamp; + if (!peace_detector_is_stable(self)) { + vector_3d_copy(self->previous_measurement, data); + return false; + } + return true; +} diff --git a/lbs-server/src/fused/PeaceDetector.h b/lbs-server/src/fused/PeaceDetector.h new file mode 100644 index 0000000..92cf06f --- /dev/null +++ b/lbs-server/src/fused/PeaceDetector.h @@ -0,0 +1,38 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once +#ifndef __PEACEDETECTOR_H_ +#define __PEACEDETECTOR_H_ + +#include "Vector.h" +#include + + +typedef struct { + double stable_level; /* Noise level below which the data are treated as stable. */ + long stable_number; /* Number of stable measurements to treat data as stable. */ + long stable_counter; /* Down counter of stable data. < 0 means stable state. */ + Vector_3d previous_measurement; /* Previous measurement. */ + double last_timestamp; /* Timestamp of last measurement. */ +} PeaceDetector; + +extern void peace_detector_init(PeaceDetector *self, double stable_level, long stable_number); +extern void peace_detector_reset(PeaceDetector *self); +extern bool peace_detector_is_stable(PeaceDetector *self); +extern bool peace_detector_new_data(PeaceDetector *self, double timestamp, const_Vector_3d_ref data); + +#endif /* __PEACEDETECTOR_H_ */ diff --git a/lbs-server/src/fused/TangentFrame.c b/lbs-server/src/fused/TangentFrame.c new file mode 100644 index 0000000..c042897 --- /dev/null +++ b/lbs-server/src/fused/TangentFrame.c @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "TangentFrame.h" +#include "Conversions.h" + +#include + +#define globalToTangent(tf, g, t) matrix_3d_multiply_vector_3d((tf)->fsr, g, t) +#define tangentToGlobal(tf, t, g) vector_3d_multiply_matrix_3d(t, (tf)->fsr, g) + +void earth_global_to_spherical(const_Vector_3d_ref p, double* latitude, double* longitude, double* altitude) +{ + const double yz2 = fl_square(p[Y]) + fl_square(p[Z]); + const double ry = atan2(p[Y], p[Z]); + *longitude = conversions_radians_to_longitude(ry); + + const double xyz2 = yz2 + fl_square(p[X]); + const double rx = atan2(p[X], sqrt(yz2)); + *latitude = conversions_radians_to_latitude(rx); + + *altitude = sqrt(xyz2) - EARTH_RADIUS; +} + +void earth_spherical_to_global(double latitude, double longitude, double radius, Vector_3d_ref p) +{ + const double rx = conversions_degrees_to_radians(latitude); + const double ry = conversions_degrees_to_radians(longitude); + const double sin_rx = radius * sin(rx); + const double cos_rx = radius * cos(rx); + vector_3d_set(p, sin_rx, cos_rx * sin(ry), cos_rx * cos(ry)); +} + +void tangent_frame_init(TangentFrame* self) +{ + memset(self, 0, sizeof(*self)); + self->fsr[0][0] = 1; + self->fsr[1][1] = 1; + self->fsr[2][2] = 1; + self->cos_lat = 1; +} + +void tangent_frame_create(TangentFrame* self, Vector_3d_ref pos, Vector_3d_ref vel) +{ + Vector_3d p; + tangentToGlobal(self, pos, p); + Vector_3d v; + tangentToGlobal(self, vel, v); + + matrix_3d_from_vector_3d(p, self->fsr, &self->lat, &self->lon, &self->sin_lat, &self->cos_lat); + + globalToTangent(self, p, pos); + globalToTangent(self, v, vel); +} + +/** + * @brief Returns 2D vector in tangent coordinate system pointing from given 2D point to the North direction. + * @param[in] self Tangent coordinate system. + * @param[in] p 2D point coordinates in tangent coordinate system. + * @param[out] u 2D vector pointing to the North in tangent coordinate system. + */ +static void tangent_frame_to_north(const TangentFrame* self, const_Vector_2d_ref p, Vector_2d_ref u) +{ + u[PLANAR_X] = EARTH_RADIUS * self->cos_lat - p[PLANAR_X] * self->sin_lat; + u[PLANAR_Y] = - p[PLANAR_Y] * self->sin_lat; +} + +/** + * @brief Returns azimuth in tangent coordinate system from given 2D point. + * @param[in] self Tangent coordinate system. + * @param[in] p 2D point coordinates in tangent coordinate system. + * @return Azimuth from given 2D point, i.e angle in radians from North to the given point. + */ +static double tangent_frame_azimuth(const TangentFrame* self, const_Vector_2d_ref p) +{ + Vector_2d u; + tangent_frame_to_north(self, p, u); + return atan2(-u[PLANAR_Y], u[PLANAR_X]); +} + +void tangent_frame_tangent_to_spherical(const TangentFrame* self, const_Vector_3d_ref pos, + double* latitude, double* longitude, double* altitude) +{ + Vector_3d p; + tangentToGlobal(self, pos, p); + earth_global_to_spherical(p, latitude, longitude, altitude); +} + +void tangent_frame_spherical_to_tangent(const TangentFrame* self, + double latitude, double longitude, double radius, Vector_3d_ref pos) +{ + Vector_3d pg; + earth_spherical_to_global(latitude, longitude, radius, pg); + globalToTangent(self, pg, pos); +} + +void tangent_frame_spherical_to_tangent_speed(const TangentFrame* self, + const_Vector_3d_ref pos, double speed, double azimuth, double climb, Vector_3d_ref v) +{ + double rb = conversions_degrees_to_radians(azimuth) - tangent_frame_azimuth(self, pos); + vector_3d_set(v, speed * cos(rb), speed * sin(rb), climb); +} + +void tangent_frame_tangent_to_spherical_speed(const TangentFrame* self, + const_Vector_3d_ref pos, const_Vector_3d_ref vel, double* speed, double* direction, double* climb) +{ + Vector_2d to_north; + tangent_frame_to_north(self, pos, to_north); + *direction = conversions_radians_to_positive_degrees(-vector_2d_angle(vel, to_north)); + *speed = vector_2d_length(vel); + *climb = vel[Z]; +} diff --git a/lbs-server/src/fused/TangentFrame.h b/lbs-server/src/fused/TangentFrame.h new file mode 100644 index 0000000..8a1103d --- /dev/null +++ b/lbs-server/src/fused/TangentFrame.h @@ -0,0 +1,154 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file TangentFrame.h + * @brief Tangent frame. + */ + +#pragma once +#ifndef __TANGENTFRAME_H_ +#define __TANGENTFRAME_H_ + +#include "Matrix.h" +#include "Vector.h" +#include "math-ext.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Convert spherical coordinate in radians to arc-length along the Earth's great circle in meters. + * @param[in] radians Angle spanned by the great arc. + * @return (double) Corresponding value in meters along Earth's surface. + */ +#define earth_radians_to_meters(radians) ((radians) * EARTH_RADIUS) + +/** + * @brief Convert arc-length in meters along the Earth's great circle to arc angle in radians. + * @param[in] meters Arc length in meters. + * @return (double) Corresponding angle in radians. + */ +#define earth_meters_to_radians(meters) ((meters) * (1.0 / EARTH_RADIUS)); + +/** + * @brief Convert point in global coordinates to spherical coordinates. + * @param[in] p Represents coordinates of a point in global (Earth) coordinate system. + * @param[out] latitude Latitude of provided point. + * @param[out] longitude Longitude of provided point. + * @param[out] altitude Altitude of provided point. + */ +extern void earth_global_to_spherical(const_Vector_3d_ref p, double* latitude, double* longitude, double* altitude); + +/** + * @brief Convert point in spherical coordinates to global coordinates. + * @param[in] latitude Latitude. + * @param[in] longitude Longitude. + * @param[in] altitude Altitude. + * @param[out] p Represents coordinates of a point in global (Earth) coordinate system. + */ +extern void earth_spherical_to_global(double latitude, double longitude, double altitude, Vector_3d_ref p); + + + +/** + * Global frame coordinate system: + * - Center in the middle of the Earth. + * - OX directed to the North Pole (latitude = 90). + * - OY directed to the latitude = 0 and longitude = 90; + * - OZ directed to the latitude = 0 and longitude = 0; + */ + +/** + * Tangent frame coordinate system: + * - OX directed to the North. + * - OY directed to the East; + * - OZ directed to the up; + */ + +/** Tangent space T(p)E to the embedding manifold E=SxR at point p */ +typedef struct { + double lat; /* latitude */ + double lon; /* longitude */ + double sin_lat; /* sin(latitude) */ + double cos_lat; /* cos(latitude) */ + Matrix_3d fsr; /* push-forward (tangent to global) coordinate system rotation matrix */ +} TangentFrame; + +extern void tangent_frame_init(TangentFrame* self); + +/** + * @brief Create coordinate map from spherical M = S^2 x R to the tangent space TM. + * Notice that only two angles are being used, what means the course on the tangent plane + * is not straight North (x-axis), but still described by the bearing. + * @param[in/out] self Tangent coordinate system. + * @param[in/out] pos Position in old tangent frame + * @param[in/out] vel Velocity in old tangent frame + */ +extern void tangent_frame_create(TangentFrame* self, Vector_2d_ref pos, Vector_2d_ref vel); + +/** + * @brief Converts coordinates in tangent coordinate system to spherical coordinate system. + * @param[in] self Tangent coordinate system. + * @param[in] pos Coordinates of given point in tangent coordinate system. + * @param[out] latitude Latitude in degrees. + * @param[out] longitude Longitude in degrees. + * @param[out] altitude Altitude in meters. + */ +extern void tangent_frame_tangent_to_spherical(const TangentFrame* self, + const_Vector_3d_ref pos, double* latitude, double* longitude, double* altitude); + +/** + * @brief Converts coordinates in spherical coordinate system to tangent coordinate system. + * @param[in] self Tangent coordinate system. + * @param[in] latitude Latitude in degrees. + * @param[in] longitude Longitude in degrees. + * @param[in] radius Distance from the center of the Earth (altitude + Earth radius) in meters. + * @param[out] pos Coordinates of given point in tangent coordinate system. + */ +extern void tangent_frame_spherical_to_tangent(const TangentFrame* self, + double latitude, double longitude, double radius, Vector_3d_ref pos); + +/** + * @brief Converts speed in spherical coordinate system to tangent coordinate system. + * @param[in] self Tangent coordinate system. + * @param[in] pos Coordinates of location in tangent coordinate system. + * @param[in] speed Horizontal speed in m/s. + * @param[in] azimuth Bearing (angle between true North and speed direction clockwise) in radians. + * @param[in] climb Vertical speed in m/s (positive up). + * @param[out] v Speed in given location in tangent coordinate system. + */ +extern void tangent_frame_spherical_to_tangent_speed(const TangentFrame* self, + const_Vector_3d_ref pos, double speed, double azimuth, double climb, Vector_3d_ref v); + +/** + * @brief Converts speed in tangent coordinate system to spherical coordinate system. + * @param[in] self Tangent coordinate system. + * @param[in] pos Coordinates of location in tangent coordinate system. + * @param[in] vel Speed in given location in tangent coordinate system. + * @param[out] speed Horizontal speed in m/s. + * @param[out] azimuth Bearing (angle between true North and speed direction clockwise) in radians. + * @param[out] climb Vertical speed in m/s (positive up). + */ +extern void tangent_frame_tangent_to_spherical_speed(const TangentFrame* self, + const_Vector_3d_ref pos, const_Vector_3d_ref vel, double* speed, double* azimuth, double* climb); + +#ifdef __cplusplus +} +#endif + +#endif /* __TANGENTFRAME_H_ */ diff --git a/lbs-server/src/fused/TimeOffset.c b/lbs-server/src/fused/TimeOffset.c new file mode 100644 index 0000000..87af4e3 --- /dev/null +++ b/lbs-server/src/fused/TimeOffset.c @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "TimeOffset.h" +#include "Conversions.h" +#include "parameters.h" + +void time_offset_init(TimeOffset* self) +{ + moving_average_1d_filter_init(&self->filter, 1 << FL_TIMO_AEMA_PLATEAU_BITS); + time_offset_reset(self); +} + +void time_offset_reset(TimeOffset* self) +{ + moving_average_1d_filter_reset(&self->filter); + self->first_reference_time = FL_UNDEFINED_TIME; + self->last_reference_time = FL_UNDEFINED_TIME; + self->reference_count = 0; + self->correct_time = FL_UNDEFINED_TIME; +} + +void time_offset_exit(TimeOffset* self) +{ + moving_average_1d_filter_exit(&self->filter); +} + +static void time_offset_new(TimeOffset* self, double dt) +{ + moving_average_1d_filter_process(&self->filter, dt); +} + +void time_offset_reference_time(TimeOffset* self, double time) +{ + if (self->reference_count == 0) { + self->first_reference_time = time; + } + self->last_reference_time = time; + self->reference_count++; + + if (KNOWN_TIME(self->correct_time)) { + const double dt = self->last_reference_time - (self->correct_time - time_offset_get(self)); + if (dt < 0) { + time_offset_new(self, self->correct_time - self->last_reference_time); + } + } +} + +double time_offset_correct_time(TimeOffset* self, double time) +{ + self->correct_time = time; + if (KNOWN_TIME(self->last_reference_time)) { + double dt = self->correct_time - time_offset_get(self) + - self->last_reference_time; + if (dt < 0) { + time_offset_new(self, self->correct_time - self->last_reference_time); + dt = 0; + } else if (self->reference_count > 1) { + const double mean_sensor_dt = (self->last_reference_time - self->first_reference_time) / (self->reference_count - 1); + if (dt > mean_sensor_dt) { + time_offset_new(self, self->correct_time - self->last_reference_time - mean_sensor_dt); + dt = mean_sensor_dt; + } + } + return self->last_reference_time + dt; + } + + return self->correct_time; +} + +double time_offset_get(const TimeOffset* self) +{ + return moving_average_1d_filter_get_estimate(&self->filter); +} diff --git a/lbs-server/src/fused/TimeOffset.h b/lbs-server/src/fused/TimeOffset.h new file mode 100644 index 0000000..4f74491 --- /dev/null +++ b/lbs-server/src/fused/TimeOffset.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file TimeOffset.h + * @brief Time offset. + */ + +#pragma once +#ifndef __TIMEOFFSET_H_ +#define __TIMEOFFSET_H_ + +#include "MovingAverageFilters.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef struct { + MovingAverage1dFilter filter; + double first_reference_time; /* Arrival time of the first sensor event. */ + double last_reference_time; /* Arrival time of the last sensor event. */ + unsigned reference_count; /* At 100Hz rate of arrival this will suffice for ~32 years. */ + double correct_time; /* Arrival time of the last position measurement event. */ +} TimeOffset; + +extern void time_offset_init(TimeOffset* self); +extern void time_offset_reset(TimeOffset* self); +extern void time_offset_exit(TimeOffset* self); + +/** + * @brief Supplies reference time + * @param[in] time Reference time in [s]. + */ +extern void time_offset_reference_time(TimeOffset* self, double time); + +/* + * @brief Convert location timestamp to internal time in seconds. + * The time-offset is adjusted if necessary to ensure temporal ordering of events. + * @param[in] time Location time + * @return Internal time. + */ +extern double time_offset_correct_time(TimeOffset* self, double time); + +extern double time_offset_get(const TimeOffset* self); + +#ifdef __cplusplus +} +#endif + +#endif /* __TIMEOFFSET_H_ */ diff --git a/lbs-server/src/fused/Vector.c b/lbs-server/src/fused/Vector.c new file mode 100644 index 0000000..77afc60 --- /dev/null +++ b/lbs-server/src/fused/Vector.c @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "Vector.h" + +void vector_3d_normalization(Vector_3d_ref self) +{ + double len = vector_3d_length(self); + if (len > 0) { + double out = 1.0 / len; + self[X] *= out; + self[Y] *= out; + self[Z] *= out; + } +} + +double vector_3d_distance(const_Vector_3d_ref p, const_Vector_3d_ref q) +{ + return sqrt(fl_square(p[X] - q[X]) + fl_square(p[Y] - q[Y]) + fl_square(p[Z] - q[Z])); +} + +void vector_3d_linear_combination(double a1, const_Vector_3d_ref v1, + double a2, const_Vector_3d_ref v2, Vector_3d_ref v) +{ + v[X] = a1 * v1[X] + a2 * v2[X]; + v[Y] = a1 * v1[Y] + a2 * v2[Y]; + v[Z] = a1 * v1[Z] + a2 * v2[Z]; +} + +double vector_3d_angle(const_Vector_3d_ref v1, const_Vector_3d_ref v2) +{ + Vector_3d v; + vector_3d_cross_vector_3d(v1, v2, v); + return atan2(vector_3d_length(v), vector_3d_dot_product(v1, v2)); +} diff --git a/lbs-server/src/fused/Vector.h b/lbs-server/src/fused/Vector.h new file mode 100644 index 0000000..b375f8e --- /dev/null +++ b/lbs-server/src/fused/Vector.h @@ -0,0 +1,201 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file Vector.h + * @brief vector. + */ + +#pragma once +#ifndef __VECTOR_H_ +#define __VECTOR_H_ + +#include "math-ext.h" + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** Labels for 3D vectors indexes. */ +typedef enum { + X, + Y, + Z, + GEO_DIMENSION +} fl_geo_spatial; + +typedef double Vector_3d[GEO_DIMENSION]; +typedef double* Vector_3d_ref; +typedef const double* const_Vector_3d_ref; + + +#define vector_3d_clear(self) memset(self, 0, sizeof(Vector_3d)) +#define vector_3d_clear_array(self) memset(self, 0, sizeof(self)) + +/** + * @brief Inlined initialization of 3D vector. + * @param[in] v 3D vector + * @param[in] x Initial x coordinate + * @param[in] y Initial y coordinate + * @param[in] z Initial z coordinate + */ +#define vector_3d_set(v, x, y, z)\ + do {\ + (v)[X] = x;\ + (v)[Y] = y;\ + (v)[Z] = z;\ + } while (0) + +#define vector_3d_copy(dst, src) memcpy(dst, src, sizeof(Vector_3d)) + +/** + * @brief Inlined difference of two 3D vectors. + * @param[in] u 3D vector + * @param[in] v 3D vector + * @param[out] out Difference of a @u and a @v + */ +#define vector_3d_sub_vector_3d(u, v, out)\ + do {\ + (out)[X] = (u)[X] - (v)[X];\ + (out)[Y] = (u)[Y] - (v)[Y];\ + (out)[Z] = (u)[Z] - (v)[Z];\ + } while (0) + +/** + * @brief Inlined cross-product of two 3D vectors. + * @param[in] u 3D vector + * @param[in] v 3D vector + * @param[out] out Cross-product of a @u and a @v + */ +#define vector_3d_cross_vector_3d(u, v, out)\ + do {\ + (out)[X] = (u)[Y] * (v)[Z] - (u)[Z] * (v)[Y];\ + (out)[Y] = (u)[Z] * (v)[X] - (u)[X] * (v)[Z];\ + (out)[Z] = (u)[X] * (v)[Y] - (u)[Y] * (v)[X];\ + } while (0) + +/** + * @brief Inlined product of 3D vector and number. + * @param[in] v 3D vector + * @param[in] d Number + * @param[out] out Cross-product of a @v and a @d + */ +#define vector_3d_mul_number(v, d, out)\ + do {\ + (out)[X] = (v)[X] * (d);\ + (out)[Y] = (v)[Y] * (d);\ + (out)[Z] = (v)[Z] * (d);\ + } while (0) + +extern void vector_3d_normalization(Vector_3d_ref v); + +/** + * @brief Dot product of two 3D vectors. + * @param[in] u 3D vector + * @param[in] v 3D vector + * @return Scalar product of a @u and a @v. + */ +#define vector_3d_dot_product(u, v) ((u)[X] * (v)[X] + (u)[Y] * (v)[Y] + (u)[Z] * (v)[Z]) + +/** + * @brief Squared length of a 3D vector. + * @param[in] x 3D vector + * @return Squared length of @a x, i.e. |x|^2 = x * x + */ +#define vector_3d_length2(x) vector_3d_dot_product(x, x) + +/** + * @brief Length of a 3D vector. + * @param[in] x 3D vector + * @return Length of @a x, i.e. |x| = sqrt(x * x) + */ +#define vector_3d_length(x) sqrt(vector_3d_length2(x)) + +/** + * @brief Distance between two 3D vectors. + * @param[in] p First 3D vector + * @param[in] q Second 3D vector + * @return Euclidean distance between vectors. + */ +extern double vector_3d_distance(const_Vector_3d_ref p, const_Vector_3d_ref q); + +extern void vector_3d_linear_combination(double a1, const_Vector_3d_ref v1, + double a2, const_Vector_3d_ref v2, Vector_3d_ref v); + +/** + * @brief Angle between two vectors. + * @param[in] v1 First 3D vector + * @param[in] v2 Second 3D vector + * @return Angle between vectors in radians. Always in the range [0, PI]. + */ +extern double vector_3d_angle(const_Vector_3d_ref v1, const_Vector_3d_ref v2); + +/** Labels for 2D vectors indexes. */ +typedef enum { + PLANAR_X, + PLANAR_Y, + PLANAR_DIMENSION +} fl_planar; + +typedef double Vector_2d[PLANAR_DIMENSION]; +typedef double* Vector_2d_ref; +typedef const double* const_Vector_2d_ref; + +/** + * @brief Dot product of two 2D vectors. + * @param[in] u 2D vector + * @param[in] v 2D vector + * @return (double) Dot (scalar) product of a @u and a @v. + */ +#define vector_2d_dot_product(u, v) ((u)[PLANAR_X] * (v)[PLANAR_X] + (u)[PLANAR_Y] * (v)[PLANAR_Y]) + +/** + * @brief Cross product of two 2D vectors. + * @param[in] u 2D vector + * @param[in] v 2D vector + * @return (double) Cross product of a @u and a @u i.e. |u||v|sin(uv). + */ +#define vector_2d_cross_product(u, v) ((u)[PLANAR_X] * (v)[PLANAR_Y] - (u)[PLANAR_Y] * (v)[PLANAR_X]) + +/** + * @brief Squared length of a 2D vector in Euclidean metric. + * @param[in] v 2D vector + * @return (double) Squared length of a @v, i.e. |v|^2 = v * v + */ +#define vector_2d_length2(v) vector_2d_dot_product(v, v) + +/** + * @brief Length of a 2D vector in Euclidean metric. + * @param[in] v 2D vector + * @return (double) Length of a @v, i.e. |v| = sqrt(v * v) + */ +#define vector_2d_length(v) sqrt(vector_2d_length2(v)) + +/** + * @brief Angle between two vectors. + * @param[in] v1 First 2D vector + * @param[in] v2 Second 2D vector + * @return (double) Angle between vectors in radians. + */ +#define vector_2d_angle(v1, v2) atan2(vector_2d_cross_product(v1, v2), vector_2d_dot_product(v1, v2)) + +#ifdef __cplusplus +} +#endif + +#endif /* __VECTOR_H_ */ diff --git a/lbs-server/src/fused/Waema3dEstimator.c b/lbs-server/src/fused/Waema3dEstimator.c new file mode 100644 index 0000000..853cce0 --- /dev/null +++ b/lbs-server/src/fused/Waema3dEstimator.c @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "Waema3dEstimator.h" +#include "debug_util.h" + +void waema3d_estimator_reset(Waema3dEstimator* self, double alpha, double max_evidence, double ascale2) +{ + vector_3d_clear(self->wf); + vector_3d_clear(self->value); + self->max_evidence = max_evidence; + self->ascale2 = ascale2; + self->alpha = alpha; + self->scale2 = ascale2; + self->evidence = 0; + self->weight_norm = 1; +} + +void waema3d_estimator_set(Waema3dEstimator* self, double scale2, double evidence, double weight_norm, const_Vector_3d_ref w0) +{ + self->scale2 = scale2; + self->evidence = evidence; + self->weight_norm = weight_norm; + vector_3d_copy(self->value, w0); +} + +const_Vector_3d_ref waema3d_estimator_filter(Waema3dEstimator* self, const_Vector_3d_ref wm) +{ + const double fw2 = vector_3d_length2(self->wf); + const double weight = exp(-fw2 * self->scale2); + if (self->evidence < self->max_evidence) { + self->evidence += weight; + if (self->evidence > self->max_evidence) { + self->evidence = self->max_evidence; + } + self->weight_norm = 1.0 / self->evidence; + self->scale2 = self->ascale2 * (1.0 + fl_square(self->evidence) * self->alpha); + } + const double update_rate = weight * self->weight_norm; + const double decay_rate = 1.0 - update_rate; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("fw2=%g, e=%g, s2=%g, w=%g"), + fw2, self->evidence, self->scale2, weight); + vector_3d_linear_combination(decay_rate, self->value, update_rate, wm, self->value); + vector_3d_sub_vector_3d(wm, self->value, self->wf); + return self->wf; +} diff --git a/lbs-server/src/fused/Waema3dEstimator.h b/lbs-server/src/fused/Waema3dEstimator.h new file mode 100644 index 0000000..5903450 --- /dev/null +++ b/lbs-server/src/fused/Waema3dEstimator.h @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file Waema3dEstimator.h + * @brief Weighted AEMA filter. + */ + +#pragma once +#ifndef __WAEMA3DESTIMATOR_H_ +#define __WAEMA3DESTIMATOR_H_ + +#include "Vector.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Weighted AEMA filter. This is generalization of the 3D AEMA filter by + * using 'evidence' as the sum of weights rather than sample count. + * The sample weights are estimated using Gaussian curve with time-decreasing variance. + */ +typedef struct { + double max_evidence; + double ascale2; + double alpha; + double scale2; /* = 1 / (2 variance) */ + double evidence; /* min(accumulated weight, plateau_evidence) */ + double weight_norm; /* = 1 / evidence */ + Vector_3d wf; + Vector_3d value; /* filter output (estimated value) */ +} Waema3dEstimator; + +extern void waema3d_estimator_reset(Waema3dEstimator* self, double alpha, double aevidence, double ascale2); +extern void waema3d_estimator_set(Waema3dEstimator* self, double scale2, double evidence, double weight_norm, const_Vector_3d_ref w0); +extern const_Vector_3d_ref waema3d_estimator_filter(Waema3dEstimator* self, const_Vector_3d_ref wm); + +#ifdef __cplusplus +} +#endif + +#endif /* __WAEMA3DESTIMATOR_H_ */ diff --git a/lbs-server/src/fused/accelerometer-filter.c b/lbs-server/src/fused/accelerometer-filter.c deleted file mode 100644 index 4647bc2..0000000 --- a/lbs-server/src/fused/accelerometer-filter.c +++ /dev/null @@ -1,146 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ -#endif - -#define __FL_ACCELEROMETER_FILTER_C__ - -#define _USE_MATH_DEFINES -#include -#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, =%5.2f"), t, am[GEO_SPATIAL_X], am[GEO_SPATIAL_Y], am[GEO_SPATIAL_Z], sqrt(af2), sqrt(g2)); - if (af2 > SQUARE(FL_ACCEL_NOISE_LEVEL) && g2 > 1) { - fl_real norm_out = 1.0 / sqrt(af2); - - gu[GEO_SPATIAL_X] = af[GEO_SPATIAL_X] * norm_out; - gu[GEO_SPATIAL_Y] = af[GEO_SPATIAL_Y] * norm_out; - gu[GEO_SPATIAL_Z] = af[GEO_SPATIAL_Z] * norm_out; - *w2gu = __gres.base.decay_rate / (fl_square(gu[GEO_SPATIAL_X]) + fl_square(gu[GEO_SPATIAL_Y]) + fl_square(gu[GEO_SPATIAL_Z] - 1) + GEO_SPATIAL_DIMENSION * DEFAULT_ACCELERATION_SIGMA2); - } else { - gu[GEO_SPATIAL_X] = 0; - gu[GEO_SPATIAL_Y] = 0; - gu[GEO_SPATIAL_Z] = 1; /* default */ - *w2gu = __gres.base.decay_rate / (GEO_SPATIAL_DIMENSION * DEFAULT_ACCELERATION_SIGMA2); - } - - fl_acceleration g = sqrt(g2); - if (g > 1) { - fl_real calibration; - - calibration = EARTH_GRAVITY / g; - al[GEO_SPATIAL_X] = calibration * am[GEO_SPATIAL_X] - EARTH_GRAVITY * gu[GEO_SPATIAL_X]; - al[GEO_SPATIAL_Y] = calibration * am[GEO_SPATIAL_Y] - EARTH_GRAVITY * gu[GEO_SPATIAL_Y]; - al[GEO_SPATIAL_Z] = calibration * am[GEO_SPATIAL_Z] - EARTH_GRAVITY * gu[GEO_SPATIAL_Z]; - } else { - al[GEO_SPATIAL_X] = am[GEO_SPATIAL_X]; - al[GEO_SPATIAL_Y] = am[GEO_SPATIAL_Y]; - al[GEO_SPATIAL_Z] = am[GEO_SPATIAL_Z]; - } - fl_moti_process(t, al); -} diff --git a/lbs-server/src/fused/accelerometer-filter.h b/lbs-server/src/fused/accelerometer-filter.h deleted file mode 100644 index 8e70f1e..0000000 --- a/lbs-server/src/fused/accelerometer-filter.h +++ /dev/null @@ -1,125 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file accelerometer-filter.h - * @brief Accelerometer calibration (scale and sampling frequency), and - * low-pass 3D filtering. - */ - -#pragma once -#ifndef __FL_ACCELEROMETER_FILTER_H__ -#define __FL_ACCELEROMETER_FILTER_H__ - -#include "types.h" -#include "motion-detector.h" -#include "parameters.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************//** - * [public] Initializaton of the singleton unit (static constructor). - * - * @param[in] motion_changed - * Callback to be invoked when the detected state of motion changes. - * @param[in] handler - * Arbitrary user parameter passed to the callback. - */ -void fl_accel_init(motion_changed_cb motion_changed, gpointer handler); - -/***************************************************************************//** - * [public] Cleanup of the singleton unit (static destructor). - */ -void fl_accel_exit(void); - -/***************************************************************************//** - * [public] Reset of the internal state back to initial one. This function is - * used for repetitive testing and module soft restarts. - * - * @param[in] av - * Stationary acceleration state to be set (typically {0, 0, g}). - */ -void fl_accel_reset(const_fl_acceleration_vector_ref av); - -/***************************************************************************//** - * [public] Adapted internal state to the given sampling frequency. Call this - * method whenever the measured frequency changes significantly. - * - * @param[in] f - * Measured accelerometer sampling frequency in Hertz. - * @param[in] t - * Time of the event in seconds. - */ -void fl_accel_set_sampling_frequency(fl_hertz samplingFrequency, fl_seconds t); - -/***************************************************************************//** - * [public] Processing of a single sample from the accelerometer. This function - * performs: - * - estimation of the sampling frequency, - * - estimation of the gravity direction via low-pass 3D filter (cf. LP3D unit), - * - estimation of the gravity value (cf. GRES unit) and scale correction, - * - estimation of the linear acceleration i.e. with subtracted gravity component, - * - motion state detection (cf. MOTI unit) - * - * @param[in] t - * Time of the event in seconds. - * @param[in] am - * Measured 3D acceleration vector transformend to local frame. - * @param[out] al - * Linear acceleration in local frame. - * @param[out] gu - * Unit vector along gravity direction oriented upwards. - * @param[out] w2gu - * Estimated inverse covariance (squared weight) of @a gu. - */ -void fl_accel_process(fl_seconds t, const_fl_acceleration_vector_ref am, fl_acceleration_vector_ref al, fl_vector_3d_ref gu, fl_real *w2gu); - - -/** - * Accelerometer calibration (scale and sampling frequency), and low-pass 3D filtering. - */ - -#if defined(__FL_ACCELEROMETER_FILTER_C__) - #ifdef GTEST - #define PRIVATE - #else - #define PRIVATE \ - static - #endif -#else - #define PRIVATE \ - extern -#endif - -typedef struct { - unsigned samples; - fl_seconds last_update_time; - fl_hertz last_update_frequency; -} _fl_accel_frequency_estimator; - -PRIVATE _fl_accel_frequency_estimator __frequency_estimator; - -#if !defined(__FL_ACCELEROMETER_FILTER_C__) - #undef PRIVATE -#endif - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_ACCELEROMETER_FILTER_H__ */ diff --git a/lbs-server/src/fused/aema-estimator.c b/lbs-server/src/fused/aema-estimator.c deleted file mode 100644 index 170abf3..0000000 --- a/lbs-server/src/fused/aema-estimator.c +++ /dev/null @@ -1,170 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_DETAIL */ -#endif - -#define __FL_AEMA_ESTIMATOR_C__ - -#include -#include "aema-estimator.h" -#include "debug_util.h" - -void __aema_init(fl_aema_estimator* self, unsigned plateau_samples) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](bits=%u)"), self, plateau_samples); - self->plateau_samples = plateau_samples; - self->samples = 0; - self->update_rate = 1; - self->decay_rate = 0; -} - -/***************************************************************************//** - * [protected] Cleanup of the base class (destructor). - */ -static inline void __aema_exit(fl_aema_estimator *self) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self); -} - -void fl_aema1d_init(fl_aema1d_estimator* self, unsigned plateau_samples) -{ - __aema_init(&self->base, plateau_samples); - self->value = 0; -} - -void fl_aema2d_init(fl_aema2d_estimator* self, unsigned plateau_samples) -{ - __aema_init(&self->base, plateau_samples); - self->value[AXIAL_H] = 0; - self->value[AXIAL_V] = 0; -} - -void fl_aema3d_init(fl_aema3d_estimator* self, unsigned plateau_samples) -{ - __aema_init(&self->base, plateau_samples); - self->value[GEO_SPATIAL_X] = 0; - self->value[GEO_SPATIAL_Y] = 0; - self->value[GEO_SPATIAL_Z] = 0; -} - -void fl_aema1d_exit(fl_aema1d_estimator *self) -{ - __aema_exit(&self->base); -} - -void fl_aema2d_exit(fl_aema2d_estimator *self) -{ - __aema_exit(&self->base); -} - -void fl_aema3d_exit(fl_aema3d_estimator *self) -{ - __aema_exit(&self->base); -} - -void __aema_reset(fl_aema_estimator* self) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self); - self->samples = 0; - self->update_rate = 1; - self->decay_rate = 0; -} - -void fl_aema1d_reset(fl_aema1d_estimator* self) -{ - __aema_reset(&self->base); - self->value = 0; -} - -void fl_aema2d_reset(fl_aema2d_estimator* self) -{ - __aema_reset(&self->base); - self->value[PLANAR_X] = 0; - self->value[PLANAR_Y] = 0; -} - -void fl_aema3d_reset(fl_aema3d_estimator* self) -{ - __aema_reset(&self->base); - self->value[GEO_SPATIAL_X] = 0; - self->value[GEO_SPATIAL_Y] = 0; - self->value[GEO_SPATIAL_Z] = 0; -} - -fl_real fl_aema1d_process(fl_aema1d_estimator* self, fl_real value) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value); - __aema_process(&self->base); - /* exponential moving avarege */ - self->value = self->base.decay_rate * self->value - + self->base.update_rate * value; - - return self->value; -} - -const_fl_vector_2d_ref fl_aema2d_process(fl_aema2d_estimator* self, fl_real value_H, fl_real value_V) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V); - - __aema_process(&self->base); -#define UPDATE_VALUE(i) self->value[AXIAL_##i] = self->base.decay_rate * self->value[AXIAL_##i] + self->base.update_rate * value_##i; - UPDATE_VALUE(H); - UPDATE_VALUE(V); -#undef UPDATE_VALUE - - return self->value; -} - -const_fl_vector_3d_ref fl_aema3d_process(fl_aema3d_estimator* self, const_fl_vector_3d_ref value) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[0], value[1], value[2]); - - __aema_process(&self->base); -#define UPDATE_VALUE(i) self->value[i] = self->base.decay_rate * self->value[i] + self->base.update_rate * value[i]; - UPDATE_VALUE(GEO_SPATIAL_X); - UPDATE_VALUE(GEO_SPATIAL_Y); - UPDATE_VALUE(GEO_SPATIAL_Z); -#undef UPDATE_VALUE - - return self->value; -} - -void fl_aema1d_set_estimate(fl_aema1d_estimator* self, fl_real value) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value); - __aema_set_estimate(&self->base); - self->value = value; -} - -void fl_aema2d_set_estimate(fl_aema2d_estimator* self, fl_real value_H, fl_real value_V) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V); - __aema_set_estimate(&self->base); - self->value[AXIAL_H] = value_H; - self->value[AXIAL_V] = value_V; -} - -void fl_aema3d_set_estimate(fl_aema3d_estimator* self, const_fl_vector_3d_ref value) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[0], value[1], value[2]); - __aema_set_estimate(&self->base); - self->value[GEO_SPATIAL_X] = value[GEO_SPATIAL_X]; - self->value[GEO_SPATIAL_Y] = value[GEO_SPATIAL_Y]; - self->value[GEO_SPATIAL_Z] = value[GEO_SPATIAL_Z]; -} diff --git a/lbs-server/src/fused/aema-estimator.h b/lbs-server/src/fused/aema-estimator.h deleted file mode 100644 index 36282f5..0000000 --- a/lbs-server/src/fused/aema-estimator.h +++ /dev/null @@ -1,312 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file aema-estimator.h - * @brief Adaptive Exponential Moving Average (1st order) filter - */ - -#pragma once -#ifndef __FL_AEMA_ESTIMATOR_H__ -#define __FL_AEMA_ESTIMATOR_H__ - -#include "types.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(__FL_AEMA_ESTIMATOR_C__) - #define INLINE \ - inline -#else - #define INLINE \ - static inline -#endif - -typedef struct { - unsigned plateau_samples; /* number of samples to collect before EMA kicks-in */ - unsigned samples; /* min(number of samples collected, plateau_samples) */ - fl_real update_rate; /* = 1 / samples */ - fl_real decay_rate; /* = 1 - update_rate */ -} fl_aema_estimator; - -typedef struct { - fl_aema_estimator base; /* base object */ - fl_real value; /* filter output (estimated value) */ -} fl_aema1d_estimator; - -typedef struct { - fl_aema_estimator base; /* base object */ - fl_vector_2d value; /* filter output (estimated value) */ -} fl_aema2d_estimator; - -typedef struct { - fl_aema_estimator base; /* base object */ - fl_vector_3d value; /* filter output (estimated value) */ -} fl_aema3d_estimator; - -/***************************************************************************//** - * [public] Initializaton of the 1D AEMA object (class constructor). - * - * @param[in] self - * Object pointer - * @param[in] plateau_samples - * Number of samples to collect before switching to standard EMA mode. - */ -void fl_aema1d_init(fl_aema1d_estimator *self, unsigned plateau_samples); - -/***************************************************************************//** - * [public] Initializaton of the 2D AEMA object (class constructor). - * - * @param[in] self - * Object pointer - * @param[in] plateau_samples - * Number of samples to collect before switching to standard EMA mode. - */ -void fl_aema2d_init(fl_aema2d_estimator *self, unsigned plateau_samples); - -/***************************************************************************//** - * [public] Initializaton of the 3D AEMA object (class constructor). - * - * @param[in] self - * Object pointer - * @param[in] plateau_samples - * Number of samples to collect before switching to standard EMA mode. - */ -void fl_aema3d_init(fl_aema3d_estimator *self, unsigned plateau_samples); - -/***************************************************************************//** - * [public] Cleanup of the 1D AEMA object (class destructor). - */ -void fl_aema1d_exit(fl_aema1d_estimator *self); - -/***************************************************************************//** - * [public] Cleanup of the 2D AEMA object (class destructor). - */ -void fl_aema2d_exit(fl_aema2d_estimator *self); - -/***************************************************************************//** - * [public] Cleanup of the 3D AEMA object (class destructor). - */ -void fl_aema3d_exit(fl_aema3d_estimator *self); - -/***************************************************************************//** - * [public] Reset the 1D AEMA object back to initial state. This function is - * used for repetitive testing and module soft restarts. - * - * @param[in] self - * Object pointer - */ -void fl_aema1d_reset(fl_aema1d_estimator *self); - -/***************************************************************************//** - * [public] Reset the 2D AEMA object back to initial state. This function is - * used for repetitive testing and module soft restarts. - * - * @param[in] self - * Object pointer - */ -void fl_aema2d_reset(fl_aema2d_estimator *self); - -/***************************************************************************//** - * [public] Reset the 3D AEMA object back to initial state. This function is - * used for repetitive testing and module soft restarts. - * - * @param[in] self - * Object pointer - */ -void fl_aema3d_reset(fl_aema3d_estimator *self); - -/***************************************************************************//** - * [public] Processing of a single data sample. - * - * @param[in] self - * Object pointer - * @param[in] value - * Input value to the filter. - * - * @return - * Output value of the filter after processing. This is the same as - * returned by fl_aema1d_get_estimate(self). - */ -fl_real fl_aema1d_process(fl_aema1d_estimator *self, fl_real value); - -/***************************************************************************//** - * [public] Processing of a double data sample. - * - * @param[in] self - * Object pointer - * @param[in] value_H - * First (horizontal) input value to the filter. - * @param[in] value_V - * Second (vertical) input value to the filter. - * - * @return - * Output value of the filter after processing. This is the same as - * returned by fl_aema2d_get_estimate(self). - */ -const_fl_vector_2d_ref fl_aema2d_process(fl_aema2d_estimator *self, fl_real value_X, fl_real value_Y); - -/***************************************************************************//** - * [public] Processing of a triple data sample. - * - * @param[in] self - * Object pointer - * @param[in] value - * Input 3D vector value to the filter. - * - * @return - * Output value of the filter after processing. This is the same as - * returned by fl_aema3d_get_estimate(self). - */ -const_fl_vector_3d_ref fl_aema3d_process(fl_aema3d_estimator *self, const_fl_vector_3d_ref value); - -/***************************************************************************//** - * [public] Bring the filter to a stationary state (EMA mode, input = output). - * This method is used for testing. - * - * @param[in] self - * Object pointer - * @param[in] value - * I/o value of the filter. - */ -void fl_aema1d_set_estimate(fl_aema1d_estimator *self, fl_real value); - -/***************************************************************************//** - * [public] Bring the filter to a stationary state (EMA mode, input = output). - * This method is used for testing. - * - * @param[in] self - * Object pointer - * @param[in] value_H - * First (horizontal) i/o value of the filter. - * @param[in] value_V - * Second (vertical) i/o value of the filter. - */ -void fl_aema2d_set_estimate(fl_aema2d_estimator *self, fl_real value_X, fl_real value_Y); - -/***************************************************************************//** - * [public] Bring the filter to a stationary state (EMA mode, input = output). - * This method is used for testing. - * - * @param[in] self - * Object pointer - * @param[in] value - * I/o 3D vector value of the filter. - */ -void fl_aema3d_set_estimate(fl_aema3d_estimator *self, const_fl_vector_3d_ref value); - -/***************************************************************************//** - * [public] Fetch the current filter output. - * - * @param[in] self - * Object pointer - * - * @return - * Output value after last processed sample. - */ -INLINE fl_real fl_aema1d_get_estimate(const fl_aema1d_estimator *self) -{ - return self->value; -} - -/***************************************************************************//** - * [public] Fetch the current filter output. - * - * @param[in] self - * Object pointer - * - * @return - * Output 2D vector after last processed sample. - */ -INLINE const_fl_vector_2d_ref fl_aema2d_get_estimate(const fl_aema2d_estimator *self) -{ - return self->value; -} - -/***************************************************************************//** - * [public] Fetch the current filter output. - * - * @param[in] self - * Object pointer - * - * @return - * Output 3D vector after last processed sample. - */ -INLINE const_fl_vector_3d_ref fl_aema3d_get_estimate(const fl_aema3d_estimator *self) -{ - return self->value; -} - -/***************************************************************************//** - * [protected] Initializaton of the base object (class constructor). - * - * @param[in] self - * Object pointer - * @param[in] plateau_samples - * Number of samples to collect before switching to standard EMA mode. - */ -void __aema_init(fl_aema_estimator *self, unsigned plateau_samples); - -/***************************************************************************//** - * [protected] Reset the AEMA object back to initial state. This function is - * used for repetitive testing and module soft restarts. - * - * @param[in] self - * Object pointer - */ -void __aema_reset(fl_aema_estimator *self); - -/***************************************************************************//** - * [protected] Processing of a data sample. This is the common part of the - * derived processing methods, which implements the AEMA update/decay rate. - * - * @param[in] self - * Object pointer - */ -INLINE void __aema_process(fl_aema_estimator *self) -{ - if (self->samples < self->plateau_samples) { - self->samples++; - self->update_rate = 1.0 / self->samples; - self->decay_rate = 1.0 - self->update_rate; - } -} - -/***************************************************************************//** - * [protected] Bring the base filter to a stationary state (EMA mode). This - * method is used for testing. - * - * @param[in] self - * Object pointer - */ -INLINE void __aema_set_estimate(fl_aema_estimator* self) -{ - self->samples = self->plateau_samples; - self->update_rate = 1.0 / self->plateau_samples; - self->decay_rate = 1.0 - self->update_rate; -} - -#if !defined(__FL_AEMA_ESTIMATOR_C__) - #undef INLINE -#endif - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_AEMA_ESTIMATOR_H__ */ diff --git a/lbs-server/src/fused/conversions.c b/lbs-server/src/fused/conversions.c deleted file mode 100644 index 048177d..0000000 --- a/lbs-server/src/fused/conversions.c +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#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; -} diff --git a/lbs-server/src/fused/conversions.h b/lbs-server/src/fused/conversions.h deleted file mode 100644 index 57f3f73..0000000 --- a/lbs-server/src/fused/conversions.h +++ /dev/null @@ -1,204 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file conversions.h - * @brief Units conversion functions - */ - -#pragma once -#ifndef __FL_CONVERSIONS_H__ -#define __FL_CONVERSIONS_H__ - -#define _USE_MATH_DEFINES -#include "math-ext.h" -#include "earth.h" -#include "types.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(__FL_CONVERSIONS_C__) - #define INLINE \ - inline /* thank checkpatchinit_tizen for this contraption */ -#else - #define INLINE \ - static inline -#endif - -#define FL_TIMESTAMP_UNITS 1 /**< [s] */ - -/***************************************************************************//** - * [public] Convert location time-stamp to seconds. Notice, on Tizen this is a - * formal methd since time-stamps are also resolved in seconds. - * - * @param[in] timestamp - * Time-stamp received from the location source (GPS, WPS). - * - * @return - * Corresponding value in seconds - */ -INLINE fl_seconds fl_timestamp_to_seconds(fl_timestamp timestamp) -{ - return (fl_seconds)timestamp / FL_TIMESTAMP_UNITS; -} - -/***************************************************************************//** - * [public] Convert time in seconds to location time-stamp. The necesary - * up-rounding is is half the time-stamp unit. - * - * @param[in] timestamp - * Time-stamp received from the location source (GPS, WPS). - * - * @return - * Corresponding value in seconds - */ -INLINE fl_timestamp fl_seconds_to_timestamp(fl_seconds t) -{ - return (fl_timestamp)(t * FL_TIMESTAMP_UNITS + 0.5); -} - -/***************************************************************************//** - * [public] Convert degrees to radians. - * - * @param[in] degrees - * Value in degrees. - * - * @return - * Corresponding value in radians. - */ -INLINE fl_radians fl_degrees_to_radians(fl_degrees degrees) -{ - return (M_PI / 180) * degrees; -} - -/***************************************************************************//** - * [public] Convert radians to degrees. - * - * @param[in] radians - * Value in radians. - * - * @return - * Corresponding value in degrees. - */ -INLINE fl_degrees fl_radians_to_degrees(fl_radians radians) -{ - return (180 / M_PI) * radians; -} - -/***************************************************************************//** - * [public] Convert radians to degrees in the [0,360) range - * - * @param[in] radians - * Arbitrary value in radians. - * - * @return - * Value in degrees projected onto the [0,360) branch. - */ -fl_degrees fl_radians_to_positive_degrees(fl_radians radians); - -/***************************************************************************//** - * [public] Convert radians to degrees in the [-180,180) range. - * - * @param[in] radians - * Arbitrary value in radians. - * - * @return - * Value in degrees projected onto the [-180,180) branch. - */ -fl_degrees fl_radians_to_balanced_degrees(fl_radians radians); - -/** [public] Convert radians to latitude degrees [-90,90] */ -#define fl_radians_to_latitude fl_radians_to_balanced_degrees - -/** [public] Convert radians to longitude degrees [0, 360) */ -#define fl_radians_to_longitude fl_radians_to_positive_degrees - -/** [public] Convert radians to longitude degrees [0, 360) */ -#define fl_radians_to_cog fl_radians_to_positive_degrees - -/***************************************************************************//** - * [public] Convert speed in [km/h] to velocity in [m/s]. - * - * @note The 'speed' is an i/o value of location interfaces, while 'velocity' - * expressed in SI units is used internally by the fused location engine. - * - * @param[in] km_h - * Speed in kilometers per hour. - * - * @return - * Corresponding value in meters per second. - */ -INLINE fl_velocity fl_km_h_to_m_s(fl_km_h km_h) -{ - return km_h * (1000.0 / (60.0 * 60.0)); -} - -/***************************************************************************//** - * [public] Convert velocity in [m/s] to speed in [km/h]. - * - * @note The 'speed' is an i/o value of location interfaces, while 'velocity' - * expressed in SI units is used internally by the fused location engine. - * - * @param[in] velocity - * Velocity in meters per second. - * - * @return - * Corresponding value in kilometers per hour. - */ -INLINE fl_km_h fl_m_s_to_km_h(fl_velocity velocity) -{ - return velocity * ((60.0 * 60.0) / 1000.0); -} - -/***************************************************************************//** - * [public] Convert spherical coordinate in radians to arc-length along - * the Earth's great circle in meters. - * - * @param[in] radians - * Angle spanned by the great arc. - * - * @return - * Corresponding value in meters along Earth's surface. - */ -INLINE fl_meters fl_radians_to_meters(fl_radians radians) -{ - return radians * EARTH_RADIUS; -} - -/***************************************************************************//** - * [public] Convert arc-length in meters along the Earth's great circle to arc - * angle in radians. - * - * @param[in] meters - * Arc length in meters. - * - * @return - * Corresponding angle in radians. - */ -INLINE fl_radians fl_meters_to_radians(fl_meters meters) -{ - return meters * (1.0 / EARTH_RADIUS); -} - -#undef INLINE - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_CONVERSIONS_H_ */ diff --git a/lbs-server/src/fused/earth.h b/lbs-server/src/fused/earth.h deleted file mode 100644 index 4291251..0000000 --- a/lbs-server/src/fused/earth.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file earth.h - * @brief Constants and methods related to the Earth model. - */ - -#pragma once -#ifndef __FL_EARTH_H__ -#define __FL_EARTH_H__ - -#include "types.h" - -/***************************************************************************//** - * [public] Mean Earth gravity value in [m/s^2]. - * - * Notice that, strictly speaking, the mean Earth gravity acceleration at surface - * level is negative; the positive value we measure with an accelerometer is the - * ground reaction which counters the gravity and prevents us from falling into - * the interior. Also notice that given the inaccuracy (i.e. bias - * & miscalibration) of mobile device acceleromenters at ~1 [m/s^2] the number - * of significant digits in the reference value below is way too many. - */ -#define EARTH_GRAVITY ((fl_acceleration)9.80665) - -/***************************************************************************//** - * [public] Mean Earth radius in [m]. - */ -#define EARTH_RADIUS ((fl_meters)6371000.0) - -#endif /* __FL_EARTH_H__ */ diff --git a/lbs-server/src/fused/gravity-estimator.c b/lbs-server/src/fused/gravity-estimator.c deleted file mode 100644 index ed8356a..0000000 --- a/lbs-server/src/fused/gravity-estimator.c +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ -#endif - -#define __FL_GRAVITY_ESTIMATOR_C__ - -#include "gravity-estimator.h" -#include "debug_util.h" - -void fl_gres_init(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - fl_aema1d_init(&__gres, 1 << FL_GRES_AEMA_PLATEAU_BITS); -} - -void fl_gres_exit(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - fl_aema1d_exit(&__gres); -} - -void fl_gres_reset(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - fl_aema1d_reset(&__gres); -} diff --git a/lbs-server/src/fused/gravity-estimator.h b/lbs-server/src/fused/gravity-estimator.h deleted file mode 100644 index 9d4a638..0000000 --- a/lbs-server/src/fused/gravity-estimator.h +++ /dev/null @@ -1,113 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file gravity-estimator.h - * @brief Estimator of the gravity value used for accelerometer scale - * calibration. - */ - -#pragma once -#ifndef __FL_GRAVITY_ESTIMATOR_H__ -#define __FL_GRAVITY_ESTIMATOR_H__ - -#include "parameters.h" -#include "aema-estimator.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(__FL_GRAVITY_ESTIMATOR_C__) - #define INLINE \ - static inline /* thank checkpatchinit_tizen for this contraption */ - #define PRIVATE -#else - #define INLINE \ - static inline - #define PRIVATE \ - extern -#endif - -PRIVATE fl_aema1d_estimator __gres; - -/***************************************************************************//** - * [public] Initializaton of the singleton unit (static constructor). - */ -void fl_gres_init(void); - -/***************************************************************************//** - * [public] Cleanup of the singleton unit (static destructor). - */ -void fl_gres_exit(void); - -/***************************************************************************//** - * [public] Bring the internal state back to initial one. This function is used - * for repetitive testing and module soft restarts. - */ -void fl_gres_reset(void); - -/***************************************************************************//** - * [public] Processing of a single data sample. - * - * @param[in] g2 - * Modulus squared of the measured acceleration projected onto z-axis. - * - * @return - * Estimated value of the Earth gravity. - */ -INLINE fl_real fl_gres_process(fl_real g2) -{ - return fl_aema1d_process(&__gres, g2); -} - -/***************************************************************************//** - * [public] Bring the filter to a stationary state (EMA mode, input = output). - * This method is used for testing. - * - * @param[in] g2 - * Modulus squared value of Earth gravity measured by the accelerometer. - */ -INLINE void fl_gres_set_estimate(fl_real g2) -{ - fl_aema1d_set_estimate(&__gres, g2); -} - -/***************************************************************************//** - * [public] Fetch the current gravity estimate. - * - * @param[in] self - * Object pointer - * - * @return - * Output value after last processed sample. - */ -/** Retrieve current estimate */ -INLINE fl_real fl_gres_get_estimate(void) -{ - return fl_aema1d_get_estimate(&__gres); -} - -#if !defined(__FL_GRAVITY_ESTIMATOR_C__) - #undef PRIVATE - #undef INLINE -#endif - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_GRAVITY_ESTIMATOR_H__ */ diff --git a/lbs-server/src/fused/gyroscope-filter.c b/lbs-server/src/fused/gyroscope-filter.c deleted file mode 100644 index 7c5e4e2..0000000 --- a/lbs-server/src/fused/gyroscope-filter.c +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ -#endif - -#define __FL_GYROSCOPE_FILTER_C__ - -#define _USE_MATH_DEFINES -#include -#include "math-ext.h" -#include "gravity-estimator.h" -#include "gyroscope-filter.h" -#include "debug_util.h" - -#define GYRO_PLATEAU_EVIDENCE (1 << FL_GYRO_AEMA_PLATEAU_BITS) -#define GYRO_SCALE2_I (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_I))) -#define GYRO_SCALE2_F (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_F))) - -static fl_real __gyro_alpha; - -void fl_gyro_init(void) -{ - LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); - __gyro_alpha = (SQUARE(FL_GYRO_SPIN_SIGMA_I / FL_GYRO_SPIN_SIGMA_F) - 1) / SQUARE((fl_real)GYRO_PLATEAU_EVIDENCE); - fl_gyro_reset(); - LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0); -} - -void fl_gyro_exit(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); -} - -void fl_gyro_reset(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - __gyro_bias.scale2 = GYRO_SCALE2_I; - __gyro_bias.evidence = 0; - __gyro_bias.weight_norm = 1; - __gyro_bias.value[GEO_SPATIAL_X] = 0; - __gyro_bias.value[GEO_SPATIAL_Y] = 0; - __gyro_bias.value[GEO_SPATIAL_Z] = 0; -} - -void fl_gyro_set(const_fl_spin_vector_ref w0) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%g, %g, %g)"), w0[GEO_SPATIAL_X], w0[GEO_SPATIAL_Y], w0[GEO_SPATIAL_Z]); - __gyro_bias.scale2 = GYRO_SCALE2_F; - __gyro_bias.evidence = GYRO_PLATEAU_EVIDENCE; - __gyro_bias.weight_norm = 1.0 / GYRO_PLATEAU_EVIDENCE; - __gyro_bias.value[GEO_SPATIAL_X] = w0[GEO_SPATIAL_X]; - __gyro_bias.value[GEO_SPATIAL_Y] = w0[GEO_SPATIAL_Y]; - __gyro_bias.value[GEO_SPATIAL_Z] = w0[GEO_SPATIAL_Z]; -} - -const_fl_spin_vector_ref fl_gyro_process(fl_seconds t, const_fl_spin_vector_ref wm) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs)"), t); - static fl_spin_vector wf; - fl_real weight; - fl_real update_rate; - fl_real decay_rate; - fl_real fw2; - - fw2 = fl_vector_3d_length2(wf); - weight = exp(-fw2 * __gyro_bias.scale2); - if (__gyro_bias.evidence < GYRO_PLATEAU_EVIDENCE) { - __gyro_bias.evidence += weight; - if (__gyro_bias.evidence > GYRO_PLATEAU_EVIDENCE) - __gyro_bias.evidence = GYRO_PLATEAU_EVIDENCE; - __gyro_bias.weight_norm = 1.0 / __gyro_bias.evidence; - __gyro_bias.scale2 = GYRO_SCALE2_I * (1.0 + fl_square(__gyro_bias.evidence) * __gyro_alpha); - } - update_rate = weight * __gyro_bias.weight_norm; - decay_rate = 1.0 - update_rate; - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs), fw2=%g, e=%g, s2=%g, w=%g"), t, fw2, __gyro_bias.evidence, __gyro_bias.scale2, weight); - -# define UPDATE_VALUE(i) __gyro_bias.value[i] = decay_rate * __gyro_bias.value[i] + update_rate * wm[i]; - UPDATE_VALUE(GEO_SPATIAL_X); - UPDATE_VALUE(GEO_SPATIAL_Y); - UPDATE_VALUE(GEO_SPATIAL_Z); -# undef UPDATE_VALUE - - wf[GEO_SPATIAL_X] = wm[GEO_SPATIAL_X] - __gyro_bias.value[GEO_SPATIAL_X]; - wf[GEO_SPATIAL_Y] = wm[GEO_SPATIAL_Y] - __gyro_bias.value[GEO_SPATIAL_Y]; - wf[GEO_SPATIAL_Z] = wm[GEO_SPATIAL_Z] - __gyro_bias.value[GEO_SPATIAL_Z]; - - return wf; -} diff --git a/lbs-server/src/fused/gyroscope-filter.h b/lbs-server/src/fused/gyroscope-filter.h deleted file mode 100644 index 95783cc..0000000 --- a/lbs-server/src/fused/gyroscope-filter.h +++ /dev/null @@ -1,109 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file gyroscope-filter.h - * @brief Correction of the gyroscope bias. - */ - -#pragma once -#ifndef __FL_GYROSCOPE_FILTER_H__ -#define __FL_GYROSCOPE_FILTER_H__ - -#include "types.h" -#include "parameters.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************//** - * [public] Initializaton of the singleton unit (static constructor). - */ -void fl_gyro_init(void); - -/***************************************************************************//** - * [public] Cleanup of the singleton unit (static destructor). - */ -void fl_gyro_exit(void); - -/***************************************************************************//** - * [public] Reset of the internal state back to initial one. - */ -void fl_gyro_reset(void); - -/***************************************************************************//** - * [public] Bring the filter to a stationary state (EMA mode, input = output). - * This method is used for testing. - * - * @param[in] w0 - * 3D vector of angular velocity measured by the gyroscope in inertial - * coordinate frame. - */ -void fl_gyro_set(const_fl_spin_vector_ref w0); - -/***************************************************************************//** - * [public] Processing of a vector data sample. This method performs: - * - estimation of the gyroscope bias with the weighted AEMA filter; the - * individual samples are weighted by the distance from average using Gaussian - * curve with monotonically-decreasing sigma parameter; - * - correction of the measurement by the estimated bias. - * - * @param[in] t - * Event time in seconds. - * @param[in] wm - * 3D vector of measured angular velocity. - * - * @return - * 3D vector of bias-corrected angular velocity. - */ -const_fl_spin_vector_ref fl_gyro_process(fl_seconds t, const_fl_spin_vector_ref wm); - -#if defined(__FL_GYROSCOPE_FILTER_C__) - #ifdef GTEST - #define PRIVATE - #else - #define PRIVATE \ - static - #endif -#else - #define PRIVATE \ - extern -#endif - -/** Weighted AEMA filter. This is generalization of the 3D AEMA filter by - * using 'evidence' as the sum of weights rather than sample count. The - * sample weights are estimated using Gaussian curve with time-decreasing - * variance. - */ -typedef struct { - fl_real scale2; /* = 1 / (2 variance) */ - fl_real evidence; /* min(accumulated weight, plateau_evidence) */ - fl_real weight_norm; /* = 1 / evidence */ - fl_vector_3d value; /* filter output (estimated value) */ -} fl_waema3d_estimator; - -PRIVATE fl_waema3d_estimator __gyro_bias; - -#if !defined(__FL_GYROSCOPE_FILTER_C__) - #undef PRIVATE -#endif - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_GYROSCOPE_FILTER_H__ */ diff --git a/lbs-server/src/fused/kalman-filter.c b/lbs-server/src/fused/kalman-filter.c deleted file mode 100644 index 97b3223..0000000 --- a/lbs-server/src/fused/kalman-filter.c +++ /dev/null @@ -1,1126 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ -#endif - -#define __FL_KALMAN_FILTER_C__ - -#include -#include -#include "math-ext.h" -#include "earth.h" -#include "conversions.h" -#include "accelerometer-filter.h" -#include "gyroscope-filter.h" -#include "aema-estimator.h" -#include "kalman-filter.h" -#include "debug_util.h" - -#define TIME_FORMAT "%.3fs" - -#define PRECISION 1e-4 -#define PRECISION2 SQUARE(PRECISION) - -/* the bigger the default standard deviantions, the swifter response to first measurements */ -#define INITIAL_POSITION_SIGMA2 SQUARE(1024 * M_2PI * EARTH_RADIUS) -#define INITIAL_VELOCITY_SIGMA2 (1024 * 1024 * SQUARE(FL_DEFAULT_VELOCITY_SIGMA)) -#define INITIAL_ACCELERATION_SIGMA2 DEFAULT_ACCELERATION_SIGMA2 - -#define __init_time_offset() fl_aema1d_init(&__timestamp_offset, 1 << FL_TIMO_AEMA_PLATEAU_BITS) -#define __exit_time_offset() fl_aema1d_exit(&__timestamp_offset) -#define __new_time_offset(dt) fl_aema1d_process(&__timestamp_offset, dt) -#define __get_time_offset() fl_aema1d_get_estimate(&__timestamp_offset) - -PRIVATE void __rectify_state(_fl_kalf_state *src) -{ - unsigned i; - - for (i = GEO_SPATIAL_DIMENSION; i--;) { - _fl_kalf_sigma_matrix_ref s2xi; - fl_real uuvv, uvvu; - - s2xi = src->s2x[i]; - /* diagonal elements */ - #define CHECK(u) \ - if (s2xi[STATE_##u][STATE_##u] < 0) { \ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): src.s2x[%u][" #u "][" #u "] = %g < 0"), src, i, s2xi[STATE_##u][STATE_##u]); \ - s2xi[STATE_##u][STATE_##u] = 0; \ - } - CHECK(P); - CHECK(V); - CHECK(A); - #undef CHECK - - /* partial determinats */ - #define CHECK(u, v) \ - uuvv = s2xi[STATE_##u][STATE_##u] * s2xi[STATE_##v][STATE_##v]; \ - uvvu = s2xi[STATE_##u][STATE_##v] * s2xi[STATE_##v][STATE_##u]; \ - if (uuvv < uvvu) { \ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): det(src.s2x[%u], " #u ", " #v ") = %g < 0"), src, i, uuvv - uvvu); \ - s2xi[STATE_##u][STATE_##v] = \ - s2xi[STATE_##v][STATE_##u] *= sqrt(uuvv / uvvu); \ - } - CHECK(P, V); - CHECK(P, A); - CHECK(A, V); - #undef CHECK - } -} - -void fused_engine_init(motion_changed_cb on_motion_changed, - predicted_updated_cb on_location_changed, gpointer handler) -{ - LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("(motion_cb=%p, location_cb=%p, interval=%gs, handler=%p)"), - on_motion_changed, on_location_changed, handler); - - fl_accel_init(on_motion_changed, handler); - fl_gyro_init(); - __init_time_offset(); - __started = FALSE; - __on_location_changed = on_location_changed; - __handler = handler; - fl_kalf_reset(); - LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()")); -} - -void fused_engine_exit(void) -{ - LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); - __exit_time_offset(); - fl_gyro_exit(); - fl_accel_exit(); - LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0); -} - -void fused_engine_start(void) -{ - if (__on_location_changed) { - if (__started == FALSE) { - __started = TRUE; - LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("(): OK"), 0); - return; - } else { - LOG_FUSED_DEV(DBG_ERR, LEAVE_FUNCTION_PREFIX("ALREADY_STARTED")); - return; - } - } else { - LOG_FUSED_DEV(DBG_ERR, LEAVE_FUNCTION_PREFIX("UNINITIALIZED")); - return; - } -} - -void fused_engine_stop(void) -{ - if (__started) { - __started = FALSE; - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(): OK"), 0); - } else { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(): E_NOT_STARTED"), 0); - } -} - -void fl_kalf_reset(void) -{ - unsigned i; - - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - __last_notification.time = FL_UNDEFINED_TIME; - __time_of.sensor_event.first = FL_UNDEFINED_TIME; - __time_of.sensor_event.last = FL_UNDEFINED_TIME; - __time_of.sensor_event.count = 0; - __time_of.last_measurement = FL_UNDEFINED_TIME; - __time_of.last_p_fixing = FL_UNDEFINED_TIME; - __time_of.last_v_fixing = FL_UNDEFINED_TIME; - __time_of.last_a_fixing = FL_UNDEFINED_TIME; - __time_of.last_r_fixing = FL_UNDEFINED_TIME; - __time_of.last_r_diffusion = FL_UNDEFINED_TIME; - __time_of.last_w_fixing = FL_UNDEFINED_TIME; -#if (FL_KALMAN_ANGULAR_VELOCITY) - __time_of.last_wz_fixing = FL_UNDEFINED_TIME; - __time_of.last_b_fixing = FL_UNDEFINED_TIME; -#endif - __time_of.last_reference = FL_UNDEFINED_TIME; - - memset(&__sp, 0, sizeof(__sp)); - memset(&__se, 0, sizeof(__se)); - memset(__wp, 0, sizeof(__wp)); - memset(__we, 0, sizeof(__we)); - memset(&__sre, 0, sizeof(__sre)); - memset(&__srp, 0, sizeof(__srp)); - __srp.w2[GEO_SPATIAL_X] = M_1_PI / 3; - __srp.w2[GEO_SPATIAL_Y] = M_1_PI / 3; - __srp.w2[GEO_SPATIAL_Z] = M_1_PI / 3; - __sre.w2[GEO_SPATIAL_X] = M_1_PI / 3; - __sre.w2[GEO_SPATIAL_Y] = M_1_PI / 3; - __sre.w2[GEO_SPATIAL_Z] = M_1_PI / 3; - __s2wp = DEFAULT_ROTATION_SIGMA2; - __s2we = DEFAULT_ROTATION_SIGMA2; - __wze = 0; -#if (FL_KALMAN_ANGULAR_VELOCITY) - __s2wze = DEFAULT_ROTATION_SIGMA2; - __be = 0; -#endif - - memset(&__te, 0, sizeof(__te)); - __sp.x[GEO_SPATIAL_Z][STATE_P] = EARTH_RADIUS; - __se.x[GEO_SPATIAL_Z][STATE_P] = EARTH_RADIUS; - for (i = GEO_SPATIAL_DIMENSION; i;) { - --i; - /* diagonal terms */ - __sp.s2x[i][STATE_P][STATE_P] = INITIAL_POSITION_SIGMA2; - __sp.s2x[i][STATE_V][STATE_V] = INITIAL_VELOCITY_SIGMA2; - __sp.s2x[i][STATE_A][STATE_A] = INITIAL_ACCELERATION_SIGMA2; - __se.s2x[i][STATE_P][STATE_P] = INITIAL_POSITION_SIGMA2; - __se.s2x[i][STATE_V][STATE_V] = INITIAL_VELOCITY_SIGMA2; - __se.s2x[i][STATE_A][STATE_A] = INITIAL_ACCELERATION_SIGMA2; - __sre.m[i][i] = 1; - __srp.m[i][i] = 1; - __te.fsr[i][i] = 1; - } - __te.crx = 1; - /* pass-throughs */ - __fix_status = LOCATION_STATUS_NO_FIX; -} - -PRIVATE fl_seconds __timestamp_to_seconds(fl_timestamp timestamp) -{ - __time_of.last_measurement = fl_timestamp_to_seconds(timestamp); - if (__time_of.sensor_event.last != FL_UNDEFINED_TIME) { - fl_seconds dt; - - dt = __time_of.last_measurement - __get_time_offset() - __time_of.sensor_event.last; - if (dt < 0) { - __new_time_offset(__time_of.last_measurement - __time_of.sensor_event.last); - dt = 0; - } else if (__time_of.sensor_event.count > 1) { - fl_seconds mean_sensor_dt = (__time_of.sensor_event.last - __time_of.sensor_event.first) / (__time_of.sensor_event.count - 1); - - if (dt > mean_sensor_dt) { - __new_time_offset(__time_of.last_measurement - __time_of.sensor_event.last - mean_sensor_dt); - dt = mean_sensor_dt; - } - } - return __time_of.sensor_event.last + dt; - } - return __time_of.last_measurement; -} - -PRIVATE void __matrix_3d_multiply_matrix_3d(const_fl_matrix_3d_ref m_1, const_fl_matrix_3d_ref m_2, fl_matrix_3d_ref out) -{ - unsigned i, j; - - i = GEO_SPATIAL_DIMENSION; - do { - --i; - j = GEO_SPATIAL_DIMENSION; - do { - --j; - out[i][j] = m_1[i][GEO_SPATIAL_X] * m_2[GEO_SPATIAL_X][j] - + m_1[i][GEO_SPATIAL_Y] * m_2[GEO_SPATIAL_Y][j] - + m_1[i][GEO_SPATIAL_Z] * m_2[GEO_SPATIAL_Z][j]; - } while (j); - } while (i); -} - -PRIVATE void __create_push_forward_map(_fl_kalf_state_matrix_ref x, LocationStatus status) -{ - fl_real crx, srx; - fl_real cry, sry; - fl_real yz2, r2; - fl_vector_3d p; - fl_vector_3d v; - - /* pull-back: from the old tangent frame to global coordinates */ - p[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; - p[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; - p[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; - v[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; - v[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; - v[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; - - yz2 = fl_square(p[GEO_SPATIAL_Y]) + fl_square(p[GEO_SPATIAL_Z]); - r2 = yz2 + fl_square(p[GEO_SPATIAL_X]); - if (r2 > PRECISION2) { - fl_real _r = 1.0 / sqrt(r2); - fl_real yz = sqrt(yz2); - - __te.rx = atan2(p[GEO_SPATIAL_X], yz); - __te.srx = srx = _r * p[GEO_SPATIAL_X]; - __te.crx = crx = _r * yz; - if (yz > PRECISION) { - fl_real _yz = 1.0 / yz; - - __te.ry = atan2(p[GEO_SPATIAL_Y], p[GEO_SPATIAL_Z]); - sry = _yz * p[GEO_SPATIAL_Y]; - cry = _yz * p[GEO_SPATIAL_Z]; - } else { -UndefinedLongitude: - __te.ry = 0; - sry = 0; - cry = 1; - } - } else { - __te.rx = 0; - __te.srx = srx = 0; - __te.crx = crx = 1; - goto UndefinedLongitude; - } - /* new push-forward map */ - __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] = crx; - __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] = 0; - __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] = srx; - - __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] = -sry * srx; - __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] = cry; - __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] = sry * crx; - - __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] = -cry * srx; - __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] = -sry; - __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] = cry * crx; - /* push-forward: from the global onto the new tangent frame */ - x[GEO_SPATIAL_X][STATE_P] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z]; - x[GEO_SPATIAL_Y][STATE_P] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z]; - x[GEO_SPATIAL_X][STATE_V] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z]; - x[GEO_SPATIAL_Y][STATE_V] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z]; - x[GEO_SPATIAL_Z][STATE_P] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z]; - x[GEO_SPATIAL_Z][STATE_V] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z]; -} - -PRIVATE void __spherical_to_tangent( - const fl_spherical_position *pos, - const fl_tangent_velocity *vel, - const fl_sigma *sigma, - fl_position_vector_ref pm, - fl_velocity_vector_ref vm, - fl_vector_3d_ref s2pm, - fl_vector_3d_ref s2vm) -{ - fl_real rp, rx, ry, rb; - fl_real crx, srx; - fl_position_vector pg; - int spatial; - - spatial = (pos->status == LOCATION_STATUS_3D_FIX); - /* pass-through */ - __fix_status = pos->status; - /* global coordinates */ - rx = fl_degrees_to_radians(pos->latitude); - ry = fl_degrees_to_radians(pos->longitude); - rp = spatial ? EARTH_RADIUS + pos->altitude : __se.x[GEO_SPATIAL_Z][STATE_P]; - srx = rp * sin(rx); - crx = rp * cos(rx); - pg[GEO_SPATIAL_X] = srx; - pg[GEO_SPATIAL_Y] = crx * sin(ry); - pg[GEO_SPATIAL_Z] = crx * cos(ry); - /* push-forward map: new position onto the current tangent space */ - pm[GEO_SPATIAL_X] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z]; - pm[GEO_SPATIAL_Y] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z]; - pm[GEO_SPATIAL_Z] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z]; - - fl_real cd, cd2, sd2; - cd = pm[GEO_SPATIAL_Z] / rp; - cd2 = SQUARE(cd); - sd2 = 1 - cd2; - if (sd2 < 0) { - cd2 = 1; - sd2 = 0; - } - /* Notice: in principle we should rotate the diagonal sigma matrix here (two - * matrix multiplications). This approximation assumes the old and new points - * are close. */ - if (sigma->of_altitude > 0) { - fl_real s2h, s2v; - - s2h = fl_square(sigma->of_horizontal_pos); - s2v = fl_square(sigma->of_altitude); - s2pm[GEO_SPATIAL_X] = - s2pm[GEO_SPATIAL_Y] = cd2 * s2h + sd2 * s2v; - s2pm[GEO_SPATIAL_Z] = sd2 * s2h + cd2 * s2v; - } else { - s2pm[GEO_SPATIAL_X] = - s2pm[GEO_SPATIAL_Y] = - s2pm[GEO_SPATIAL_Z] = fl_square(sigma->of_horizontal_pos); - } - - if (vel && vm) { - fl_velocity vl; /* velocity vector length */ - - /* The velocity is given in the tangent frame of p (which is different than - * the currecnt tangent one). In principle we should pull v back into the - * global frame using p as the transformation generator, and then push it - * forward onto our frame. Except first time when the two frames are - * randomly far apart this composition is almost always near-identity, thus - * can safely be approximated with tiny correction to the bearing. - */ - rb = fl_degrees_to_radians(vel->direction) - - atan2(pm[GEO_SPATIAL_Y] * __te.srx, - -pm[GEO_SPATIAL_X] * __te.srx + EARTH_RADIUS * __te.crx); - vl = fl_km_h_to_m_s(vel->speed); - vm[GEO_SPATIAL_X] = vl * cos(rb); - vm[GEO_SPATIAL_Y] = vl * sin(rb); - vm[GEO_SPATIAL_Z] = spatial ? fl_km_h_to_m_s(vel->climb) : __se.x[GEO_SPATIAL_Z][STATE_V]; - if (sigma->of_climb > 0) { - s2vm[GEO_SPATIAL_X] = - s2vm[GEO_SPATIAL_Y] = (cd2 * fl_square(sigma->of_speed) + sd2 * fl_square(sigma->of_climb)); - s2vm[GEO_SPATIAL_Z] = (sd2 * fl_square(sigma->of_speed) + cd2 * fl_square(sigma->of_climb)); - } else { - s2vm[GEO_SPATIAL_X] = - s2vm[GEO_SPATIAL_Y] = - s2vm[GEO_SPATIAL_Z] = fl_square(sigma->of_speed); - } - } -} - -PRIVATE void __tangent_to_spherical( - const _fl_kalf_state_matrix_ref x, - const _fl_kalf_sigma_cube_ref s2x, - fl_spherical_position* pos, - fl_tangent_velocity* vel, - fl_sigma* sigma) -{ - if (pos) { - fl_position_vector p; - - /* pull-back map: from the tangent frame to global coordinates */ - p[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; - p[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; - p[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; - - fl_real yz2, yz; - fl_real xyz2; - fl_radians rx; - fl_radians ry; - - yz2 = fl_square(p[GEO_SPATIAL_Y]) + fl_square(p[GEO_SPATIAL_Z]); - yz = sqrt(yz2); - if (yz2 > PRECISION2) - ry = atan2(p[GEO_SPATIAL_Y], p[GEO_SPATIAL_Z]); - else - ry = 0; /* avoid #IND values */ - xyz2 = yz2 + fl_square(p[GEO_SPATIAL_X]); - if (xyz2 > PRECISION2) - rx = atan2(p[GEO_SPATIAL_X], yz); - else - rx = 0; /* avoid #IND values */ - pos->timestamp = fl_seconds_to_timestamp(__last_notification.time > FL_UNDEFINED_TIME ? __last_notification.time : 0); - pos->latitude = fl_radians_to_latitude(rx); - pos->longitude = fl_radians_to_longitude(ry); - pos->altitude = sqrt(xyz2) - EARTH_RADIUS; - pos->status = __fix_status; - - fl_real rb; - fl_real u[PLANAR_DIMENSION]; - - u[PLANAR_X] = EARTH_RADIUS * __te.crx - x[GEO_SPATIAL_X][STATE_P] * __te.srx; - u[PLANAR_Y] = - x[GEO_SPATIAL_Y][STATE_P] * __te.srx; - rb = -atan2(x[GEO_SPATIAL_X][STATE_V] * u[PLANAR_Y] - x[GEO_SPATIAL_Y][STATE_V] * u[PLANAR_X], - x[GEO_SPATIAL_X][STATE_V] * u[PLANAR_X] + x[GEO_SPATIAL_Y][STATE_V] * u[PLANAR_Y]); - - if (vel) { - vel->timestamp = pos->timestamp; - vel->speed = fl_m_s_to_km_h(sqrt(fl_square(x[GEO_SPATIAL_Y][STATE_V]) + fl_square(x[GEO_SPATIAL_X][STATE_V]))); - vel->direction = fl_radians_to_cog(rb); - vel->climb = fl_m_s_to_km_h(x[GEO_SPATIAL_Z][STATE_V]); - } - if (sigma) { - sigma->level = LOCATION_ACCURACY_LEVEL_NONE; - sigma->of_horizontal_pos = sqrt((s2x[GEO_SPATIAL_X][STATE_P][STATE_P] + s2x[GEO_SPATIAL_Y][STATE_P][STATE_P]) * 0.5); - sigma->of_altitude = s2x[GEO_SPATIAL_Z][STATE_P][STATE_P]; - sigma->of_speed = sqrt((s2x[GEO_SPATIAL_X][STATE_V][STATE_V] + s2x[GEO_SPATIAL_Y][STATE_V][STATE_V]) * 0.5); - sigma->of_climb = s2x[GEO_SPATIAL_Z][STATE_V][STATE_V]; - } - } -} - -PRIVATE void __kalman_2d1o(fl_seconds t, _fl_kalf_state_component s, const_fl_vector_3d_ref xm, const_fl_vector_3d_ref s2xm) -{ - unsigned i; - - /* @note WPS-specific: restrict the update to horizontal position */ - i = PLANAR_DIMENSION; - do { - --i; - const_fl_vector_3d_ref s2pis; - _fl_kalf_state_matrix_ref s2ei; - _fl_kalf_state_matrix_ref s2pi; - fl_real s2k, s2mi, s2piss; - - s2mi = s2xm[i]; - s2ei = __se.s2x[i]; - s2pi = __sp.s2x[i]; - s2pis = s2pi[s]; - s2piss = s2pis[s]; - s2k = s2mi + s2piss; - if (s2k > PRECISION2) { - fl_real inv_s2k; - fl_real dx; - - inv_s2k = 1.0 / s2k; - dx = xm[i] - __sp.x[i][s]; - - /* reduced Joseph form in special 3x1 case: subtract scaled projection */ - #define SET_FIELD(a, b) s2ei[a][b] = inv_s2k * ((s2pi[a][b] * s2piss - s2pis[a] * s2pis[b]) + s2pi[a][b] * s2mi) - #define CPY_FIELD(a, b) s2ei[a][b] = s2ei[b][a] - SET_FIELD(STATE_P, STATE_P); - SET_FIELD(STATE_P, STATE_V); - SET_FIELD(STATE_P, STATE_A); - CPY_FIELD(STATE_V, STATE_P); - SET_FIELD(STATE_V, STATE_V); - SET_FIELD(STATE_V, STATE_A); - CPY_FIELD(STATE_A, STATE_P); - CPY_FIELD(STATE_A, STATE_V); - SET_FIELD(STATE_A, STATE_A); - #undef CPY_FIELD - #undef SET_FIELD - - #define SET_FIELD(a) __se.x[i][a] = __sp.x[i][a] + dx * (inv_s2k * s2pis[a]) - SET_FIELD(STATE_P); - SET_FIELD(STATE_V); - SET_FIELD(STATE_A); - #undef SET_FIELD - } - } while (i); - /* the third coordinate is of `special care': we want to preserve it in the tangent frame, along with its sigma */ - if (s == STATE_P) - __se.x[GEO_SPATIAL_Z][STATE_P] = xm[GEO_SPATIAL_Z]; - - __copy_state(&__sp, &__se); - __time_of.last_reference = t; -} - -/* Update of the 2D position */ -PRIVATE void __kalman_p(fl_seconds t, const_fl_position_vector_ref pm, const_fl_vector_3d_ref s2pm) -{ - __predict_pv(t); - __kalman_2d1o(t, STATE_P, pm, s2pm); - __time_of.last_p_fixing = t; - __time_of.last_reference = t; -} - -/* Update of the 3D position & velocity */ -PRIVATE void __kalman_pv( - fl_seconds t, - const_fl_position_vector_ref pm, /* p-measurement */ - const_fl_velocity_vector_ref vm, /* v-measurement */ - const_fl_vector_3d_ref s2pm, /* p-measurement variance */ - const_fl_vector_3d_ref s2vm /* v-measurement variance */ -) { - unsigned i; - - __predict_pv(t); - for (i = GEO_SPATIAL_DIMENSION; i;) { - --i; - - _fl_kalf_matrix_2d s2k; - fl_real det; - /* shortcuts */ - _const_fl_kalf_sigma_matrix_ref s2xpi = __sp.s2x[i]; - const fl_real *xpi = __sp .x[i]; - - /* Sk = Sp + Sm */ - s2k[STATE_P][STATE_P] = s2xpi[STATE_P][STATE_P] + s2pm[i]; - s2k[STATE_P][STATE_V] = /* == s2pi[STATE_P][STATE_V] */ - s2k[STATE_V][STATE_P] = s2xpi[STATE_V][STATE_P]; - s2k[STATE_V][STATE_V] = s2xpi[STATE_V][STATE_V] + s2vm[i]; - det = s2k[STATE_P][STATE_P] * s2k[STATE_V][STATE_V] - s2k[STATE_V][STATE_P] * s2k[STATE_P][STATE_V]; - if (fl_square(det) > PRECISION2) { - fl_real denom; - _fl_kalf_matrix_2d s2i; - _fl_kalf_matrix_2d k0, k1; - - /* Si = Sk^{-1} */ - denom = 1.0 / det; - s2i[STATE_P][STATE_P] = denom * s2k[STATE_V][STATE_V]; - s2i[STATE_P][STATE_V] = - s2i[STATE_V][STATE_P] = -denom * s2k[STATE_V][STATE_P]; - s2i[STATE_V][STATE_V] = denom * s2k[STATE_P][STATE_P]; - - /* K = Sp.Si */ - #define SET_K0(j, k) k0[j][k] = s2xpi[j][STATE_P] * s2i[STATE_P][k] + s2xpi[j][STATE_V] * s2i[STATE_V][k] - SET_K0(STATE_P, STATE_P); - SET_K0(STATE_P, STATE_V); - SET_K0(STATE_V, STATE_P); - SET_K0(STATE_V, STATE_V); - #undef SET_K0 - /* 1 - K = Sm.Si, use positive form */ - k1[STATE_P][STATE_P] = s2pm[i] * s2i[STATE_P][STATE_P]; - k1[STATE_P][STATE_V] = s2pm[i] * s2i[STATE_P][STATE_V]; - k1[STATE_V][STATE_P] = s2vm[i] * s2i[STATE_V][STATE_P]; - k1[STATE_V][STATE_V] = s2vm[i] * s2i[STATE_V][STATE_V]; - /* state update */ - __se.x[i][STATE_P] = k0[STATE_P][STATE_P] * pm[i] + k0[STATE_P][STATE_V] * vm[i] - + k1[STATE_P][STATE_P] * xpi[STATE_P] + k1[STATE_P][STATE_V] * xpi[STATE_V]; - __se.x[i][STATE_V] = k0[STATE_V][STATE_P] * pm[i] + k0[STATE_V][STATE_V] * vm[i] - + k1[STATE_V][STATE_P] * xpi[STATE_P] + k1[STATE_V][STATE_V] * xpi[STATE_V]; - /* covariance update Se = K1,K1.Sp + K,K.Sm */ - _fl_kalf_sigma_matrix_ref s2ei = __se.s2x[i]; - - #define SET_S2E(j, k) s2ei[j][k] = \ - k1[j][STATE_P] * k1[k][STATE_P] * s2xpi[STATE_P][STATE_P] + k0[j][STATE_P] * k0[k][STATE_P] * s2pm[i] \ - + k1[j][STATE_P] * k1[k][STATE_V] * s2xpi[STATE_P][STATE_V] \ - + k1[j][STATE_V] * k1[k][STATE_P] * s2xpi[STATE_V][STATE_P] \ - + k1[j][STATE_V] * k1[k][STATE_V] * s2xpi[STATE_V][STATE_V] + k0[j][STATE_V] * k0[k][STATE_V] * s2vm[i] - - SET_S2E(STATE_P, STATE_P); - SET_S2E(STATE_P, STATE_V); - SET_S2E(STATE_V, STATE_P); - SET_S2E(STATE_V, STATE_V); - #undef SET_S2E - } - /* upon arrival of new position-velocity pair (a maximally uncertain) the pair (p, v) decorrelates from a: */ - __se.s2x[i][STATE_P][STATE_A] = 0; - __se.s2x[i][STATE_V][STATE_A] = 0; - __se.s2x[i][STATE_A][STATE_P] = 0; - __se.s2x[i][STATE_A][STATE_V] = 0; - __se.s2x[i][STATE_A][STATE_A] = __sp.s2x[i][STATE_A][STATE_A]; - } - __copy_state(&__sp, &__se); - __time_of.last_p_fixing = t; - __time_of.last_v_fixing = t; - __time_of.last_reference = t; -} - -#if (FL_KALMAN_ANGULAR_VELOCITY) -void __kalman_wz(fl_seconds t, fl_real wzm, fl_real s2wzm) -{ - fl_real s2wzp; - - if (__time_of.last_wz_fixing > FL_UNDEFINED_TIME) { - fl_real dt = t - __time_of.last_wz_fixing; - fl_real s2wzk; - #define wzp __wze /* constant prediction */ - - /* propagate/diffuse sigma^2 */ - s2wzp = __s2wze + dt * SQUARE(FL_DEFAULT_SPIN_SIGMA); - s2wzk = s2wzp + s2wzm; - if (s2wzk > PRECISION2) { - fl_real inv_s2k; - - inv_s2k = 1.0 / s2wzk; - LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=" TIME_FORMAT ", w=%.4g, s2w=%.4g): __wze:%g->%g, __s2wze:%g->%g"), - t, wzm, s2wzm, __wze, inv_s2k * (s2wzp * wzm + s2wzm * wzp), - __s2wze, inv_s2k * s2wzp * s2wzm * 2); - __s2wze = inv_s2k * s2wzp * s2wzm * 2; - __wze = inv_s2k * (s2wzp * wzm + s2wzm * wzp); - } else { - __wze = wzm; - __s2wze = s2wzm; - } - #undef wzp - } else { - __wze = wzm; - __s2wze = s2wzm; - } - __time_of.last_wz_fixing = t; -} -#endif - -/* Blend two unit 3D vectors usign inverse covariances as weights (to avoid singularities) */ -PRIVATE void __inv_kalman_u( - fl_seconds t, - const_fl_vector_3d_ref u1, fl_real w2u1, - const_fl_vector_3d_ref u2, fl_real w2u2, - fl_vector_3d_ref out, fl_real *w2out) -{ - unsigned i; - - *w2out = 0.5 * (w2u1 + w2u2); - if (*w2out > PRECISION2) { - fl_real e2 = 0; - fl_real _w2out = 1.0 / (*w2out); - - i = GEO_SPATIAL_DIMENSION; - do { - fl_real e; - --i; - out[i] = e = _w2out * (u1[i] * w2u1 + u2[i] * w2u2); - e2 += SQUARE(e); - } while (i); - - if (e2 > PRECISION2) { - fl_real _e; - - /* normalize to unity */ - _e = 1.0 / sqrt(e2); - i = GEO_SPATIAL_DIMENSION; - do - out[--i] *= _e; - while (i); - } - } else { - i = GEO_SPATIAL_DIMENSION; - do - out[--i] = 0; - while (i); - } -} - -/* 3D cross-product with normalization */ -PRIVATE void __cross_product(const_fl_vector_3d_ref in1, const_fl_vector_3d_ref in2, fl_vector_3d_ref out) -{ - fl_real out2; - - /* The cross-product preserves system handedness: it's right-handed in the - * right-handed system, and left-handed in the left-handed one. - */ - VECTOR_CROSS_VECTOR_3D(in1, in2, out); - - out2 = fl_square(out[GEO_SPATIAL_X]) - + fl_square(out[GEO_SPATIAL_Y]) - + fl_square(out[GEO_SPATIAL_Z]); - if (fl_square(out2 - 1) > PRECISION2) { - fl_real _out = 1.0 / sqrt(out2); - /* normalize */ - out[GEO_SPATIAL_X] *= _out; - out[GEO_SPATIAL_Y] *= _out; - out[GEO_SPATIAL_Z] *= _out; - } -} - -PRIVATE void __signal_updated_location(fl_seconds t) -{ - if (FL_MIN_NOTIFICATION_FILTRATION && __started && __time_of.last_p_fixing > FL_UNDEFINED_TIME) { - t += __get_time_offset(); - if ((t - __last_notification.time) * 1000 >= FL_MIN_NOTIFICATION_INTERVAL) { - __get_prediction_vector(t, &__last_notification); - __on_location_changed(&__last_notification, __handler); - } - } -} - -/* Filter input: Enter the raw position measurement */ -void fused_engine_process_position_2d_event(const fl_spherical_position* pos, const fl_sigma* sigma) -{ - if (pos && sigma) { - if (pos->status >= LOCATION_STATUS_2D_FIX) { - fl_seconds t; - fl_position_vector pm; /* p-measurement */ - fl_vector_3d s2pm; /* p-measurement variance */ - - t = __timestamp_to_seconds(pos->timestamp); - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g�, %g�), sp=%gm)"), - t, - pos->latitude, - pos->longitude, - sigma->of_horizontal_pos); - __spherical_to_tangent(pos, NULL, sigma, pm, NULL, s2pm, NULL); - /* Kalman merging of 2D position */ - __kalman_p(t, pm, s2pm); - __create_push_forward_map(__se.x, pos->status); - memcpy(__sp.x, __se.x, sizeof(__sp.x)); - __signal_updated_location(t); - } else { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(location.pos.status=%d)"), pos->status); - } - } else { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%p, %p): INVALID ARGUMENT"), pos, sigma); - } -} - -/* Filter input: Enter the raw position measurement */ -void fused_engine_process_position_2x3d_event(const fl_spherical_position *pos, const fl_tangent_velocity *vel, const fl_sigma *sigma) -{ - if (pos && vel && sigma) { - if (pos->status >= LOCATION_STATUS_2D_FIX) { - fl_seconds t; - fl_position_vector pm; /* p-measurement */ - fl_velocity_vector vm; /* v-measurement */ - fl_vector_3d s2pm; /* p-measurement variance */ - fl_vector_3d s2vm; /* v-measurement variance */ - - t = __timestamp_to_seconds(pos->timestamp); - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g�, %g�, %gm), cog=%g�, sog=%gkm/h, climb=%gkm/h, sp=%gm, sv=%gm/s)"), - t, - pos->latitude, - pos->longitude, - pos->altitude, - vel->direction, - vel->speed, - vel->climb, - sigma->of_horizontal_pos, - sigma->of_speed); - __spherical_to_tangent(pos, vel, sigma, pm, vm, s2pm, s2vm); - fl_real vh = fl_km_h_to_m_s(vel->speed); - #if (FL_KALMAN_ANGULAR_VELOCITY) - fl_real vhe = sqrt(fl_square(__se.x[GEO_SPATIAL_X][STATE_V]) + fl_square(__se.x[GEO_SPATIAL_Y][STATE_V])); - fl_real b = fl_degrees_to_radians(vel->direction); - if (vh > PRECISION && __time_of.last_b_fixing > FL_UNDEFINED_TIME) { - fl_real dt = t - __time_of.last_b_fixing; - if (vh * vhe * dt > 0) { - fl_radians db; - fl_real wzm, s2wzm; - - /* circular difference */ - db = b - __be; - if (db > M_PI) - db -= M_2PI; - else if (db < -M_PI) - db += M_2PI; - - wzm = db / dt; - s2wzm = 4 * fl_square(atan2(2 * sqrt(vh * vhe), FL_DEFAULT_VELOCITY_SIGMA)); - __kalman_wz(t, wzm, s2wzm); - } - } - __be = b; - __time_of.last_b_fixing = t; - #endif - __kalman_pv(t, pm, vm, s2pm, s2vm); - __create_push_forward_map(__se.x, pos->status); - memcpy(__sp.x, __se.x, sizeof(__sp.x)); - __signal_updated_location(t); - } else { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(location.pos.status=%d)"), pos->status); - } - } else { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%p, %p): INVALID ARGUMENT"), pos, sigma); - } -} - -void fused_engine_get_location(fl_spherical_position* pos, fl_tangent_velocity* vel, fl_sigma* sigma) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(), __last_notification.time=%gs"), __last_notification.time); - __tangent_to_spherical(__sp.x, __sp.s2x, pos, vel, sigma); -} - -PRIVATE void __get_prediction_vector(fl_seconds t, fl_position_4d* prediction) -{ - prediction->time = t; - /* pull-back map: from the old tangent frame back to global coordinates */ - prediction->spatial[GEO_SPATIAL_X] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; - prediction->spatial[GEO_SPATIAL_Y] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; - prediction->spatial[GEO_SPATIAL_Z] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; -} - -fl_real fl_kalf_get_distance(const_fl_position_vector_ref p, const_fl_position_vector_ref q) -{ - return sqrt(fl_square(p[GEO_SPATIAL_X] - q[GEO_SPATIAL_X]) - + fl_square(p[GEO_SPATIAL_Y] - q[GEO_SPATIAL_Y]) - + fl_square(p[GEO_SPATIAL_Z] - q[GEO_SPATIAL_Z])); -} - -PRIVATE void __quaternion_to_rotation(_const_fl_kalf_quaternion_ref q, fl_matrix_3d_ref out) -{ - out[GEO_SPATIAL_X][GEO_SPATIAL_X] = SQUARE(q[QTR_0]) + SQUARE(q[QTR_X]) - SQUARE(q[QTR_Y]) - SQUARE(q[QTR_Z]); - out[GEO_SPATIAL_X][GEO_SPATIAL_Y] = 2 * (q[QTR_X] * q[QTR_Y] + q[QTR_Z] * q[QTR_0]); - out[GEO_SPATIAL_X][GEO_SPATIAL_Z] = 2 * (q[QTR_X] * q[QTR_Z] - q[QTR_Y] * q[QTR_0]); - - out[GEO_SPATIAL_Y][GEO_SPATIAL_X] = 2 * (q[QTR_Y] * q[QTR_X] - q[QTR_Z] * q[QTR_0]); - out[GEO_SPATIAL_Y][GEO_SPATIAL_Y] = SQUARE(q[QTR_0]) - SQUARE(q[QTR_X]) + SQUARE(q[QTR_Y]) - SQUARE(q[QTR_Z]); - out[GEO_SPATIAL_Y][GEO_SPATIAL_Z] = 2 * (q[QTR_Y] * q[QTR_Z] + q[QTR_X] * q[QTR_0]); - - out[GEO_SPATIAL_Z][GEO_SPATIAL_X] = 2 * (q[QTR_Z] * q[QTR_X] + q[QTR_Y] * q[QTR_0]); - out[GEO_SPATIAL_Z][GEO_SPATIAL_Y] = 2 * (q[QTR_Z] * q[QTR_Y] - q[QTR_X] * q[QTR_0]); - out[GEO_SPATIAL_Z][GEO_SPATIAL_Z] = SQUARE(q[QTR_0]) - SQUARE(q[QTR_X]) - SQUARE(q[QTR_Y]) + SQUARE(q[QTR_Z]); -} - -//PRIVATE void __process_linear_acceleration(fl_seconds t, const_fl_acceleration_vector_ref al) -//{ -// unsigned i; -// -// for (i = GEO_SPATIAL_DIMENSION; i--;) { -// __se. x[i][STATE_A] = 0; -// __se.s2x[i][STATE_A][STATE_A] = DEFAULT_ACCELERATION_SIGMA2; -// } -// __predict_pv(t); -// __time_of.last_a_fixing = t; -// __signal_updated_location(t); -//} - -void fused_engine_process_sensory_event(fl_seconds t, fl_sensory_source sensor_id, const_sensor_vector_3d_ref data) -{ - if (__time_of.sensor_event.count == 0) - __time_of.sensor_event.first = t; - __time_of.sensor_event.last = t; - __time_of.sensor_event.count++; - - if (__time_of.last_measurement > FL_UNDEFINED_TIME) { - fl_seconds dt = t - (__time_of.last_measurement - __get_time_offset()); - if (dt < 0) - __new_time_offset(__time_of.last_measurement - t); - } - if (data) { - switch (sensor_id) { - case ACCELEROMETER: { - fl_acceleration_vector am; - fl_acceleration_vector al; - fl_real w2gu; - fl_vector_3d gu; - unsigned i; - - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", RAC, a=(%g, %g, %g)[m/s^2])"), t, data[DEV_SPATIAL_X], data[DEV_SPATIAL_Y], data[DEV_SPATIAL_Z]); - /* extrapolate the rotation matrix */ - __predict_rw(t); - - /* float, math system -> double, geographic system, rotated into local (tangent) frame */ - am[GEO_SPATIAL_X] = __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z]; - am[GEO_SPATIAL_Y] = __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z]; - am[GEO_SPATIAL_Z] = __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z]; - /* pass through gravity filter to estimate |g| and downward direction */ - fl_accel_process(t, am, al, gu, &w2gu); - if (w2gu > PRECISION2) { - fl_vector_3d gn; - - /* rotate back to device frame */ - VECTOR_DOT_MATRIX_3D(gu, __srp.m, gn); - /* w2gn = w2gu because w2gu is diagonal and srp orthogonal */ - /* update rotation matrix */ - __inv_kalman_u(t, gn, w2gu, __srp.m[GEO_SPATIAL_Z], __srp.w2[GEO_SPATIAL_Z], __sre.m[GEO_SPATIAL_Z], &__sre.w2[GEO_SPATIAL_Z]); - __cross_product(__sre.m[GEO_SPATIAL_Z], __srp.m[GEO_SPATIAL_X], __sre.m[GEO_SPATIAL_Y]); - __cross_product(__sre.m[GEO_SPATIAL_Y], __sre.m[GEO_SPATIAL_Z], __sre.m[GEO_SPATIAL_X]); - - __copy_system_rotation(&__srp, &__sre); - __time_of.last_r_fixing = t; - } - /* process linear acceleration */ - for (i = GEO_SPATIAL_DIMENSION; i--;) { - __se. x[i][STATE_A] = 0; - __se.s2x[i][STATE_A][STATE_A] = DEFAULT_ACCELERATION_SIGMA2; - } - __predict_pv(t); - __time_of.last_a_fixing = t; - __signal_updated_location(t); - // __process_linear_acceleration(t, al); - return; - } - case GYROSCOPE: { - fl_spin_vector wm; - - /* float, degrees, math/device coordinates (right-handed) -> double, radians, geographic/tangent coordinates (left-handed) */ - wm[GEO_SPATIAL_X] = -fl_degrees_to_radians(data[DEV_SPATIAL_X]); - wm[GEO_SPATIAL_Y] = -fl_degrees_to_radians(data[DEV_SPATIAL_Y]); - wm[GEO_SPATIAL_Z] = -fl_degrees_to_radians(data[DEV_SPATIAL_Z]); - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", GYR, w=(%g, %g, %g)[rad/s])"), t, wm[GEO_SPATIAL_X], wm[GEO_SPATIAL_Y], wm[GEO_SPATIAL_Z]); - __integrate_rw(t, fl_gyro_process(t, wm), DEFAULT_ROTATION_SIGMA2); - return; - } - default: - /* prevent compiler warning */ - return; - } - } - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(t=" TIME_FORMAT ", sensor_id=%d, data=%p)"), t, sensor_id, data); -} - -PRIVATE void __integrate_rw(fl_seconds t, const_fl_spin_vector_ref wm, fl_real s2wm) -{ - unsigned i; - - if (__time_of.last_w_fixing > FL_UNDEFINED_TIME) { - fl_seconds dt; - - dt = t - __time_of.last_w_fixing; - if (dt > 0) { - fl_real w2; - - __wp[GEO_SPATIAL_X] = 0.5 * (__we[GEO_SPATIAL_X] + wm[GEO_SPATIAL_X]); - __wp[GEO_SPATIAL_Y] = 0.5 * (__we[GEO_SPATIAL_Y] + wm[GEO_SPATIAL_Y]); - __wp[GEO_SPATIAL_Z] = 0.5 * (__we[GEO_SPATIAL_Z] + wm[GEO_SPATIAL_Z]); - w2 = fl_vector_3d_length2(__wp); - fl_real wz = __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_X] * __wp[GEO_SPATIAL_X] - + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * __wp[GEO_SPATIAL_Y] - + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * __wp[GEO_SPATIAL_Z]; - - #if (FL_KALMAN_ANGULAR_VELOCITY) - __kalman_wz(t, wz, SQUARE(FL_DEFAULT_SPIN_SIGMA)); - #else - __wze = wz; - #endif - __predict_pv(t); - __se.s2x[GEO_SPATIAL_X][STATE_A][STATE_A] = fl_square(__sp.x[GEO_SPATIAL_X][STATE_A] - __se.x[GEO_SPATIAL_X][STATE_A]); - __se.s2x[GEO_SPATIAL_Y][STATE_A][STATE_A] = fl_square(__sp.x[GEO_SPATIAL_Y][STATE_A] - __se.x[GEO_SPATIAL_Y][STATE_A]); - __copy_state(&__se, &__sp); - __time_of.last_reference = t; - - /* Integrate over dt: We use the fact that for any scalar t the quaternions - * [exp(w), exp(tw)] = 0 - * commute. - */ - fl_real dt2 = dt * dt / 24; - - if (w2 > PRECISION2) { - fl_real w; - fl_real wt, swtw; - _fl_kalf_quaternion qwt; - fl_matrix_3d dsr; - fl_real qwt2; - - w = sqrt(w2); - wt = w * dt * 0.5; - swtw = sin(wt) / w; /* system rotates opposite to the device */ - qwt[QTR_0] = cos(wt); - qwt[QTR_X] = swtw * __wp[GEO_SPATIAL_X] + dt2 * (__we[GEO_SPATIAL_Y] * wm[GEO_SPATIAL_Z] - __we[GEO_SPATIAL_Z] * wm[GEO_SPATIAL_Y]); - qwt[QTR_Y] = swtw * __wp[GEO_SPATIAL_Y] + dt2 * (__we[GEO_SPATIAL_Z] * wm[GEO_SPATIAL_X] - __we[GEO_SPATIAL_X] * wm[GEO_SPATIAL_Z]); - qwt[QTR_Z] = swtw * __wp[GEO_SPATIAL_Z] + dt2 * (__we[GEO_SPATIAL_X] * wm[GEO_SPATIAL_Y] - __we[GEO_SPATIAL_Y] * wm[GEO_SPATIAL_X]); - qwt2 = fl_square(qwt[QTR_0]) + fl_vector_3d_length2(&qwt[QTR_X]); - if (qwt2 > PRECISION2) { - fl_real norm_qwt = 1.0 / sqrt(qwt2); - - qwt[QTR_0] *= norm_qwt; - qwt[QTR_X] *= norm_qwt; - qwt[QTR_Y] *= norm_qwt; - qwt[QTR_Z] *= norm_qwt; - } - __quaternion_to_rotation(qwt, dsr); - __matrix_3d_multiply_matrix_3d(__srp.m, dsr, __sre.m); - __copy_system_rotation(&__srp, &__sre); - __time_of.last_r_fixing = t; - } - } - } - i = GEO_SPATIAL_DIMENSION; - do { - --i; - // Kalmaning the rotation rates would amount to an EMA low-pass filter - we don't want it - __wp[i] = __we[i] = wm[i]; - } while (i); - __s2we = s2wm; - __time_of.last_w_fixing = t; -} - -PRIVATE void __predict_rw(fl_seconds t) -{ - unsigned i; - - if (__time_of.last_w_fixing > FL_UNDEFINED_TIME) { - fl_seconds dt; - - dt = t - __time_of.last_w_fixing; - if (dt > 0) { - i = GEO_SPATIAL_DIMENSION; - do { - --i; - __wp[i] = __we[i]; - } while (i); - - /* Integrate over dt: We use the fact that for any scalar t the quaternions - * [exp(w), exp(tw)] = 0 - * commute. - */ - fl_real w2; - - w2 = fl_vector_3d_length2(__wp); - if (w2 > PRECISION2) { - fl_real w; - fl_real wt, swt; - _fl_kalf_quaternion qwt; - fl_matrix_3d srp; /* local copy */ - fl_matrix_3d dsr; - - w = sqrt(w2); - wt = w * dt * 0.5; - swt = sin(wt) / w; - qwt[QTR_0] = cos(wt); - qwt[QTR_X] = swt * __wp[GEO_SPATIAL_X]; - qwt[QTR_Y] = swt * __wp[GEO_SPATIAL_Y]; - qwt[QTR_Z] = swt * __wp[GEO_SPATIAL_Z]; - __quaternion_to_rotation(qwt, dsr); - /* inlined memcpy() */ - i = sizeof(__srp.m) / sizeof(fl_real); - do { - --i; - srp[0][i] = __srp.m[0][i]; - } while (i); - __matrix_3d_multiply_matrix_3d(srp, dsr, __srp.m); - } - } - } - /* otherwise there's no q change */ -} - -PRIVATE void __predict_pv(fl_seconds t) -{ - fl_seconds dt; - unsigned i; - - if (__time_of.last_reference > FL_UNDEFINED_TIME) - dt = t - __time_of.last_reference; - else - /* no prior data */ - return; - - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT "), dt=" TIME_FORMAT), t, dt); - if (dt > PRECISION2) { - fl_real wt = __wze * dt; - if (fabs(__wze) > PRECISION) { - fl_real cwt, swt; - fl_real cwtw, swtw; - fl_real inv_wz; - - cwt = cos(wt); - swt = sin(wt); - inv_wz = 1.0 / __wze; - cwtw = (cwt - 1) * inv_wz; - swtw = swt * inv_wz; - - __sp.x[GEO_SPATIAL_X][STATE_P] = __se.x[GEO_SPATIAL_X][STATE_V] * swtw + __se.x[GEO_SPATIAL_Y][STATE_V] * cwtw + __se.x[GEO_SPATIAL_X][STATE_P]; - __sp.x[GEO_SPATIAL_Y][STATE_P] = __se.x[GEO_SPATIAL_Y][STATE_V] * swtw - __se.x[GEO_SPATIAL_X][STATE_V] * cwtw + __se.x[GEO_SPATIAL_Y][STATE_P]; - __sp.x[GEO_SPATIAL_X][STATE_V] = __se.x[GEO_SPATIAL_X][STATE_V] * cwt - __se.x[GEO_SPATIAL_Y][STATE_V] * swt; - __sp.x[GEO_SPATIAL_Y][STATE_V] = __se.x[GEO_SPATIAL_Y][STATE_V] * cwt + __se.x[GEO_SPATIAL_X][STATE_V] * swt; - } else { - __sp.x[GEO_SPATIAL_X][STATE_P] = __se.x[GEO_SPATIAL_X][STATE_P] - dt * (wt * __se.x[GEO_SPATIAL_Y][STATE_V] * 0.5 - __se.x[GEO_SPATIAL_X][STATE_V]); - __sp.x[GEO_SPATIAL_Y][STATE_P] = __se.x[GEO_SPATIAL_Y][STATE_P] + dt * (wt * __se.x[GEO_SPATIAL_X][STATE_V] * 0.5 + __se.x[GEO_SPATIAL_Y][STATE_V]); - __sp.x[GEO_SPATIAL_X][STATE_V] = __se.x[GEO_SPATIAL_X][STATE_V] - wt * (wt * __se.x[GEO_SPATIAL_X][STATE_V] * 0.5 + __se.x[GEO_SPATIAL_Y][STATE_V]); - __sp.x[GEO_SPATIAL_Y][STATE_V] = __se.x[GEO_SPATIAL_Y][STATE_V] - wt * (wt * __se.x[GEO_SPATIAL_Y][STATE_V] * 0.5 - __se.x[GEO_SPATIAL_X][STATE_V]); - } - __sp.x[GEO_SPATIAL_Z][STATE_P] = __se.x[GEO_SPATIAL_Z][STATE_P] + dt * __se.x[GEO_SPATIAL_Z][STATE_V]; - __sp.x[GEO_SPATIAL_Z][STATE_V] = __se.x[GEO_SPATIAL_Z][STATE_V]; - - __sp.x[GEO_SPATIAL_X][STATE_A] = -__sp.x[GEO_SPATIAL_Y][STATE_V] * __wze; - __sp.x[GEO_SPATIAL_Y][STATE_A] = __sp.x[GEO_SPATIAL_X][STATE_V] * __wze; - __sp.x[GEO_SPATIAL_Z][STATE_A] = 0; - - for (i = GEO_SPATIAL_DIMENSION; i;) { - --i; - - _const_fl_kalf_sigma_matrix_ref s2ei = __se.s2x[i]; - _fl_kalf_sigma_matrix_ref s2pi = __sp.s2x[i]; - - s2pi[STATE_A][STATE_A] = s2ei[STATE_A][STATE_A]; - s2pi[STATE_A][STATE_V] = s2ei[STATE_A][STATE_V] - + s2ei[STATE_A][STATE_A] * dt; - s2pi[STATE_A][STATE_P] = s2ei[STATE_A][STATE_P] - + (s2ei[STATE_A][STATE_V] - + s2ei[STATE_A][STATE_A] * dt / 2) * dt; - - s2pi[STATE_V][STATE_A] = s2pi[STATE_A][STATE_V]; /* by symmetry */ - s2pi[STATE_V][STATE_V] = s2ei[STATE_V][STATE_V] - + (s2ei[STATE_V][STATE_A] * 2 - + s2ei[STATE_A][STATE_A] * dt) * dt; - s2pi[STATE_V][STATE_P] = s2ei[STATE_V][STATE_P] - + (s2ei[STATE_V][STATE_V] - + s2ei[STATE_P][STATE_A] - + (s2ei[STATE_V][STATE_A] * 3 - + s2ei[STATE_A][STATE_A] * dt) / 2 * dt) * dt; - - s2pi[STATE_P][STATE_A] = s2pi[STATE_A][STATE_P]; /* by symmetry */ - s2pi[STATE_P][STATE_V] = s2pi[STATE_V][STATE_P]; /* by symmetry */ - s2pi[STATE_P][STATE_P] = s2ei[STATE_P][STATE_P] - + (s2ei[STATE_P][STATE_V] * 2 - + (s2ei[STATE_V][STATE_V] - + s2ei[STATE_A][STATE_P] - + (s2ei[STATE_A][STATE_V] - + s2ei[STATE_A][STATE_A] * dt / 4) * dt) * dt) * dt; - } - // process noise - __sp.s2x[GEO_SPATIAL_X][STATE_A][STATE_A] += fl_square(__sp.x[GEO_SPATIAL_X][STATE_A] - __se.x[GEO_SPATIAL_X][STATE_A]); - __sp.s2x[GEO_SPATIAL_Y][STATE_A][STATE_A] += fl_square(__sp.x[GEO_SPATIAL_Y][STATE_A] - __se.x[GEO_SPATIAL_Y][STATE_A]); - __rectify_state(&__sp); - } -} diff --git a/lbs-server/src/fused/kalman-filter.h b/lbs-server/src/fused/kalman-filter.h deleted file mode 100644 index 42fde96..0000000 --- a/lbs-server/src/fused/kalman-filter.h +++ /dev/null @@ -1,619 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file kalman-filter.h - * @brief Implementation of a 4D (2D position + 2D velocity) Kalman filter - */ - -#pragma once -#ifndef __FL_KALMAN_FILTER_H__ -#define __FL_KALMAN_FILTER_H__ - -#include -#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/( - ^2) - * @param[in] w2v - * Inverse covariance of v: w2v = 1/( - ^2) - * - * @return - * Estimated weight of the product. - */ -PRIVATE INLINE fl_real __product_weights2(fl_real w2u, fl_real w2v) -{ - return w2u * w2v / (w2u + w2v); -} - -/***************************************************************************//** - * [private] 3D cross-product of two unit vectors. The resultant vector is - * normalized to unity, in case @a u1 and @a u2 are not orthogonal. - * - * @param[in] u1 - * Input unit vector - * @param[in] u2 - * Input unit vector - * @param[out] v - * Output unit vector - */ -PRIVATE void __cross_product(const_fl_vector_3d_ref u1, const_fl_vector_3d_ref u2, fl_vector_3d_ref v); - -/***************************************************************************//** - * [private] Create coordinate map from spherical M = S^2 x R to the tangent - * space TM. Notice that only two angles are being used, what means the course - * on the tangent plane is not straight North (x-axis), but still described by - * the bearing. - * - * @param[in] x - * Position and velocity in old tangent frame - * @param[out] status - * Status of the supplied data - */ -PRIVATE void __create_push_forward_map(_fl_kalf_state_matrix_ref x, LocationStatus status); - -/***************************************************************************//** - * [private] Convert API location to internal representation, i.e. perform - * conversion of: - * - units to SI; - * - coordinate system M -> TM. - * - * @param[in] pos - * Position in API format - * @param[in] vel - * Velocity in API format - * @param[in] sigma - * Uncertainty in API format - * @param[out] pm - * 3D position on TM in SI units - * @param[out] vm - * 3D velocity on TM in SI units - * @param[out] s2pm - * 3D position variances on TM in SI units - * @param[out] s2vm - * 3D velocity variances on TM in SI units - */ -PRIVATE void __spherical_to_tangent( - const fl_spherical_position *pos, - const fl_tangent_velocity *vel, - const fl_sigma *sigma, - fl_position_vector_ref pm, - fl_velocity_vector_ref vm, - fl_vector_3d_ref s2pm, - fl_vector_3d_ref s2vm); - -/***************************************************************************//** - * [private] Convert tangent frame data to API location representation, i.e. - * perform conversion of: - * - units from SI to API (degrees, km/h, timestamps); - * - coordinate system TM -> M. - * - * @param[in] x - * 3x3 state matrix - * @param[in] s2x - * 3x3x3 covariance cube - * @param[out] pos - * Position in API format - * @param[out] vel - * Velocity in API format - * @param[out] sigma - * Uncertainty in API format - */ -PRIVATE void __tangent_to_spherical( - const _fl_kalf_state_matrix_ref x, - const _fl_kalf_sigma_cube_ref s2x, - fl_spherical_position *pos, - fl_tangent_velocity *vel, - fl_sigma *sigma); - -/***************************************************************************//** - * [private] Retrieve the current prediction in global frame M. - * - * @param[in] t - * Internal time - * @param[out] prediction - * Spatio-temporal (4D) position in global coordinates. - */ -PRIVATE void __get_prediction_vector(fl_seconds t, fl_position_4d *prediction); - -/***************************************************************************//** - * [private] Map a quaternion into system rotation matrix. - * - * @param[in] q - * Quaternioninc (4D) representation of a rotation. - * @param[out] sr - * Corresponding system rotation matrix (3x3D) - */ -PRIVATE void __quaternion_to_rotation(_const_fl_kalf_quaternion_ref q, fl_matrix_3d_ref sr); - -/***************************************************************************//** - * [private] Update system rotation matrix (__se) given a measurement of the - * device angular velocity. - * - * @param[in] t - * Internal time of the measurement - * @param[in] wm - * 3D vector of angular velocity [rad/s] in device coordinate frame - * @param[in] s2wm - * Estimated variance of the measuremnt. - */ -PRIVATE void __integrate_rw(fl_seconds t, const_fl_spin_vector_ref wm, fl_real s2wm); - -/***************************************************************************//** - * [private] Extrapolate the system rotation (__sp) to the given time assuming - * constant angular velocity. - * - * @param[in] t - * Internal time - */ -PRIVATE void __predict_rw(fl_seconds t); - -/***************************************************************************//** - * [private] Planar (2D) single-order Kalman blending (of either position, - * velocity, or acceleration). Assumes the specific prediction is already - * performed. - * - * @param[in] t - * Internal time - * @param[in] s - * State component identifier to which @a xm and @a s2xm refer. - * @param[in] xm - * Measured data - * @param[in] s2xm - * Data variances - */ -PRIVATE void __kalman_2d1o(fl_seconds t, _fl_kalf_state_component s, const_fl_vector_3d_ref xm, const_fl_vector_3d_ref s2xm); - -/***************************************************************************//** - * [private] Planar (2D) Kalman blending of position. - * - * @param[in] t - * Internal time - * @param[in] pm - * Measured position - * @param[in] s2pm - * Position variances - */ -PRIVATE void __kalman_p(fl_seconds t, const_fl_position_vector_ref pm, const_fl_vector_3d_ref s2pm); - -/***************************************************************************//** - * [private] Spatial (3D) Kalman blending of position and velocity. - * - * @param[in] t - * Internal time - * @param[in] pm - * Measured position - * @param[in] vm - * Measured velocity - * @param[in] s2pm - * Position variances - * @param[in] s2vm - * Velocity variances - */ -PRIVATE void __kalman_pv( - fl_seconds t, - const_fl_position_vector_ref pm, - const_fl_velocity_vector_ref vm, - const_fl_vector_3d_ref s2pm, - const_fl_vector_3d_ref s2vm); - -/***************************************************************************//** - * [private] Axial (1D) Kalman blending of angular velocity along z-axis (turn). - * - * @param[in] t - * Internal time - * @param[in] wzm - * Z-projection of the measured angular velocity - * @param[in] s2wzm - * Corresponding variance - */ -void __kalman_wz(fl_seconds t, fl_real wzm, fl_real s2wzm); - -/***************************************************************************//** - * [private] Kalman blending two unit 3D vectors usign inverse covariances as - * weights (to avoid singularities). - * - * @param[in] t - * Internal time - * @param[in] u1 - * 3D vector - * @param[in] u2 - * 3D vector - * @param[in] w2u1 - * Inverse variance of @a u1 (can be zero) - * @param[in] w2u2 - * Inverse variance of @a u2 (can be zero) - * @param[out] v - * Output 3D vector - * @param[out] w2v - * Output inverse variance of @a v - */ -PRIVATE void __inv_kalman_u( - fl_seconds t, - const_fl_vector_3d_ref u1, fl_real w2u1, - const_fl_vector_3d_ref u2, fl_real w2u2, - fl_vector_3d_ref v, fl_real *w2v); - -/***************************************************************************//** - * [private] Extrapolate position, velocity, and the covariance-matrices up to - * the given time. - * - * @param[in] t - * Internal time - */ -PRIVATE void __predict_pv(fl_seconds t); - -/***************************************************************************//** - * [private] Check the notification conditions; if satisfied get the current - * prediction in global coordinate frame and signal it to the user. - * - * @param[in] t - * Internal time - */ -PRIVATE void __signal_updated_location(fl_seconds t); - -#if !defined(__FL_KALMAN_FILTER_C__) - #undef PRIVATE - #undef INLINE -#endif - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_KALMAN_FILTER_H__ */ diff --git a/lbs-server/src/fused/lp-3d-filter.c b/lbs-server/src/fused/lp-3d-filter.c deleted file mode 100644 index e6bfd71..0000000 --- a/lbs-server/src/fused/lp-3d-filter.c +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ -#endif - -#define __FL_LP_3D_FILTER_C__ - -#include -#include "math-ext.h" -#include "lp-3d-filter.h" -#include "debug_util.h" - -#define LP3D_INPUT_FORMAT "% 6.2f" - -void fl_lp3d_init(void) -{ - LOG_FUSED_DEV(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); - __lp3d_G = 0; - __lp3d_B1 = -2; - __lp3d_B2 = 1; - fl_lp3d_reset(); - LOG_FUSED_DEV(DBG_LOW, LEAVE_FUNCTION_PREFIX("(): OK"), 0); -} - -void fl_lp3d_exit(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); -} - -void fl_lp3d_set_sampling_frequency(fl_hertz f) -{ - if (f > 0) { - double omegaC = tan(M_PI * FL_LP3D_CUTOFF_FREQUENCY / f); - double omegaC2 = fl_square(omegaC); - double _B0; - - _B0 = 1.0 / (1 + 2 * omegaC * cos(M_PI_4) + omegaC2); - __lp3d_B1 = _B0 * 2*(omegaC2 - 1); - __lp3d_B2 = _B0 * (1 - 2 * omegaC * cos(M_PI_4) + omegaC2); - __lp3d_G = _B0 * omegaC2; - - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%g Hz): OK"), f); - return; - } else { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%g Hz): E_INVALID_ARGUMENT"), f); - return; - } -} - -void fl_lp3d_reset(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - memset(__lp3d_u, 0, sizeof(__lp3d_u)); - memset(__lp3d_v, 0, sizeof(__lp3d_v)); -} - -void fl_lp3d_reset_to(const_fl_acceleration_vector_ref stationary) -{ - unsigned i; - - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT "})"), - stationary[0], - stationary[1], - stationary[2]); - - i = GEO_SPATIAL_DIMENSION; - do { - --i; - /* up-conversion */ - __lp3d_u[TIME_SHIFT_1][i] = stationary[i]; - __lp3d_u[TIME_SHIFT_2][i] = stationary[i]; - __lp3d_v[TIME_SHIFT_1][i] = stationary[i]; - __lp3d_v[TIME_SHIFT_2][i] = stationary[i]; - } while (i); -} - -const_fl_acceleration_vector_ref fl_lp3d_process(const_fl_acceleration_vector_ref u) -{ - unsigned i; - - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT "})"), - u[0], - u[1], - u[2]); - - i = GEO_SPATIAL_DIMENSION; - do { - --i; - - fl_acceleration u0 = u[i]; - #define u1 __lp3d_u[TIME_SHIFT_1][i] - #define u2 __lp3d_u[TIME_SHIFT_2][i] - fl_real v0; - #define v1 __lp3d_v[TIME_SHIFT_1][i] - #define v2 __lp3d_v[TIME_SHIFT_2][i] - - v0 = __lp3d_G * (u0 + 2 * u1 + u2) - __lp3d_B1 * v1 - __lp3d_B2 * v2; - u2 = u1; - u1 = u0; - v2 = v1; - v1 = v0; - - #undef u1 - #undef u2 - #undef v1 - #undef v2 - } while (i); - - return __lp3d_v[TIME_SHIFT_1]; -} diff --git a/lbs-server/src/fused/lp-3d-filter.h b/lbs-server/src/fused/lp-3d-filter.h deleted file mode 100644 index 9277cc8..0000000 --- a/lbs-server/src/fused/lp-3d-filter.h +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file lp-3d-filter.h - * @brief Gravity direction estimator via low-pass 3D 2nd order Butterworth - * filter. - */ - -#pragma once -#ifndef __FL_LP_3D_FILTER_H__ -#define __FL_LP_3D_FILTER_H__ - -#include "types.h" -#include "math-ext.h" -#include "parameters.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************//** - * [public] Initializaton of the singleton unit (static constructor). - */ -void fl_lp3d_init(void); - -/***************************************************************************//** - * [public] Cleanup of the singleton unit (static destructor). - */ -void fl_lp3d_exit(void); - -/***************************************************************************//** - * [public] Adjust internal parameters to the given sampling frequency and reset - * the the past state memory. - * - * @param[in] f - * The sampling frequency in [Hz] of the accelerometer (or processing - * callback if downsampled). - */ -void fl_lp3d_set_sampling_frequency(fl_hertz f); - -/***************************************************************************//** - * [public] Reset the past state memory. - */ -void fl_lp3d_reset(void); - -/***************************************************************************//** - * [public] Reset to the given stationary state (input = output). - * - * @param[in] stationary - * 3D vector of acceleration measured in tangent coordinate frame. - */ -void fl_lp3d_reset_to(const_fl_acceleration_vector_ref stationary); - -/***************************************************************************//** - * [public] Process single input sample through the 3D Butterworth filter. - * - * @param[in] u - * 3D acceleration input vector. - * - * @return - * 3D acceleration output vector. - */ -const_fl_acceleration_vector_ref fl_lp3d_process(const_fl_acceleration_vector_ref u); - -#if defined(__FL_LP_3D_FILTER_C__) - #define INLINE \ - inline -#else - #define INLINE -#endif -#if defined(__FL_LP_3D_FILTER_C__) - #ifdef GTEST - #define PRIVATE - #else - #define PRIVATE \ - static - #endif -#else - #define PRIVATE \ - extern -#endif - -typedef enum { - TIME_SHIFT_1, /* t - 1 */ - TIME_SHIFT_2, /* t - 2 */ - /* always the last */ - TIME_SHIFT_COUNT -} _fl_lp3d_time_shift; - -/* filter coefficients */ -PRIVATE fl_real __lp3d_G; /* omega^2/B_0 */ -PRIVATE fl_real __lp3d_B1; /* B_1/B_0 */ -PRIVATE fl_real __lp3d_B2; /* B_2/B_0 */ -/* memory of past states */ -PRIVATE fl_acceleration_vector __lp3d_u[TIME_SHIFT_COUNT]; -PRIVATE fl_acceleration_vector __lp3d_v[TIME_SHIFT_COUNT]; - -#if (LP3D_FEATURE_SIGMA2) -PRIVATE fl_real __lp3d_u2[TIME_SHIFT_COUNT]; -PRIVATE fl_real __lp3d_v2[TIME_SHIFT_COUNT]; -#endif - -#if !defined(__FL_LP_3D_FILTER_C__) - #undef PRIVATE - #undef INLINE -#endif - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_LP_3D_FILTER_H__ */ diff --git a/lbs-server/src/fused/math-ext.c b/lbs-server/src/fused/math-ext.c deleted file mode 100644 index 6ff3419..0000000 --- a/lbs-server/src/fused/math-ext.c +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file math-ext.c - * @brief Math extensions - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ -#endif - -#define __FL_MATH_EXT_C__ - -#include -#include "math-ext.h" -#include "debug_util.h" - -#define PRECISION 1e-5 - -double fl_asin(double x) -{ - if (x >= 1) { - if (x > 1 + PRECISION) { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%g) OUT_OF_DOMAIN"), x); - } - return M_PI_2; - } else if (x <= -1) { - if (x < -(1 + PRECISION)) { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(%g) OUT_OF_DOMAIN"), x); - } - return -M_PI_2; - } else - return asin(x); -} - diff --git a/lbs-server/src/fused/math-ext.h b/lbs-server/src/fused/math-ext.h index 61e4eef..07e5e3e 100644 --- a/lbs-server/src/fused/math-ext.h +++ b/lbs-server/src/fused/math-ext.h @@ -20,8 +20,8 @@ */ #pragma once -#ifndef __FL_MATH_EXT_H__ -#define __FL_MATH_EXT_H__ +#ifndef __MATH_EXT_H__ +#define __MATH_EXT_H__ #define _USE_MATH_DEFINES #include @@ -31,14 +31,6 @@ extern "C" { #endif -#if defined(__FL_MATH_EXT_C__) - #define INLINE \ - static inline -#else - #define INLINE \ - static inline -#endif - #if !defined(M_PI) /* C99 */ #define M_PI 3.14159265358979323846 /* pi */ #define M_PI_2 1.57079632679489661923 /* pi/2 */ @@ -54,154 +46,34 @@ extern "C" { #define M_1_SQRT3 0.577350269189625764509 /* 1/sqrt(3) */ #define M_2PI (2 * M_PI) /* 2*pi */ -/*******************************************************************//** - * [public] Second power of a number. Use this macro for preprocessed - * values and static variables. - * - * @param[in] x - * Arbitrary number - * - * @return - * Second power of @a x. +/** + * @brief Second power of a number. Use this macro for preprocessed values and static variables. + * @param[in] x Arbitrary number + * @return Second power of @a x. */ #define SQUARE(x) ((x) * (x)) -/*******************************************************************//** - * [public] Second power of a number. Use for dynamic variables. - * - * @param[in] x - * Arbitrary number - * - * @return - * Second power of @a x. - */ -INLINE double fl_square(double x) -{ - return SQUARE(x); -} - -/*******************************************************************//** - * [public] Domain-safe arcus-sine. If the argument runs out of asin() - * domain it is cast-back onto the nearest edge. If it departs beyond - * numerical error margin, the error is reported, and breakpoint fired. - * No exception is thrown in release mode. - * - * @param[in] x - * Number in the [-1, 1] range. - * - * @return - * - arcus sine of @x if x < 1 && x > -1, - * - -pi if x <= -1, - * - pi if x >= 1. - */ -double fl_asin(double x); - -/*******************************************************************//** - * [public] Squared length of a 2D vector in Euclidean metric. - * - * @param[in] x - * 2D vector - * - * @return - * Squared length of @a x, i.e. |x|^2 = x.x - */ -INLINE double fl_vector_2d_length2(const double x[]) -{ - return SQUARE(x[0]) + SQUARE(x[1]); -} - -/*******************************************************************//** - * [public] Squared length of a 3D vector in Euclidean metric. - * - * @param[in] x - * 3D vector - * - * @return - * Squared length of @a x, i.e. |x|^2 = x.x - */ -INLINE double fl_vector_3d_length2(const double x[]) -{ - return SQUARE(x[0]) + SQUARE(x[1]) + SQUARE(x[2]); -} - -/*******************************************************************//** - * [public] Inlined left-action of a linear operator on a 3D - * covector. - * - * @param[in] m - * 3x3D matrix - * @param[in] v - * 3D vector - * @param[out] out - * Left-action of the linear operator @a m on the vector @a v, i.e. - * out = m.v - */ -#define MATRIX_DOT_VECTOR_3D(m, v, out)\ - do {\ - (out)[GEO_SPATIAL_X] = (m)[GEO_SPATIAL_X][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_X][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_X][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\ - (out)[GEO_SPATIAL_Y] = (m)[GEO_SPATIAL_Y][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\ - (out)[GEO_SPATIAL_Z] = (m)[GEO_SPATIAL_Z][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\ - } while (0) - -/*******************************************************************//** - * [public] Inlined right-action of a linear operator on a 3D - * covector. In Euclidean metric this is the same as left action of the - * transposed operator on the vector. - * - * @param[in] v - * 3D vector - * @param[in] m - * 3x3D matrix - * @param[out] out - * Right-action of the linear operator @a m on the covector @a v, i.e. - * out = v.m - */ -#define VECTOR_DOT_MATRIX_3D(v, m, out)\ - do {\ - (out)[GEO_SPATIAL_X] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_X] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_X] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_X];\ - (out)[GEO_SPATIAL_Y] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_Y] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Y];\ - (out)[GEO_SPATIAL_Z] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_Z] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Z];\ - } while (0) - -/*******************************************************************//** - * [public] Inlined cross-product of two 3D vectors. - * - * @param[in] u - * 3D vector - * @param[in] v - * 3D vector - * @param[out] out - * Cross-product of @a u and @a v +/** + * @brief Second power of a number. Use for dynamic variables. + * @param[in] x Arbitrary number + * @return[double] Second power of @a x. */ -#define VECTOR_CROSS_VECTOR_3D(u, v, out)\ - do {\ - (out)[GEO_SPATIAL_X] = (u)[GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Z] - (u)[GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Y];\ - (out)[GEO_SPATIAL_Y] = (u)[GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_X] - (u)[GEO_SPATIAL_X] * (v)[GEO_SPATIAL_Z];\ - (out)[GEO_SPATIAL_Z] = (u)[GEO_SPATIAL_X] * (v)[GEO_SPATIAL_Y] - (u)[GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_X];\ - } while (0) +#define fl_square(x) ({double __y__ = (x); __y__ * __y__;}) -/*******************************************************************//** - * [public] Scalar Euclidean product of two 3D vectors. - * - * @param[in] x - * 3D vector - * @param[in] y - * 3D vector - * - * @return - * Scalar product of @a x and @a y. +/** + * @brief Mean Earth gravity value in [m/s^2]. + * Notice that, strictly speaking, the mean Earth gravity acceleration at surface + * level is negative; the positive value we measure with an accelerometer is the + * ground reaction which counters the gravity and prevents us from falling into the interior. + * Also notice that given the inaccuracy (i.e. bias & miscalibration) of mobile device + * acceleromenters at ~1 [m/s^2] the number of significant digits in the reference value below is way too many. */ -INLINE double fl_vector_3d_dot_product(const double x[], const double y[]) -{ - return x[0] * y[0] + x[1] * y[1] + x[2] * y[2]; -} +#define EARTH_GRAVITY 9.80665 -#if !defined(__FL_MATH_EXT_C__) - #undef INLINE -#endif +#define EARTH_RADIUS 6371000.0 #ifdef __cplusplus } #endif -#endif /* __FL_MATH_EXT_H__ */ +#endif /* __MATH_EXT_H__ */ diff --git a/lbs-server/src/fused/motion-detector.c b/lbs-server/src/fused/motion-detector.c deleted file mode 100644 index f220867..0000000 --- a/lbs-server/src/fused/motion-detector.c +++ /dev/null @@ -1,109 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if !defined(LOG_THRESHOLD) - /* custom logging threshold - keep undefined on the repo */ - /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ -#endif - -#define __FL_MOTION_DETECTOR_C__ - -#define _USE_MATH_DEFINES -#include -#include "math-ext.h" -#include "motion-detector.h" -#include "debug_util.h" - -#if (FL_MOTION_DETECTOR) - -void fl_moti_init(motion_changed_cb motion_changed, gpointer handler) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(motion_cb=%p, handler=%p)"), motion_changed, handler); - fl_aema1d_init(&__moti_al2, 1 << FL_MOTI_AEMA_PLATEAU_BITS); - __moti_immobile_time = 0; - __moti_state = MOTI_UNDECIDED; - __moti_last_notification = MOTI_UNDECIDED; - __moti_motion_changed = motion_changed; - __moti_handler = handler; -} - -void fl_moti_exit(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - __moti_handler = NULL; - __moti_motion_changed = NULL; - __moti_last_notification = MOTI_UNDECIDED; - __moti_state = MOTI_UNDECIDED; - __moti_immobile_time = 0; - fl_aema1d_exit(&__moti_al2); -} - -void fl_moti_reset(void) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"), 0); - __moti_immobile_time = 0; - __moti_state = MOTI_UNDECIDED; - __moti_last_notification = MOTI_UNDECIDED; - fl_aema1d_reset(&__moti_al2); -} - -PRIVATE void __moti_set_state(fl_motion_state state) -{ - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%u->%u)"), __moti_state, state); - __moti_state = state; - if (__moti_motion_changed && __moti_last_notification != state) { - __moti_motion_changed(state, __moti_handler); - __moti_last_notification = state; - } -} - -void fl_moti_process(fl_seconds t, fl_acceleration_vector_ref al) -{ - fl_real al2; - - al2 = fl_aema1d_process(&__moti_al2, fl_vector_3d_length2(al)); - if (al2 > SQUARE(FL_MOTI_MOTION_LEVEL)) { - switch (__moti_state) { - case MOTI_UNDECIDED: - case MOTI_IMMOBILE: - case MOTI_SLEEP: - __moti_set_state(MOTI_MOVING); - break; - - default: /* MOTI_MOVING */ - break; - } - } else if (al2 < SQUARE(FL_MOTI_NOISE_LEVEL)) { - switch (__moti_state) { - case MOTI_UNDECIDED: - case MOTI_MOVING: - __moti_set_state(MOTI_IMMOBILE); - __moti_immobile_time = t; - break; - - case MOTI_IMMOBILE: - if (t - __moti_immobile_time > FL_MOTI_IMMOBILITY_INTERVAL) - __moti_set_state(MOTI_SLEEP); - break; - - default: - /* MOTI_SLEEP: */ - break; - } - } -} - -#endif diff --git a/lbs-server/src/fused/motion-detector.h b/lbs-server/src/fused/motion-detector.h deleted file mode 100644 index 8d31ee1..0000000 --- a/lbs-server/src/fused/motion-detector.h +++ /dev/null @@ -1,133 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file motion-detector.h - * @brief Accelerometer-based detector of significant motion. - */ - -#pragma once -#ifndef __FL_MOTION_DETECTOR_H__ -#define __FL_MOTION_DETECTOR_H__ - -#include -#include "types.h" -#include "parameters.h" -#include "aema-estimator.h" - -#ifdef __cplusplus -extern "C" { -#endif - -typedef enum { - MOTI_UNDECIDED, /* initial */ - MOTI_MOVING, /* acceleration above MOTION_LEVEL */ - MOTI_IMMOBILE, /* acceleration below NOISE_LEVEL */ - MOTI_SLEEP, /* immobile for IMMOBILITY_INTERVAL */ -} fl_motion_state; - -/***************************************************************************//** - * [public] Callback invoked on every change in motion state. - * - * @param[in] state - * New motion state. - * @param[in] handler - * Arbitrary pointer to user data specified during the init() call. - */ -typedef void (*motion_changed_cb)(fl_motion_state state, gpointer handler); - -#if (FL_MOTION_DETECTOR) - -/***************************************************************************//** - * [public] Initializaton of the singleton unit (static constructor). - * - * @param[in] motion_changed - * Callback to be invoked when the detected state of motion changes. - * @param[in] handler - * Arbitrary user parameter passed to the callback. - */ -void fl_moti_init(motion_changed_cb motion_changed, gpointer handler); - -/***************************************************************************//** - * [public] Cleanup of the singleton unit (static destructor). - */ -void fl_moti_exit(void); - -/***************************************************************************//** - * [public] Reset of the internal state back to initial one. This function is - * used for repetitive testing and module soft restarts. - */ -void fl_moti_reset(void); - -/***************************************************************************//** - * [public] Processing of a single sample from the accelerometer. This function - * implments a hysteresis in the 2D space of linear acceleration modulus |al|^2 - * vs motion state: - * - Crossing the FL_MOTI_MOTION_LEVEL from below changes state to MOVING; - * - Crossing the FL_MOTI_NOISE_LEVEL from above changes state to IMMOBILE; - * - Staying IMMOBILE for longer than FL_MOTI_IMMOBILITY_INTERVAL changes - * state to SLEEP. - * - * @param[in] t - * Time of the event in seconds. - * @param[in] al - * 3D vector of linear acceleration. - */ -void fl_moti_process(fl_seconds t, fl_acceleration_vector_ref al); - -#if defined(__FL_MOTION_DETECTOR_C__) - #ifdef GTEST - #define PRIVATE - #else - #define PRIVATE \ - static - #endif -#else - #define PRIVATE \ - extern -#endif - -PRIVATE fl_aema1d_estimator __moti_al2; /**< Linear acceleration low-pass filter */ -PRIVATE fl_seconds __moti_immobile_time; /**< Start time of immobility */ -PRIVATE fl_motion_state __moti_state; /**< Curent state */ -PRIVATE fl_motion_state __moti_last_notification; /**< Last notified state */ -PRIVATE motion_changed_cb __moti_motion_changed; /**< User callback */ -PRIVATE gpointer __moti_handler; /**< User data */ - -/***************************************************************************//** - * [private] Change the state indicator and notify the user. - * - * @param[in] state - * New state value (does not have to be different than present one). - */ -PRIVATE void __moti_set_state(fl_motion_state state); - -#if !defined(__FL_MOTION_DETECTOR_C__) - #undef PRIVATE -#endif - -#else - #define fl_moti_init(motion_changed, handler) - #define fl_moti_exit() - #define fl_moti_reset() - #define fl_moti_process(t, al) -#endif - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_MOTION_DETECTOR_H__ */ diff --git a/lbs-server/src/fused/parameters.h b/lbs-server/src/fused/parameters.h index 2f2c053..4513493 100644 --- a/lbs-server/src/fused/parameters.h +++ b/lbs-server/src/fused/parameters.h @@ -33,15 +33,21 @@ #define FL_KALMAN_ANGULAR_VELOCITY 1 /** 0 = off, 1 = Filtration of output location notifications */ #define FL_MIN_NOTIFICATION_FILTRATION 1 -/** Minimal interval in miliseconds between consecutive output location notifications */ -#define FL_MIN_NOTIFICATION_INTERVAL 1000 +/** Minimal interval in seconds between consecutive output location notifications */ +#define FL_MIN_NOTIFICATION_INTERVAL 1.0 +/** Maximum time in seconds from last input location for sending output position to clients */ +#define FL_MAX_TIME_FROM_LAST_POS 300.0 +/** Max time in seconds from last gps when pedometer skip may occur (filtration pedometer false positive in driving case) */ +#define FL_MAX_PEDOMETER_SKIP_TIME 60.0 +/** Min speed in km/h from last gps when pedometer skip may occur (filtration pedometer false positive in driving case) */ +#define FL_MIN_PEDOMETER_SKIP_SPEED 10.0 /** 0 = off, 1 = fused devel logs on */ #define FL_ENABLE_DEVEL_LOG 0 /* Free parameters */ /** [ms] Sensor sampling interval */ -#define FL_SENSOR_SAMPLING_INTERVAL 100 +#define FL_SENSOR_SAMPLING_INTERVAL 10 /** [m] Default standard deviation of measured position */ #define FL_DEFAULT_POSITION_SIGMA 8.0 /** [m/s] Default standard deviation of measured linear velocity */ @@ -49,14 +55,14 @@ /** [rad/s] Default standard deviation of measured angular velocity */ #define FL_DEFAULT_SPIN_SIGMA 0.2 /** [m/s^2] Accelerometer noise level */ -#define FL_ACCEL_NOISE_LEVEL ((fl_acceleration)0.25) +#define FL_ACCEL_NOISE_LEVEL 0.25 /** [Hz] Low-pass acceleration frequency threshold for estimation of gravity direction */ -#define FL_LP3D_CUTOFF_FREQUENCY ((fl_hertz)1.0) +#define FL_LP3D_CUTOFF_FREQUENCY 10 /** [m/s^2] Upper level of IMMOBILE state */ #define FL_MOTI_NOISE_LEVEL FL_ACCEL_NOISE_LEVEL /** [m/s^2] Lower level of MOVING state */ #define FL_MOTI_MOTION_LEVEL (3*FL_ACCEL_NOISE_LEVEL) -/** [s] Continuous immobility interval before transitioning to SLEEP state */ +/** [s] Continuous immobility interval before transition to SLEEP state */ #define FL_MOTI_IMMOBILITY_INTERVAL 90 /** Log-2 of the AEMA averaging constant used for motion detection. */ #define FL_MOTI_AEMA_PLATEAU_BITS 4 diff --git a/lbs-server/src/fused/types.h b/lbs-server/src/fused/types.h deleted file mode 100644 index 8c8f9e9..0000000 --- a/lbs-server/src/fused/types.h +++ /dev/null @@ -1,210 +0,0 @@ -/* - * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * @file types.h - * @brief Core types definitions - */ - -#pragma once -#ifndef __FL_TYPES_H__ -#define __FL_TYPES_H__ - -#include "location-types.h" -#include "location-position.h" -#include "location-velocity.h" -#include "location-accuracy.h" - -#ifdef __cplusplus -extern "C" { -#endif - -typedef double fl_real; /**< Default precision for the floating-point data in fused location */ -typedef float sensor_real; /**< Default precision for the floating-point data in sensors */ -typedef fl_real fl_seconds; /**< Time variables in SI units */ -typedef unsigned int fl_timestamp; /**< Time-stamps in [s] */ -typedef fl_real fl_hertz; /**< Frequency variables in SI units */ -typedef fl_real fl_meters; /**< Length variables in SI units */ -typedef fl_real fl_velocity; /**< Linear velocity in SI units [m/s] */ -typedef fl_real fl_km_h; /**< Linear velocity in SI-derived units [km/h] */ -typedef fl_real fl_acceleration; /**< Linear acceleration in SI units [m/s^2] */ -typedef sensor_real sensor_acceleration; /**< Linear acceleration in SI units [m/s^2] supplied by */ -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 */ -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 */ -typedef fl_real fl_utesla; /**< Magnetic field strength (B) in [uT] */ -typedef sensor_real sensor_utesla; /**< Magnetic field strength (B) in [uT] supplied by */ - -#define FL_UNDEFINED_TIME ((fl_seconds)-1e+48) - -/** Labels for a planar 2D space*/ -typedef enum { - PLANAR_X, - PLANAR_Y, - /* always the last */ - PLANAR_DIMENSION -} fl_planar; - -/** Labels for an axial (or cylyndrical without phase) 2D space */ -typedef enum { - AXIAL_H, - AXIAL_V, - /* always the last */ - AXIAL_DIMENSION -} fl_axial; - -/** Labels for a geographic coordinate system (left-handed) */ -typedef enum { - GEO_SPATIAL_X, - GEO_SPATIAL_Y, - GEO_SPATIAL_Z, - /* always the last */ - GEO_SPATIAL_DIMENSION -} fl_geo_spatial; - -/** Labels for a device coordinate system (right-handed) */ -typedef enum { - DEV_SPATIAL_X = GEO_SPATIAL_Y, - DEV_SPATIAL_Y = GEO_SPATIAL_X, - DEV_SPATIAL_Z = GEO_SPATIAL_Z, - DEV_SPATIAL_DIMENSION = GEO_SPATIAL_DIMENSION -} fl_dev_spatial; - -/** Labels for sensors */ -typedef enum { - ACCELEROMETER, - GYROSCOPE, - SENSOR_SOURCE_NUM -} fl_sensory_source; - -/** Flag indicators for sensors' combination */ -typedef enum { - NO_SENSOR_FLAG = 0, - ACCELEROMETER_FLAG = (1 << ACCELEROMETER), - GYROSCOPE_FLAG = (1 << GYROSCOPE), - /* always the last */ - SUPPORTED_SENSORS_MASK = (1 << SENSOR_SOURCE_NUM) - 1 -} fl_sensory_flags; - -/** Three Cartesian coordinates of a point on the Earth surface */ -typedef fl_meters fl_position_vector[GEO_SPATIAL_DIMENSION]; -typedef fl_meters *fl_position_vector_ref; -typedef const fl_meters *const_fl_position_vector_ref; - -typedef fl_velocity fl_velocity_vector[GEO_SPATIAL_DIMENSION]; -typedef fl_velocity *fl_velocity_vector_ref; -typedef const fl_velocity *const_fl_velocity_vector_ref; - -/** Three device-oriented axes of the acceleration */ -typedef sensor_acceleration sensor_acceleration_vector[GEO_SPATIAL_DIMENSION]; -typedef sensor_acceleration *sensor_acceleration_vector_ref; -typedef const sensor_acceleration *const_sensor_acceleration_vector_ref; -typedef fl_acceleration fl_acceleration_vector[GEO_SPATIAL_DIMENSION]; -typedef fl_acceleration *fl_acceleration_vector_ref; -typedef const fl_acceleration *const_fl_acceleration_vector_ref; - -/** Three imaginary axes of the rotation quaternion */ -typedef sensor_spin sensor_spin_vector[GEO_SPATIAL_DIMENSION]; -typedef sensor_spin *sensor_spin_vector_ref; -typedef const sensor_spin *const_sensor_spin_vector_ref; -typedef fl_spin fl_spin_vector[GEO_SPATIAL_DIMENSION]; -typedef fl_spin *fl_spin_vector_ref; -typedef const fl_spin *const_fl_spin_vector_ref; - -typedef sensor_utesla sensor_magnetic_vector[GEO_SPATIAL_DIMENSION]; -typedef sensor_utesla *sensor_magnetic_vector_ref; -typedef const sensor_utesla *const_sensor_magnetic_vector_ref; -typedef fl_utesla fl_magnetic_vector[GEO_SPATIAL_DIMENSION]; -typedef fl_utesla *fl_magnetic_vector_ref; -typedef const fl_utesla *const_fl_magnetic_vector_ref; - -typedef const sensor_real *const_sensor_vector_3d_ref; - -typedef fl_real fl_vector_2d[AXIAL_DIMENSION]; -typedef fl_real *fl_vector_2d_ref; -typedef const fl_real *const_fl_vector_2d_ref; - -typedef fl_real fl_vector_3d[GEO_SPATIAL_DIMENSION]; -typedef fl_real *fl_vector_3d_ref; -typedef const fl_real *const_fl_vector_3d_ref; - -typedef fl_real fl_matrix_3d[GEO_SPATIAL_DIMENSION][GEO_SPATIAL_DIMENSION]; -typedef fl_real(*fl_matrix_3d_ref)[GEO_SPATIAL_DIMENSION]; -typedef fl_real(*const const_fl_matrix_3d_ref)[GEO_SPATIAL_DIMENSION]; - -/* Spatio-temporal position in Cartesian coordinates */ -typedef struct { - fl_position_vector spatial; - fl_seconds time; -} fl_position_4d; - -/*! Position in spherical coordintes */ -typedef LocationPosition fl_spherical_position; -/*! Velocity in coordinates of tangent space (tangent plane x ray coordinate) */ -typedef LocationVelocity fl_tangent_velocity; -/*! Standard deviations (1-sigma, or CEP 67%) */ -typedef struct { - LocationAccuracyLevel level; /**< ovelapps with 'accuracy.level' */ - fl_meters of_horizontal_pos; /**< std. dev. of horizontal position in [m] */ - fl_meters of_altitude; /**< std. dev. of vertical position in [m] */ - fl_velocity of_speed; /**< std. dev. of horizontal velocity in [m/s] */ - fl_velocity of_climb; /**< std. dev. of vertical velocity in [m/s] */ -} fl_sigma; - -/** Huge sigma value representing "almost flat" distribution. Unfortunately, - * we're forced by the API to use uncertainties (standard deviations, CEPs, - * or variances if squared) instead of the certainties (weights, or accuracies) - * which are their inverses and do not suffer this initialization problem. - * Even more peculiar is the custom of twisting the brain and calling - * uncertainty 'accuracy'. By this token zero represents the maximum, while - * infinity - the minimum. Beware. - */ -#define FLAT_SIGMA 1e24 -#define FLAT_SIGMA2 (FLAT_SIGMA * FLAT_SIGMA) - -typedef union { - LocationAccuracy accuracy; - fl_sigma sigma; -} fl_uncertainty_union; - -/*! Complete location data */ -typedef struct { - fl_spherical_position pos; - fl_tangent_velocity vel; - union { - LocationAccuracy accuracy; - fl_sigma sigma; - }; -} fl_location; - -/***************************************************************************//** - * Callback invoked on every new predicted position. - * - * @param[in] position - * The structure holds current filter output: spatio-temporal vector of - * dimension (3+1) in Cartesian coordinate system. - * @param[in] handler - * Arbitrary pointer to user data specified during the init() call. - */ -typedef void (*predicted_updated_cb)(const fl_position_4d* position, gpointer handler); - -#ifdef __cplusplus -} -#endif - -#endif /* __FL_TYPES_H__ */ diff --git a/lbs-server/src/lbs_server.c b/lbs-server/src/lbs_server.c index a8eb904..ce54e60 100644 --- a/lbs-server/src/lbs_server.c +++ b/lbs-server/src/lbs_server.c @@ -47,6 +47,7 @@ #define MAX_BATCH_INTERVAL 255 #define MAX_BATCH_PERIOD 60000 + typedef struct { /* gps variables */ pos_data_t position; @@ -101,6 +102,7 @@ typedef struct { gint fused_balance_count; gboolean is_fused_running; gint current_location_source; + gint fused_test_block; } lbs_server_s; #ifndef TIZEN_DEVICE @@ -233,6 +235,7 @@ static void nps_set_status(lbs_server_s *lbs_server, LbsStatus status) static void nps_update_position(lbs_server_s *lbs_server, NpsManagerPositionExt wps) { + LOG_NPS(DBG_LOW, "ENTER >>>"); if (!lbs_server) { LOG_NPS(DBG_ERR, "lbs-server is NULL!!"); return; @@ -256,8 +259,9 @@ static void nps_update_position(lbs_server_s *lbs_server, NpsManagerPositionExt LOG_NPS(DBG_LOW, "[WPS] time: %d, latitude : %.6lf, longitude : %.6lf", lbs_server->wps.timestamp, lbs_server->wps.latitude, lbs_server->wps.longitude); #ifdef TIZEN_DEVICE - if (lbs_server->is_fused_running) - send_wps_position_to_fused_engine(wps.timestamp, wps.latitude, wps.longitude, wps.hor_accuracy, wps.ver_accuracy); + if (lbs_server->is_fused_running) { + send_wps_position_to_fused_engine(wps.timestamp, wps.latitude, wps.longitude, wps.hor_accuracy); + } #endif lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_NPS, @@ -1160,8 +1164,7 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat start_tracking(lbs_server, method, fused_mode); - } - else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP")) { + } else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP")) { while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) { if (!g_strcmp0(key, "METHOD")) method = g_variant_get_int32(value); @@ -1350,24 +1353,22 @@ static void shutdown(gpointer userdata, gboolean *shutdown_arr) } #ifdef TIZEN_DEVICE -static void fused_update_position_cb(fl_location *location, gpointer user_data) +static void fused_update_position_cb(gint timestamp, gdouble latitude, gdouble longitude, gdouble altitude, + gdouble speed, gdouble direction, gdouble climb, gdouble hor_accuracy, gdouble ver_accuracy, gpointer user_data) { // LOG_FUSED_FUNC; - if (location == NULL || user_data == NULL) { - LOG_FUSED(LOG_ERROR, "parameter is NULL"); + if (user_data == NULL) { + LOG_FUSED(LOG_ERROR, "user_data parameter is NULL"); return; } lbs_server_s *lbs_server = (lbs_server_s *)(user_data); LbsPositionExtFields fields = (LBS_POSITION_EXT_FIELDS_LATITUDE | LBS_POSITION_EXT_FIELDS_LONGITUDE | LBS_POSITION_EXT_FIELDS_ALTITUDE | LBS_POSITION_EXT_FIELDS_SPEED | LBS_POSITION_EXT_FIELDS_DIRECTION | LBS_POSITION_EXT_FIELDS_CLIMB); - GVariant *accuracy = g_variant_new("(idd)", LBS_ACCURACY_LEVEL_DETAILED, - location->accuracy.horizontal_accuracy, location->accuracy.vertical_accuracy); + GVariant *accuracy = g_variant_new("(idd)", LBS_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy); lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_FUSED, - fields, location->pos.timestamp, - location->pos.latitude, location->pos.longitude, location->pos.altitude, - location->vel.speed, location->vel.direction, location->vel.climb, accuracy); + fields, timestamp, latitude, longitude, altitude, speed, direction, climb, accuracy); } #endif @@ -1397,9 +1398,10 @@ static void gps_update_position_cb(pos_data_t *pos, gps_error_t error, void *use gps_set_position(pos); #ifdef TIZEN_DEVICE - if (lbs_server->is_fused_running) + 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); + } #endif lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_GPS, fields, pos->timestamp, diff --git a/packaging/lbs-server.spec b/packaging/lbs-server.spec index 31853ba..c1c981b 100644 --- a/packaging/lbs-server.spec +++ b/packaging/lbs-server.spec @@ -1,6 +1,6 @@ Name: lbs-server Summary: LBS Server for Tizen -Version: 1.2.6 +Version: 1.2.7 Release: 1 Group: Location/Service License: Apache-2.0