From: Marcin Masternak Date: Thu, 30 Nov 2017 10:52:11 +0000 (+0100) Subject: [Fused] Engine update. X-Git-Tag: submit/tizen/20171220.070758^0 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=161bcc20aa20a6baf3d050c95cba6cae82f8b028;p=platform%2Fcore%2Flocation%2Flbs-server.git [Fused] Engine update. - fused small fix Change-Id: I976fdc5677f573b7d739a46206c6973103f87d74 Signed-off-by: Marcin Masternak --- diff --git a/lbs-server/src/debug_util.h b/lbs-server/src/debug_util.h index 160e000..c581335 100644 --- a/lbs-server/src/debug_util.h +++ b/lbs-server/src/debug_util.h @@ -21,7 +21,6 @@ #include #include -#include "fused/parameters.h" #ifdef __cplusplus extern "C" { diff --git a/lbs-server/src/fused.c b/lbs-server/src/fused.c index 64f2c23..5e2c193 100644 --- a/lbs-server/src/fused.c +++ b/lbs-server/src/fused.c @@ -27,6 +27,7 @@ #include "fused/FusionEngine.h" #include "fused.h" #include "debug_util.h" +#include #define PTR2ENUM(ptr, type) ((type)(int)((char*)(ptr) - ((char*)0))) #define ENUM2PTR(value) (((char*)0) + (int)(value)) @@ -42,29 +43,19 @@ typedef struct { 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; - /** Fused location data */ typedef struct { bool is_started; + bool is_initialized; 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 pthread_mutex_t fusion_engine_input_sync_mutex = PTHREAD_MUTEX_INITIALIZER; static void sensors_create() { @@ -133,9 +124,8 @@ static void sensor_start(fl_sensor_source sensor) && 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) { + if (ret == SENSOR_ERROR_NONE) fused.sensors[sensor].is_started = TRUE; - } } } @@ -147,9 +137,8 @@ static void sensor_stop(fl_sensor_source sensor) && 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) { + if (ret == SENSOR_ERROR_NONE) fused.sensors[sensor].is_started = FALSE; - } } } @@ -168,6 +157,11 @@ void location_fused_start() { LOG_FUSED_FUNC; + if (!fused.is_initialized || fused.is_started) { + LOG_FUSED(LOG_DEBUG, "[FUSED] Fused Location Start failed!!"); + return; + } + char *calibration = vconf_get_str(VCONFKEY_LOCATION_GYRO_DRIFT); LOG_FUSED(LOG_DEBUG, "calibration: vconf_get_str(VCONFKEY_LOCATION_GYRO_DRIFT) %s = \"%s\"", calibration != NULL ? "SUCCESS" : "FAILED", @@ -185,12 +179,14 @@ void location_fused_start() void location_fused_stop() { LOG_FUSED_FUNC; + + if (!fused.is_initialized || !fused.is_started) + return; + unsigned int i; for (i = 0; i < SENSOR_SOURCE_COUNT; i++) { sensor_stop(i); } - fusion_engine_stop(&fused.fusion_engine); - fused.is_started = FALSE; char *calibration = fusion_engine_get_calibration(&fused.fusion_engine); if (calibration) { @@ -201,107 +197,102 @@ void location_fused_stop() } else { LOG_FUSED(LOG_DEBUG, "fusion_engine_get_calibration() FAILED"); } + + fusion_engine_stop(&fused.fusion_engine); + fused.is_started = FALSE; } -void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server) +bool location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server) { LOG_FUSED_FUNC; + + fused.is_initialized = FALSE; if (!_fused_pos_cb || !lbs_server) { - LOG_FUSED(LOG_ERROR, "location_fused_init failed !!!"); - return; + LOG_FUSED(LOG_ERROR, "location_fused_init FAILED (NULL arguments)"); + return fused.is_initialized; } + fused.fused_pos_cb = _fused_pos_cb; + fused.lbs_server_handle = lbs_server; + + time_t now = time(NULL); + unsigned long long boot_time_ns = get_current_boot_time(); + double 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, utc_offset_sec); + fused.is_initialized = fusion_engine_init(&fused.fusion_engine, utc_offset_sec); + if (!fused.is_initialized) { + LOG_FUSED(LOG_ERROR, "location_fused_init FAILED (fusion_engine_init FAILED)"); + return fused.is_initialized; + } + 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; - fused.is_started = 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, fused.utc_offset_sec); + fused.is_started = FALSE; + + return fused.is_initialized; } void location_fused_deinit() { LOG_FUSED_FUNC; - if (fused.is_started) - location_fused_stop(); - sensors_destroy(); - fusion_engine_destroy(&fused.fusion_engine); - fused.fused_pos_cb = NULL; - fused.lbs_server_handle = NULL; - fused.is_started = FALSE; - fused.last_gps_speed = -1.0; - fused.last_gps_timestamp = 0; - fused.last_wps_timestamp = 0; + if (fused.is_initialized) { + if (fused.is_started) + location_fused_stop(); + + sensors_destroy(); + fusion_engine_destroy(&fused.fusion_engine); + fused.fused_pos_cb = NULL; + fused.lbs_server_handle = NULL; + fused.is_started = FALSE; + fused.is_initialized = FALSE; + } } 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) { -// LOG_FUSED_FUNC; - - if (!fused.is_started) + if (!fused.is_initialized || !fused.is_started) return; - fused.last_gps_timestamp = timestamp; - fused.last_gps_speed = speed; + pthread_mutex_lock(&fusion_engine_input_sync_mutex); - 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); + bool is_new_position = fusion_engine_gps_event(&fused.fusion_engine, (double)timestamp, latitude, longitude, altitude, + KMPH_TO_MPS(speed), bearing, hor_accuracy, ver_accuracy); if (is_new_position) on_engine_new_position(); + + pthread_mutex_unlock(&fusion_engine_input_sync_mutex); } void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy) { -// LOG_FUSED_FUNC; - - if (!fused.is_started) + if (!fused.is_initialized || !fused.is_started) return; - fused.last_wps_timestamp = timestamp; + pthread_mutex_lock(&fusion_engine_input_sync_mutex); - bool is_new_position = fusion_engine_position_2d_event(&fused.fusion_engine, - (double)timestamp, latitude, longitude, hor_accuracy); + bool is_new_position = fusion_engine_wps_event(&fused.fusion_engine, (double)timestamp, latitude, longitude, hor_accuracy); if (is_new_position) on_engine_new_position(); + + pthread_mutex_unlock(&fusion_engine_input_sync_mutex); } static void on_engine_new_position() { -// LOG_FUSED_FUNC; - - 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((gint)fused.last_location.utc_time, (gdouble)fused.last_location.latitude, (gdouble)fused.last_location.longitude, (gdouble)fused.last_location.altitude, - (gdouble)fused.last_location.speed, + (gdouble)MPS_TO_KMPH(fused.last_location.speed), (gdouble)fused.last_location.direction, (gdouble)fused.last_location.climb, (gdouble)fused.last_location.hor_dev, @@ -313,52 +304,40 @@ static void on_engine_new_position() 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) { + pthread_mutex_lock(&fusion_engine_input_sync_mutex); + 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]); + event->values[X], event->values[Y], event->values[Z]); break; + case GYROSCOPE: is_new_position = fusion_engine_gyro_event(&fused.fusion_engine, (double)(long long)event->timestamp / 1000000.0, - to_radians(event->values[DEV_SPATIAL_X]), - to_radians(event->values[DEV_SPATIAL_Y]), - to_radians(event->values[DEV_SPATIAL_Z])); + to_radians(event->values[X]), + to_radians(event->values[Y]), + to_radians(event->values[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, + case PEDOMETER: + is_new_position = fusion_engine_pedometer_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 + + pthread_mutex_unlock(&fusion_engine_input_sync_mutex); + } else { LOG_FUSED(LOG_ERROR, "event == NULL"); + } } diff --git a/lbs-server/src/fused.h b/lbs-server/src/fused.h index e398349..37c4317 100644 --- a/lbs-server/src/fused.h +++ b/lbs-server/src/fused.h @@ -39,7 +39,7 @@ typedef enum { 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); -void location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server); +bool location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server); void location_fused_deinit(); diff --git a/lbs-server/src/fused/CalibrationFilter.c b/lbs-server/src/fused/CalibrationFilter.c index 49c53cd..2820450 100644 --- a/lbs-server/src/fused/CalibrationFilter.c +++ b/lbs-server/src/fused/CalibrationFilter.c @@ -17,27 +17,17 @@ #include "CalibrationFilter.h" #include "Time.h" #include "math-ext.h" + #include static const Vector_3d ZERO = { 0, 0, 0 }; - -/** Minimum measurements in stable state to calculate new calibration parameters. */ -static const int MIN_MEAS = 100; - +static const int MIN_MEAS = 100; /** Minimum measurements in stable state to calculate new calibration parameters. */ +static const int PARAMS_LEN = 100; /** Length of buffer for calibration parameters. */ +static const int PARAMS_VERSION = 1; /** Format of calibration parameters. */ +static const double DT = 100; /** Time of decorrelation in [s], currently 100s. */ +static const double DAY = 24L * 60L * 60L; /** Days duration in [s], currently 24h. */ static const double MAX_PRECISION = 100000; -/** Time of decorrelation in [s], currently 100s. */ -static const double DT = 100; - -/** Days duration in [s], currently 24h. */ -static const double DAY = 24L * 60L * 60L; - -/** Length of buffer for calibration parameters. */ -static const int PARAMS_LEN = 100; - -/** Format of calibration parameters. */ -static const int PARAMS_VERSION = 1; - void calibration_filter_init(CalibrationFilter* self, double utcOffset) { self->utc_offset = utcOffset; @@ -92,9 +82,9 @@ const_Vector_3d_ref calibration_filter_filter(CalibrationFilter* self, double ti const double curPrec = self->cal_prec * exp(-dt / DT); double newPrec = self->cal_nr / vector_3d_length2(std_dev); - if (newPrec > MAX_PRECISION) { + if (newPrec > MAX_PRECISION) newPrec = MAX_PRECISION; - } + const double alpha = curPrec / (newPrec + curPrec); const double alpha1 = 1. - alpha; @@ -128,9 +118,8 @@ void calibration_filter_clear_calibration(CalibrationFilter* self) char* calibration_filter_get_calibration(CalibrationFilter* self) { char* params = (char*) calloc(PARAMS_LEN, sizeof(char)); - if (params != NULL) { - snprintf(params, PARAMS_LEN, "%i,%.9f,%.10f,%.10f,%.10f,%.10f", - PARAMS_VERSION, + if (params) { + snprintf(params, PARAMS_LEN, "%i,%.9f,%.10f,%.10f,%.10f,%.10f", PARAMS_VERSION, IS_KNOWN_TIME(self->calibration_time) ? self->calibration_time + self->utc_offset : 0, self->cal_prec, self->bias[0], self->bias[1], self->bias[2]); } @@ -139,21 +128,20 @@ char* calibration_filter_get_calibration(CalibrationFilter* self) void calibration_filter_set_calibration(CalibrationFilter* self, const char* params) { - if (params != NULL) { + if (params) { int version; double cal_time; double cal_prec; double bias0; double bias1; double bias2; - if (sscanf(params, "%i,%lf,%lf,%lf,%lf,%lf", - &version, &cal_time, &cal_prec, &bias0, &bias1, &bias2) == 6) { + if (sscanf(params, "%i,%lf,%lf,%lf,%lf,%lf", &version, &cal_time, &cal_prec, &bias0, &bias1, &bias2) == 6) { if (version == PARAMS_VERSION) { - if (cal_time == 0) { + if (cal_time == 0) SET_UNKNOWN_TIME(self->calibration_time); - } else { + else self->calibration_time = cal_time - self->utc_offset; - } + self->cal_prec = cal_prec; self->bias[0] = bias0; self->bias[1] = bias1; diff --git a/lbs-server/src/fused/FusionEngine.c b/lbs-server/src/fused/FusionEngine.c index e3dc9c5..aea952b 100644 --- a/lbs-server/src/fused/FusionEngine.c +++ b/lbs-server/src/fused/FusionEngine.c @@ -26,10 +26,10 @@ #define DEFAULT_ROTATION_SIGMA2 SQUARE(1.0 / 256) /* [(rad/s)^2] */ -void fusion_engine_init(FusionEngine* self, double utcOffset) +bool fusion_engine_init(FusionEngine* self, double utcOffset) { LOG_FUSED_DEV(DBG_LOW, INDENT("")); - location_filter_init(&self->location_filter); + bool initialized = location_filter_init(&self->location_filter); orientation_init(&self->orientation, DEFAULT_ROTATION_SIGMA2); gyroscope_filter_init(&self->gyro_bias); time_offset_init(&self->timestamp_offset, utcOffset); @@ -39,10 +39,15 @@ void fusion_engine_init(FusionEngine* self, double utcOffset) 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); hr_calculator_init(&self->hr_calculator); wgs_conversion_init(&self->wgs_conversion); + if (!initialized) { + fusion_engine_destroy(self); + return false; + } + fusion_engine_reset(self); LOG_FUSED_DEV(DBG_LOW, UNINDENT("()")); + return true; } void fusion_engine_destroy(FusionEngine* self) @@ -85,22 +90,37 @@ void fusion_engine_reset(FusionEngine* self) time_offset_reset(&self->timestamp_offset); peace_detector_reset(&self->acc_peace_detector); peace_detector_reset(&self->gyro_peace_detector); - wgs_conversion_init(&self->wgs_conversion); + wgs_conversion_reset(&self->wgs_conversion); calibration_filter_reset(&self->calibration_filter); hr_calculator_reset(&self->hr_calculator); + self->last_gps_speed = 0.0; + SET_UNKNOWN_TIME(self->last_gps_timestamp); + SET_UNKNOWN_TIME(self->last_wps_timestamp); } /** - * @brief Check the notification conditions; if satisfied get the current - * prediction in global coordinate frame and signal it to the user. + * @brief Check the notification conditions. * @param[in] time Internal time */ static bool fusion_engine_signal_updated_location(FusionEngine* self, double time) { LOG_FUSED_DEV(DBG_ERR, "t=%.9f, fixing=%.17e", time, self->last_notification_time); - if (FL_MIN_NOTIFICATION_FILTRATION && self->engine_started && self->was_position) { - if (!IS_KNOWN_TIME(self->last_notification_time) || - time - self->last_notification_time >= FL_MIN_NOTIFICATION_INTERVAL) { + if (self->engine_started && self->was_position) { + if (FL_MAX_NOTIFICATION_INTERVAL_FROM_LAST_POS_FILTRATION_USE) { + const double last_pos_time = fmax(self->last_gps_timestamp, self->last_wps_timestamp); + if (IS_KNOWN_TIME(last_pos_time)) { + const double seconds_from_last_position = time - last_pos_time; + LOG_FUSED_DEV(LOG_DEBUG, "max time %.0f/%.0f[s]", seconds_from_last_position, FL_MAX_NOTIFICATION_INTERVAL_FROM_LAST_POS); + if (seconds_from_last_position > FL_MAX_NOTIFICATION_INTERVAL_FROM_LAST_POS) { + LOG_FUSED(LOG_DEBUG, "max time %.0f/%.0f[s] have passed -> notification skip", + seconds_from_last_position, FL_MAX_NOTIFICATION_INTERVAL_FROM_LAST_POS); + return false; + } + } + } + if (!FL_MIN_NOTIFICATION_INTERVAL_FILTRATION_USE + || !IS_KNOWN_TIME(self->last_notification_time) + || time - self->last_notification_time >= FL_MIN_NOTIFICATION_INTERVAL) { self->last_notification_time = time; return true; } @@ -131,20 +151,20 @@ static void update_wgs_conversion(FusionEngine* self) } } -bool fusion_engine_position_2d_event(FusionEngine* self, double time, - double latitude, double longitude, double sigma_of_horizontal_pos) +bool fusion_engine_wps_event(FusionEngine* self, double time, double latitude, double longitude, double hor_acc) { const double t = time_offset_correct_time(&self->timestamp_offset, time); - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e), sp=%.17em)"), t, latitude, longitude, sigma_of_horizontal_pos); + self->last_wps_timestamp = t; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e), sp=%.17em)"), t, latitude, longitude, hor_acc); self->fix_status = LOCATION_STATUS_2D_FIX; create_wgs_conversion(&self->wgs_conversion, latitude, longitude); - Vector_3d pm; /* pos measurement */ + Vector_3d pm; wgs_conversion_wgs_to_local(&self->wgs_conversion, latitude, longitude, &pm[X], &pm[Y]); pm[Z] = location_filter_get_z(&self->location_filter); - Vector_3d s2pm; /* pos measurement variance */ - s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(sigma_of_horizontal_pos); + Vector_3d s2pm; + s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(hor_acc); LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, loc_pos=(%.17e, %.17e))"), t, pm[0], pm[1]); @@ -155,20 +175,20 @@ bool fusion_engine_position_2d_event(FusionEngine* self, double time, return fusion_engine_signal_updated_location(self, t); } -bool fusion_engine_local_position_2d_event(FusionEngine* self, double time, - double x, double y, double sigma_of_horizontal_pos) +bool fusion_engine_local_wps_event(FusionEngine* self, double time, double x, double y, double hor_acc) { double t = time_offset_correct_time(&self->timestamp_offset, time); t = time; - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e), sp=%.17em)"), t, x, y, sigma_of_horizontal_pos); + self->last_wps_timestamp = t; + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e), sp=%.17em)"), t, x, y, hor_acc); self->fix_status = LOCATION_STATUS_2D_FIX; - Vector_3d pm; /* pos measurement */ + Vector_3d pm; pm[X] = x; pm[Y] = y; pm[Z] = location_filter_get_z(&self->location_filter); - Vector_3d s2pm; /* pos measurement variance */ - s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(sigma_of_horizontal_pos); + Vector_3d s2pm; + s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(hor_acc); LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, loc_pos=(%.17e, %.17e))"), t, pm[0], pm[1]); location_filter_update_position(&self->location_filter, t, pm, s2pm); @@ -177,50 +197,56 @@ bool fusion_engine_local_position_2d_event(FusionEngine* self, double time, return fusion_engine_signal_updated_location(self, t); } -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) +bool fusion_engine_gps_event(FusionEngine* self, double time, double latitude, double longitude, + double altitude, double speed, double azimuth, double hor_acc, double ver_acc) { - double t = time_offset_correct_time(&self->timestamp_offset, time); - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e, %.17e), dir=%.17e, spd=%.17e, climb=%.17e, acc=%.17e, sv=%.17em/s)"), - time, latitude, longitude, altitude, direction, speed, climb, hor_accuracy, sigma_of_speed); + const double t = time_offset_correct_time(&self->timestamp_offset, time); + self->last_gps_timestamp = t; + self->last_gps_speed = speed; + const double speed_acc = hor_acc * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA; + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e, %.17e), spd=%.17e, azim=%.17e, acc=%.17e, sv=%.17em/s)"), + time, latitude, longitude, altitude, speed, azimuth, hor_acc, speed_acc); create_wgs_conversion(&self->wgs_conversion, latitude, longitude); self->fix_status = LOCATION_STATUS_3D_FIX; - Vector_3d pm; /* pos measurement */ + Vector_3d pm; wgs_conversion_wgs_to_local(&self->wgs_conversion, latitude, longitude, &pm[X], &pm[Y]); pm[Z] = altitude; - double heading; - wgs_conversion_wgs_azimuth_to_local_bearing(&self->wgs_conversion, latitude, longitude, direction, &heading); #if (FL_KALMAN_ANGULAR_VELOCITY) + double heading; + wgs_conversion_wgs_azimuth_to_local_bearing(&self->wgs_conversion, latitude, longitude, azimuth, &heading); Vector_3d vel; location_filter_get_velocity(&self->location_filter, vel); double vhe = vector_2d_length(vel); - double azimuth = to_radians(direction); - orientation_new_azimuth(&self->orientation, t, azimuth, vhe, speed); + orientation_new_heading(&self->orientation, t, heading, vhe, speed); location_filter_update_heading_rate(&self->location_filter, t, orientation_get_heading_rate(&self->orientation), HEADING_RATE_DEV); #endif - location_filter_update_position_speed(&self->location_filter, t, pm, hor_accuracy, speed, sigma_of_speed); + location_filter_update_position_speed(&self->location_filter, t, pm, hor_acc, speed, speed_acc); self->was_position = true; update_wgs_conversion(self); return fusion_engine_signal_updated_location(self, t); } -bool fusion_engine_local_position_3d_event(FusionEngine* self, double time, - double x, double y, double altitude, - double hor_accuracy, double ver_accuracy, double speed, double sigma_of_speed, - double direction, double climb, double sigma_of_climb) +/** + * For ips_devel test + */ +bool fusion_engine_local_gps_event(FusionEngine* self, double time, double x, double y, + double altitude, double speed, double bearing, double hor_acc, double ver_acc) { double t = time_offset_correct_time(&self->timestamp_offset, time); t = time; - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e, %.17e), dir=%.17e, spd=%.17e, climb=%.17e, acc=%.17e, sv=%.17em/s)"), - time, x, y, altitude, direction, speed, climb, hor_accuracy, sigma_of_speed); + self->last_gps_timestamp = t; + self->last_gps_speed = speed; + const double speed_acc = hor_acc * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA; + + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, pos=(%.17e, %.17e, %.17e), dir=%.17e, spd=%.17e, acc=%.17e, sv=%.17em/s)"), + time, x, y, altitude, bearing, speed, hor_acc, speed_acc); - Vector_3d pm; /* pos measurement */ + Vector_3d pm; self->fix_status = LOCATION_STATUS_3D_FIX; pm[X] = x; @@ -232,19 +258,19 @@ bool fusion_engine_local_position_3d_event(FusionEngine* self, double time, location_filter_get_velocity(&self->location_filter, vel); double vhe = vector_2d_length(vel); double azimuth = to_radians(direction); - orientation_new_azimuth(&self->orientation, t, azimuth, vhe, speed); + orientation_new_heading(&self->orientation, t, azimuth, vhe, speed); location_filter_update_heading_rate(&self->location_filter, t, orientation_get_heading_rate(&self->orientation), HEADING_RATE_DEV); #endif - location_filter_update_position_speed(&self->location_filter, t, pm, hor_accuracy, speed, sigma_of_speed); + location_filter_update_position_speed(&self->location_filter, t, pm, hor_acc, speed, speed_acc); self->was_position = true; return fusion_engine_signal_updated_location(self, t); } void fusion_engine_get_wgs_location(FusionEngine* self, WgsLocation* location) { - if (location == NULL) { + if (!location) { LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(), NULL pointer")); return; } @@ -275,9 +301,12 @@ void fusion_engine_get_wgs_location(FusionEngine* self, WgsLocation* location) location->climb_dev = location_filter_get_climb_dev(&self->location_filter); } +/** + * For ips_devel test + */ void fusion_engine_get_local_position(FusionEngine* self, WgsLocation* location) { - if (location == NULL) { + if (!location) { LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(), NULL pointer")); return; } @@ -304,18 +333,38 @@ void fusion_engine_get_local_position(FusionEngine* self, WgsLocation* location) location->climb_dev = location_filter_get_climb_dev(&self->location_filter); } -bool fusion_engine_speed_event(FusionEngine* self, double time, double speed, double sigma_of_speed) +bool fusion_engine_pedometer_event(FusionEngine* self, double time, double speed, double speed_acc) { - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPEED(), t=%.9f, speed=%.17e, sigma=%.17e"), time, speed, sigma_of_speed); - location_filter_update_speed(&self->location_filter, time, speed, sigma_of_speed); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("PEDOMETER(), t=%.9f, speed=%.17e, sigma=%.17e"), time, speed, speed_acc); + if (FL_PEDOMETER_DURING_DRIVING_FILTRATION_USE + && IS_KNOWN_TIME(self->last_gps_timestamp)) { + const double seconds_from_last_gps = time - self->last_gps_timestamp; + if (seconds_from_last_gps <= FL_PEDOMETER_SKIP_MAX_INTERVAL_FROM_LAST_GPS + && fabs(self->last_gps_speed) >= FL_PEDOMETER_SKIP_MIN_SPEED_FROM_LAST_GPS) { + LOG_FUSED(LOG_DEBUG, "seconds_from_last_gps = %.3f/%.3f[s] && last_gps_speed = %.2f/%.1f[m/s] -> PEDOMETER SKIP", + seconds_from_last_gps, FL_PEDOMETER_SKIP_MAX_INTERVAL_FROM_LAST_GPS, + self->last_gps_speed, FL_PEDOMETER_SKIP_MIN_SPEED_FROM_LAST_GPS); + return false; + } + } + if (FL_MIN_SPEED_FROM_PEDOMETER_FILTRATION_USE && fabs(speed) < FL_MIN_SPEED_FROM_PEDOMETER) + return false; + + return fusion_engine_speed_event(self, time, speed, speed_acc); +} + +bool fusion_engine_speed_event(FusionEngine* self, double time, double speed, double speed_acc) +{ + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPEED(), t=%.9f, speed=%.17e, sigma=%.17e"), time, speed, speed_acc); + location_filter_update_speed(&self->location_filter, time, speed, speed_acc); return fusion_engine_signal_updated_location(self, time); } static void on_new_heading(FusionEngine* self) { LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.9f, h=%.17e"), - orientation_get_heading_time(&self->orientation), - orientation_get_heading(&self->orientation)); + orientation_get_heading_time(&self->orientation), orientation_get_heading(&self->orientation)); + if (hr_calculator_new_heading(&self->hr_calculator, orientation_get_heading_time(&self->orientation), orientation_get_heading(&self->orientation) @@ -329,7 +378,7 @@ static void on_new_heading(FusionEngine* self) bool fusion_engine_acc_event(FusionEngine* self, double time, double x, double y, double z) { - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, a=(%.17e, %.17e, %.17e)[m/s^2])"), time, x, y, z); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.9f, a=(%.17e, %.17e, %.17e)[m/s^2]"), time, x, y, z); time_offset_reference_time(&self->timestamp_offset, time); Vector_3d acc = { x, y, z }; @@ -342,7 +391,7 @@ bool fusion_engine_acc_event(FusionEngine* self, double time, double x, double y bool fusion_engine_gyro_event(FusionEngine* self, double time, double x, double y, double z) { - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, a=(%.17e, %.17e, %.17e))"), time, x, y, z); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.9f, a=(%.17e, %.17e, %.17e)[rad/s]"), time, x, y, z); 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); diff --git a/lbs-server/src/fused/FusionEngine.h b/lbs-server/src/fused/FusionEngine.h index fa62fed..7e8de02 100644 --- a/lbs-server/src/fused/FusionEngine.h +++ b/lbs-server/src/fused/FusionEngine.h @@ -41,9 +41,9 @@ extern "C" { #endif 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). */ + 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 { @@ -62,29 +62,32 @@ typedef struct { PeaceDetector gyro_peace_detector; CalibrationFilter calibration_filter; HeadingRateCalculator hr_calculator; + double last_gps_speed; + double last_gps_timestamp; + double last_wps_timestamp; } FusionEngine; typedef struct { - double time; /*/< Event time in [s]. */ - double utc_time; /*/< Event UTC 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] */ + double time; /** Event time in [s]. */ + double utc_time; /** Event UTC 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 in [m/s] */ + double direction; /** The course made in degrees relative to true north clockwise. The value is always in the range [0.0, 360.0] degree. */ + double climb; /** The vertical speed in [m/s] */ + double hor_dev; /** Accuracy (standard deviation) of horizontal position in [m] */ + double ver_dev; /** Accuracy (standard deviation) of vertical position in [m] */ + double speed_dev; /** Accuracy (standard deviation) of horizontal velocity in [m/s] */ + double climb_dev; /** Accuracy (standard deviation) of vertical velocity in [m/s] */ } WgsLocation; /** * @brief Initialization of the engine. */ -extern void fusion_engine_init(FusionEngine* self, double utcOffset); +extern bool fusion_engine_init(FusionEngine* self, double utcOffset); -/** Cleanup of the singleton unit (static destructor). This corresponds to service unload. */ +/** Destructor. This corresponds to service unload. */ extern void fusion_engine_destroy(FusionEngine* self); /** @@ -110,9 +113,9 @@ extern void fusion_engine_reset(FusionEngine* self); * @param[in] time Internal time in [s]. * @param[in] latitude Latitude in degrees. * @param[in] longitude Longitude in degrees. - * @param[in] sigma Horizontal standard deviation of provided position in [m]. + * @param[in] hor_acc Horizontal accuracy (standard deviation) of provided position in [m]. */ -extern bool fusion_engine_position_2d_event(FusionEngine* self, double time, double latitude, double longitude, double sigma); +extern bool fusion_engine_wps_event(FusionEngine* self, double time, double latitude, double longitude, double hor_acc); /** * Used by IpsDevel for development. @@ -120,9 +123,9 @@ extern bool fusion_engine_position_2d_event(FusionEngine* self, double time, dou * @param[in] time Internal time in [s]. * @param[in] x X coordinate in [m]. * @param[in] y Y coordinate in [m]. - * @param[in] sigma Horizontal standard deviation of provided position in [m]. + * @param[in] hor_acc Horizontal accuracy (standard deviation) of provided position in [m]. */ -extern bool fusion_engine_local_position_2d_event(FusionEngine* self, double time, double x, double y, double sigma); +extern bool fusion_engine_local_wps_event(FusionEngine* self, double time, double x, double y, double hor_acc); /** * @brief Process 3D position along with 3D velocity (GPS). @@ -133,17 +136,13 @@ extern bool fusion_engine_local_position_2d_event(FusionEngine* self, double tim * @param[in] latitude Latitude in degrees. * @param[in] longitude Longitude in degrees. * @param[in] altitude Altitude in [m]. - * @param[in] hor_accuracy Horizontal standard deviation of provided position in [m]. - * @param[in] ver_accuracy Standard deviation of altitude in [m]. * @param[in] speed Horizontal speed in [m/s]. - * @param[in] sigma_of_speed Standard deviation of horizontal speed in [m/s]. - * @param[in] direction Direction (azimuth) of movement in degrees. - * @param[in] climb Vertical speed in [m/s]. - * @param[in] sigma_of_climb Standard deviation of vertical speed in [m/s]. + * @param[in] azimuth Direction (azimuth) of movement in degrees. + * @param[in] hor_acc Horizontal accuracy (standard deviation) of provided position in [m]. + * @param[in] ver_acc Accuracy (standard deviation) of altitude in [m]. */ -extern bool fusion_engine_position_3d_event(FusionEngine* self, double time, - double latitude, double longitude, double altitude, double hor_accuracy, double ver_accuracy, - double speed, double sigma_of_speed, double direction, double climb, double sigma_of_climb); +extern bool fusion_engine_gps_event(FusionEngine* self, double time, double latitude, double longitude, + double altitude, double speed, double azimuth, double hor_acc, double ver_acc); /** * Used by IpsDevel for development. @@ -152,46 +151,42 @@ extern bool fusion_engine_position_3d_event(FusionEngine* self, double time, * @param[in] x X local coordinate in [m]. * @param[in] y Y local coordinate in [m]. * @param[in] altitude Altitude in [m]. - * @param[in] hor_accuracy Horizontal standard deviation of provided position in [m]. - * @param[in] ver_accuracy Standard deviation of altitude in [m]. * @param[in] speed Horizontal speed in [m/s]. - * @param[in] sigma_of_speed Standard deviation of horizontal speed in [m/s]. * @param[in] direction Direction (azimuth) of movement in degrees. - * @param[in] climb Vertical speed in [m/s]. - * @param[in] sigma_of_climb Standard deviation of vertical speed in [m/s]. + * @param[in] hor_acc Horizontal accuracy (standard deviation) of provided position in [m]. + * @param[in] ver_acc Accuracy (standard deviation) of altitude in [m]. */ -extern bool fusion_engine_local_position_3d_event(FusionEngine* self, double time, - double x, double y, double altitude, double hor_accuracy, double ver_accuracy, - double speed, double sigma_of_speed, double direction, double climb, double sigma_of_climb); +extern bool fusion_engine_local_gps_event(FusionEngine* self, double time, double x, double y, + double altitude, double speed, double bearing, double hor_acc, double ver_acc); /** * @brief Process speed measurement events. - * @param[in] time Internal time in [s]. - * @param[in] speed Horizontal speed in [m/s]. - * @param[in] sigma_of_speed Standard deviation of horizontal speed in [m/s]. + * @param[in] time Internal time in [s]. + * @param[in] speed Horizontal speed in [m/s]. + * @param[in] speed_acc Accuracy (standard deviation) of horizontal speed in [m/s]. */ -extern bool fusion_engine_speed_event(FusionEngine* self, double time, double speed, double sigma_of_speed); +extern bool fusion_engine_speed_event(FusionEngine* self, double time, double speed, double speed_acc); /** * @brief Process pedometer events. The time offset is corrected if necessary. - * @param[in] time Sensory time-stamp - * @param[in] speed Speed in [m/s]. - * @param[in] sigma_of_speed Speed deviation in [m/s]. + * @param[in] time Sensory time-stamp + * @param[in] speed Speed in [m/s]. + * @param[in] speed_acc Accuracy (standard deviation) of speed in [m/s]. */ 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. * @param[in] time Sensory time-stamp in [s]. - * @param[in] x X coordinate of angular rotation velocity vector in [rad/s]. - * @param[in] y Y coordinate of angular rotation velocity vector in [rad/s]. - * @param[in] z Z coordinate of angular rotation velocity vector in [rad/s]. + * @param[in] x X coordinate of angular rotation velocity vector in [rad/s]. + * @param[in] y Y coordinate of angular rotation velocity vector in [rad/s]. + * @param[in] z Z coordinate of angular rotation velocity vector in [rad/s]. */ extern bool fusion_engine_gyro_event(FusionEngine* self, double time, double x, double y, double z); /** * @brief Process heading events. The time offset is corrected if necessary. - * @param[in] time Sensory time-stamp in [s]. + * @param[in] time Sensory time-stamp in [s]. * @param[in] heading Heading in [rad] anticlockwise. */ extern bool fusion_engine_heading_event(FusionEngine* self, @@ -199,33 +194,33 @@ extern bool fusion_engine_heading_event(FusionEngine* self, /** * @brief Process heading rate events. The time offset is corrected if necessary. - * @param[in] time Sensory time-stamp in [s]. - * @param[in] heading_rate Heading in [rad/s] anticlockwise. - * @param[in] heading_rate_deviation Heading change rate in [rad/s] anticlockwise. + * @param[in] time Sensory time-stamp in [s]. + * @param[in] heading_rate Heading change rate in [rad/s] anticlockwise. + * @param[in] heading_rate_acc Accuracy (standard deviation) of heading change rate in [rad/s] anticlockwise. */ extern bool fusion_engine_heading_rate_event(FusionEngine* self, - double time, double heading_rate, double heading_rate_deviation); + double time, double heading_rate, double heading_rate_acc); /** * @brief Process heading events. The time offset is corrected if necessary. - * @param[in] time Sensory time-stamp in [s]. - * @param[in] speed Horizontal speed in [m/s]. - * @param[in] speed_deviation Standard deviation of horizontal speed in [m/s]. - * @param[in] heading_rate Heading in [rad] anticlockwise. - * @param[in] heading_rate_deviation Heading change rate in [rad/s] anticlockwise. + * @param[in] time Sensory time-stamp in [s]. + * @param[in] speed Horizontal speed in [m/s]. + * @param[in] speed_acc Accuracy (standard deviation) of horizontal speed in [m/s]. + * @param[in] heading_rate Heading change rate in [rad/s] anticlockwise. + * @param[in] heading_rate_acc Accuracy (standard deviation) of heading change rate in [rad/s] anticlockwise. */ bool fusion_engine_speed_heading_rate_event(FusionEngine* self, double time, - double speed, double speed_deviation, double heading_rate, double heading_rate_deviation); + double speed, double speed_acc, double heading_rate, double heading_rate_acc); /** - * @brief Retrieve the last processed location, typically during the on_location_changed notification. + * @brief Retrieve the last processed location. * @param[out] location Current WGS location */ extern void fusion_engine_get_wgs_location(FusionEngine* self, WgsLocation* location); /** * Used by IpsDevel for development. - * @brief Retrieve the last processed position in local coordinates, typically during the on_location_changed notification. + * @brief Retrieve the last processed position in local coordinates. * @param[out] location Current local position */ extern void fusion_engine_get_local_position(FusionEngine* self, WgsLocation* location); @@ -276,6 +271,14 @@ extern char* fusion_engine_get_calibration(FusionEngine* self); */ extern void fusion_engine_set_calibration(FusionEngine* self, const char* params); +/** + * @brief Process speed measurement events from pedometer. + * @param[in] time Internal time in [s]. + * @param[in] speed Horizontal speed in [m/s]. + * @param[in] speed_acc Accuracy (standard deviation) of horizontal speed in [m/s]. + */ +extern bool fusion_engine_pedometer_event(FusionEngine* self, double time, double speed, double speed_acc); + #ifdef __cplusplus } #endif diff --git a/lbs-server/src/fused/HeadingRateCalculator.c b/lbs-server/src/fused/HeadingRateCalculator.c index 7426dd1..8450acb 100644 --- a/lbs-server/src/fused/HeadingRateCalculator.c +++ b/lbs-server/src/fused/HeadingRateCalculator.c @@ -37,18 +37,18 @@ void hr_calculator_set_heading_rate_deviation(HeadingRateCalculator* self, doubl bool hr_calculator_new_heading(HeadingRateCalculator* self, double timestamp, double heading) { - if (!IS_KNOWN_TIME(timestamp)) { + if (!IS_KNOWN_TIME(timestamp)) return false; - } + if (!IS_KNOWN_TIME(self->timestamp)) { self->timestamp = timestamp; self->heading = heading; return false; } - if (timestamp <= self->timestamp - || timestamp - self->timestamp < self->report_rate) { + + if (timestamp <= self->timestamp || timestamp - self->timestamp < self->report_rate) return false; - } + const double headingChange = correct_angle(heading - self->heading); self->heading = heading; const double dt = timestamp - self->timestamp; diff --git a/lbs-server/src/fused/KalmanFilter.c b/lbs-server/src/fused/KalmanFilter.c index 7422e31..767e38a 100644 --- a/lbs-server/src/fused/KalmanFilter.c +++ b/lbs-server/src/fused/KalmanFilter.c @@ -15,23 +15,31 @@ */ #include "KalmanFilter.h" +#include "debug_util.h" #include -void kalman_filter_init(KalmanFilter *self, const unsigned int stateVectorSize) +bool kalman_filter_init(KalmanFilter *self, const unsigned int stateVectorSize) { self->mStateVectorSize = stateVectorSize; self->stateVector = (double *)malloc(self->mStateVectorSize * sizeof(double)); self->mCovariance = matrix_create_empty(self->mStateVectorSize, self->mStateVectorSize); + + if (self->stateVector && self->mCovariance) + return true; + + kalman_filter_destroy(self); + return false; } void kalman_filter_destroy(KalmanFilter *self) { - if (self->stateVector != NULL) { + if (self->stateVector) { free(self->stateVector); self->stateVector = NULL; } - if (self->mCovariance != NULL) { + + if (self->mCovariance) { matrix_destroy(self->mCovariance); self->mCovariance = NULL; } @@ -67,8 +75,7 @@ double kalman_filter_get_variable_variance(const KalmanFilter *self, const unsig * V := stateTransitionFunction(V) * C := stateTransitionMatrix * C * stateTransitionMatrix^T + noiseMatrix */ -void kalman_filter_predict(KalmanFilter *self, - kalman_filter_state_transition_function stateTransitionFunction, +void kalman_filter_predict(KalmanFilter *self, kalman_filter_state_transition_function stateTransitionFunction, const Matrix *stateTransitionMatrix, const Matrix *noiseMatrix) { double newStateVector[self->mStateVectorSize]; @@ -78,12 +85,13 @@ void kalman_filter_predict(KalmanFilter *self, Matrix *m3 = matrix_multiply_by_matrix(m1, m2); Matrix *m4 = matrix_add_matrix(m3, noiseMatrix); - kalman_filter_set_state(self, newStateVector, m4->mData); + if (m4) + kalman_filter_set_state(self, newStateVector, m4->mData); - matrix_destroy(m1); - matrix_destroy(m2); - matrix_destroy(m3); matrix_destroy(m4); + matrix_destroy(m3); + matrix_destroy(m2); + matrix_destroy(m1); } /** @@ -100,30 +108,31 @@ void kalman_filter_correct(KalmanFilter *self, const Matrix *measurementMatrix, Matrix *mc_dot_mmt = matrix_multiply_by_matrix(mc, mmt); Matrix *s = matrix_add_vector_diagonal(mc_dot_mmt, noiseVector); Matrix *si = matrix_invert(s); - if (si != NULL) {/* Should never happen for proper noiseVector (i.e. with positive components). */ - Matrix *mct = matrix_transpose(mc); - Matrix *k = matrix_multiply_by_matrix(mct, si); - Matrix *k_dot_mc = matrix_multiply_by_matrix(k, mc); - Matrix *cov_sub_k_dot_mc = matrix_sub_matrix(self->mCovariance, k_dot_mc); - Matrix *newCovariance = matrix_symmetrize(cov_sub_k_dot_mc); + Matrix *mct = matrix_transpose(mc); + Matrix *k = matrix_multiply_by_matrix(mct, si); + Matrix *k_dot_mc = matrix_multiply_by_matrix(k, mc); + Matrix *cov_sub_k_dot_mc = matrix_sub_matrix(self->mCovariance, k_dot_mc); + Matrix *newCovariance = matrix_symmetrize(cov_sub_k_dot_mc); + if (newCovariance) { matrix_set_from_values(self->mCovariance, newCovariance->mData); - double *newState = matrix_multiply_by_vector(k, innovation, self->mStateVectorSize); - unsigned i; - for (i = self->mStateVectorSize; i; ) { - i--; - newState[i] += self->stateVector[i]; - } - - kalman_filter_set_state(self, newState, newCovariance->mData); + double *newState = matrix_multiply_by_vector(k, innovation); + if (newState) { + unsigned i; + for (i = self->mStateVectorSize; i; ) { + i--; + newState[i] += self->stateVector[i]; + } - free(newState); - matrix_destroy(newCovariance); - matrix_destroy(cov_sub_k_dot_mc); - matrix_destroy(k_dot_mc); - matrix_destroy(k); - matrix_destroy(mct); - matrix_destroy(si); + kalman_filter_set_state(self, newState, newCovariance->mData); + free(newState); + } } + matrix_destroy(newCovariance); + matrix_destroy(cov_sub_k_dot_mc); + matrix_destroy(k_dot_mc); + matrix_destroy(k); + matrix_destroy(mct); + matrix_destroy(si); matrix_destroy(s); matrix_destroy(mc_dot_mmt); matrix_destroy(mmt); diff --git a/lbs-server/src/fused/KalmanFilter.h b/lbs-server/src/fused/KalmanFilter.h index 1a6a754..430fd57 100644 --- a/lbs-server/src/fused/KalmanFilter.h +++ b/lbs-server/src/fused/KalmanFilter.h @@ -24,6 +24,8 @@ #include "Matrix.h" +#include + #ifdef __cplusplus extern "C" { #endif @@ -40,7 +42,7 @@ typedef void (*kalman_filter_state_transition_function)(const double* in, double * @brief Initialize KalmanFilter. * @param stateVectorSize Size of state vector. */ -extern void kalman_filter_init(KalmanFilter *self, const unsigned int stateVectorSize); +extern bool kalman_filter_init(KalmanFilter *self, const unsigned int stateVectorSize); /** * @brief Deinitialize/destroy KalmanFilter. diff --git a/lbs-server/src/fused/LocationFilter.c b/lbs-server/src/fused/LocationFilter.c index 519db28..f7bfe08 100644 --- a/lbs-server/src/fused/LocationFilter.c +++ b/lbs-server/src/fused/LocationFilter.c @@ -26,11 +26,15 @@ static Timestamp seconds_to_timestamp(double time) return IS_KNOWN_TIME(time) ? (Timestamp)(time * 1e9 + 0.5) : UNKNOWN_TIMESTAMP; } -void location_filter_init(LocationFilter* self) +bool location_filter_init(LocationFilter* self) { position_fusion_1d_init(&self->fusion1D); - position_fusion_2d_init(&self->fusion2D); - location_filter_reset(self); + if (position_fusion_2d_init(&self->fusion2D)) { + location_filter_reset(self); + return true; + } + location_filter_destroy(self); + return false; } void location_filter_destroy(LocationFilter* self) @@ -90,11 +94,9 @@ void location_filter_set_predicted_state(LocationFilter* self, const_Vector_3d_r position_fusion_1d_set_position(&self->fusion1D, pos[Z]); } -void location_filter_update_position(LocationFilter* self, - double time, const_Vector_3d_ref pm, const_Vector_3d_ref s2pm) +void location_filter_update_position(LocationFilter* self, double time, const_Vector_3d_ref pm, const_Vector_3d_ref s2pm) { - position_fusion_2d_update_position(&self->fusion2D, seconds_to_timestamp(time), - pm[X], pm[Y], sqrt((s2pm[X] + s2pm[Y]) / 2)); + position_fusion_2d_update_position(&self->fusion2D, seconds_to_timestamp(time), pm[X], pm[Y], sqrt((s2pm[X] + s2pm[Y]) / 2)); position_fusion_1d_update_position(&self->fusion1D, pm[Z], s2pm[Z]); self->was_position = true; } @@ -102,15 +104,13 @@ void location_filter_update_position(LocationFilter* self, void location_filter_update_position_speed(LocationFilter* self, double time, const_Vector_3d_ref pm, double pos_dev, double speed, double speed_dev) { - position_fusion_2d_update_position_speed(&self->fusion2D, seconds_to_timestamp(time), - pm[X], pm[Y], pos_dev, speed, speed_dev); + position_fusion_2d_update_position_speed(&self->fusion2D, seconds_to_timestamp(time), pm[X], pm[Y], pos_dev, speed, speed_dev); position_fusion_1d_update_position(&self->fusion1D, pm[Z], pos_dev); self->was_position = true; } void location_filter_update_position_velocity(LocationFilter* self, double time, - const_Vector_3d_ref pm, const_Vector_3d_ref vm, - const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm) + const_Vector_3d_ref pm, const_Vector_3d_ref vm, const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm) { position_fusion_2d_update_position_speed(&self->fusion2D, seconds_to_timestamp(time), pm[X], pm[Y], sqrt((s2pm[X] + s2pm[Y]) / 2), @@ -122,23 +122,19 @@ void location_filter_update_position_velocity(LocationFilter* self, double time, void location_filter_update_speed(LocationFilter* self, double time, double speed, double sigma_of_speed) { - if (self->was_position) { + if (self->was_position) position_fusion_2d_update_speed(&self->fusion2D, seconds_to_timestamp(time), speed, sigma_of_speed); - } } -void location_filter_update_heading_rate(LocationFilter* self, - double time, double heading_rate, double heading_rate_deviation) +void location_filter_update_heading_rate(LocationFilter* self, double time, double heading_rate, double heading_rate_deviation) { - if (self->was_position) { - position_fusion_2d_update_angular_speed(&self->fusion2D, seconds_to_timestamp(time), - heading_rate, heading_rate_deviation); - } + if (self->was_position) + position_fusion_2d_update_angular_speed(&self->fusion2D, seconds_to_timestamp(time), heading_rate, heading_rate_deviation); + } void location_filter_update_speed_heading_rate(LocationFilter* self, - double time, double speed, double speed_deviation, - double heading_rate, double heading_rate_deviation) + double time, double speed, double speed_deviation, double heading_rate, double heading_rate_deviation) { if (self->was_position) { position_fusion_2d_update_speed_angular_speed(&self->fusion2D, seconds_to_timestamp(time), diff --git a/lbs-server/src/fused/LocationFilter.h b/lbs-server/src/fused/LocationFilter.h index 6424111..4909691 100644 --- a/lbs-server/src/fused/LocationFilter.h +++ b/lbs-server/src/fused/LocationFilter.h @@ -41,7 +41,7 @@ typedef struct { /** * @brief Initialization of the filter (constructor). */ -extern void location_filter_init(LocationFilter* self); +extern bool location_filter_init(LocationFilter* self); extern void location_filter_destroy(LocationFilter* self); diff --git a/lbs-server/src/fused/Matrix.c b/lbs-server/src/fused/Matrix.c index adf718a..af2e152 100644 --- a/lbs-server/src/fused/Matrix.c +++ b/lbs-server/src/fused/Matrix.c @@ -25,10 +25,17 @@ Matrix* matrix_create_empty(const unsigned int rows, const unsigned int columns) { Matrix* self = malloc(sizeof(Matrix)); - if (self != NULL) { + if (self) { self->mRows = rows; self->mColumns = columns; self->mData = malloc(rows * columns * sizeof(double)); + if (!self->mData) { + free(self); + self = NULL; + } + } + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Can not allocate space for matrix (%u x %u)"), rows, columns); } return self; } @@ -36,86 +43,100 @@ Matrix* matrix_create_empty(const unsigned int rows, const unsigned int columns) Matrix* matrix_create_from_values(const unsigned int rows, const unsigned int columns, const void* in_values_pointer) { Matrix* self = matrix_create_empty(rows, columns); - memcpy(self->mData, in_values_pointer, self->mRows * self->mColumns * sizeof(double)); - return self; -} - -Matrix* matrix_create_from_values_start_column(const unsigned int rows, const unsigned int columns, - const void* in_values_pointer, const unsigned int start_column) -{ - Matrix* self = matrix_create_empty(rows, columns - start_column); - double (*self_values)[self->mColumns] = self->mData; - const double *values = in_values_pointer; - unsigned int row; - for (row = self->mRows; row; ) { - --row; - memcpy(&self_values[row][0], &values[row * columns + start_column], self->mColumns * sizeof(double)); - } + matrix_set_from_values(self, in_values_pointer); return self; } Matrix* matrix_create_from_copy(const Matrix* in) { - Matrix* self = matrix_create_from_values(in->mRows, in->mColumns, in->mData); - return self; + if (!in) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } + return matrix_create_from_values(in->mRows, in->mColumns, in->mData); } void matrix_destroy(Matrix* self) { - if (self != NULL) { - if (self->mData != NULL) { - free(self->mData); - } + if (self) { + free(self->mData); free(self); - self = NULL; } } void matrix_set_from_values(Matrix* self, const void* in_values_pointer) { - memcpy(self->mData, in_values_pointer, self->mRows * self->mColumns * sizeof(double)); -} - -void matrix_set_from_copy(Matrix* self, const Matrix* in) -{ - matrix_set_from_values(self, in->mData); + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + } else if (!in_values_pointer) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Source data is NULL")); + } else { + memcpy(self->mData, in_values_pointer, self->mRows * self->mColumns * sizeof(double)); + } } double matrix_get(const Matrix* self, const unsigned int row, const unsigned int column) { - double (*self_values)[self->mColumns] = self->mData; + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return 0.0; + } + const double (*self_values)[self->mColumns] = self->mData; return self_values[row][column]; } Matrix* matrix_multiply_by_matrix(const Matrix* self, const Matrix* in) { - const double (*self_values)[self->mColumns] = self->mData; - const double (*in_values)[in->mColumns] = in->mData; + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } + if (!in) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Second matrix is NULL")); + return NULL; + } + if (self->mColumns != in->mRows) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Can not multiply matrixes (%u x %u) and (%u x %u)"), + self->mRows, self->mColumns, in->mRows, in->mColumns); + return NULL; + } Matrix* result = matrix_create_empty(self->mRows, in->mColumns); - double (*result_values)[result->mColumns] = result->mData; - unsigned int row; - for (row = self->mRows; row; ) { - --row; - unsigned int col; - for (col = in->mColumns; col; ) { - --col; - double v = 0; - unsigned int i; - for (i = self->mColumns; i; ) { - --i; - v += self_values[row][i] * in_values[i][col]; + if (result) { + const double (*self_values)[self->mColumns] = self->mData; + const double (*in_values)[in->mColumns] = in->mData; + double (*result_values)[result->mColumns] = result->mData; + unsigned int row; + for (row = self->mRows; row; ) { + --row; + unsigned int col; + for (col = in->mColumns; col; ) { + --col; + double v = 0; + unsigned int i; + for (i = self->mColumns; i; ) { + --i; + v += self_values[row][i] * in_values[i][col]; + } + result_values[row][col] = v; } - result_values[row][col] = v; } } return result; } -double* matrix_multiply_by_vector(const Matrix* self, const double* vector, const unsigned int vector_length) +double* matrix_multiply_by_vector(const Matrix* self, const double* vector) { - const double (*self_values)[self->mColumns] = self->mData; + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } + if (!vector) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Vector is NULL")); + return NULL; + } double *result = (double *)malloc(self->mRows * sizeof(double)); - if (result != NULL) { + if (result) { + const double (*self_values)[self->mColumns] = self->mData; unsigned int row; for (row = self->mRows; row; ) { --row; @@ -133,17 +154,32 @@ double* matrix_multiply_by_vector(const Matrix* self, const double* vector, cons Matrix* matrix_add_matrix(const Matrix* self, const Matrix* in) { - const double (*self_values)[self->mColumns] = self->mData; - const double (*in_values)[in->mColumns] = in->mData; + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } + if (!in) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Second matrix is NULL")); + return NULL; + } + if (self->mColumns != in->mColumns || self->mRows != in->mRows) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Can not add matrixes (%u x %u) and (%u x %u)"), + self->mRows, self->mColumns, in->mRows, in->mColumns); + return NULL; + } Matrix* result = matrix_create_empty(self->mRows, self->mColumns); - double (*result_values)[result->mColumns] = result->mData; - unsigned int row; - for (row = self->mRows; row; ) { - --row; - unsigned int col; - for (col = self->mColumns; col; ) { - --col; - result_values[row][col] = self_values[row][col] + in_values[row][col]; + if (result) { + const double (*self_values)[self->mColumns] = self->mData; + const double (*in_values)[in->mColumns] = in->mData; + double (*result_values)[result->mColumns] = result->mData; + unsigned int row; + for (row = self->mRows; row; ) { + --row; + unsigned int col; + for (col = self->mColumns; col; ) { + --col; + result_values[row][col] = self_values[row][col] + in_values[row][col]; + } } } return result; @@ -151,46 +187,85 @@ Matrix* matrix_add_matrix(const Matrix* self, const Matrix* in) Matrix* matrix_sub_matrix(const Matrix* self, const Matrix* in) { - const double (*self_values)[self->mColumns] = self->mData; - const double (*in_values)[in->mColumns] = in->mData; + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } + if (!in) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Second matrix is NULL")); + return NULL; + } + if (self->mColumns != in->mColumns || self->mRows != in->mRows) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Can not subtract matrixes (%u x %u) and (%u x %u)"), + self->mRows, self->mColumns, in->mRows, in->mColumns); + return NULL; + } Matrix* result = matrix_create_empty(self->mRows, self->mColumns); - double (*result_values)[result->mColumns] = result->mData; - unsigned int row; - for (row = 0; row < self->mRows; row++) { - unsigned int col; - for (col = self->mColumns; col; ) { - --col; - result_values[row][col] = self_values[row][col] - in_values[row][col]; + if (result) { + const double (*self_values)[self->mColumns] = self->mData; + const double (*in_values)[in->mColumns] = in->mData; + double (*result_values)[result->mColumns] = result->mData; + unsigned int row; + for (row = self->mRows; row; ) { + --row; + unsigned int col; + for (col = self->mColumns; col; ) { + --col; + result_values[row][col] = self_values[row][col] - in_values[row][col]; + } } } return result; } -Matrix* matrix_add_vector_diagonal(Matrix* self, const double* vector) +Matrix* matrix_add_vector_diagonal(const Matrix* self, const double* vector) { + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } + if (!vector) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Vector is NULL")); + return NULL; + } + if (self->mRows != self->mColumns) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix must be square but is (%u x %u)"), + self->mRows, self->mColumns); + return 0; + } + const unsigned int size = self->mRows; Matrix* result = matrix_create_from_copy(self); - double (*result_values)[result->mColumns] = result->mData; - unsigned int row; - for (row = self->mRows; row; ) { - --row; - result_values[row][row] += vector[row]; + if (result) { + double (*result_values)[size] = result->mData; + unsigned int row; + for (row = size; row; ) { + --row; + result_values[row][row] += vector[row]; + } } return result; } -/** - * @return Determinant of matrix. - */ double matrix_det(const Matrix* self) { - const double (*self_values)[self->mColumns] = self->mData; - if (self->mColumns == 1) { + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return 0.0; + } + if (self->mRows != self->mColumns) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix must be square but is (%u x %u)"), + self->mRows, self->mColumns); + return 0; + } + const unsigned int size = self->mRows; + const double (*self_values)[size] = self->mData; + if (size == 1) { return self_values[0][0]; } - if (self->mColumns == 2) { + if (size == 2) { return self_values[0][0] * self_values[1][1] - self_values[1][0] * self_values[0][1]; } - if (self->mColumns == 3) { + if (size == 3) { return self_values[0][0] * (self_values[1][1] * self_values[2][2] - self_values[1][2] * self_values[2][1]) + self_values[0][1] * (self_values[1][2] * self_values[2][0] - self_values[1][0] * self_values[2][2]) + self_values[0][2] * (self_values[1][0] * self_values[2][1] - self_values[1][1] * self_values[2][0]); @@ -198,7 +273,8 @@ double matrix_det(const Matrix* self) double maxValue = 0; int maxColumn = 0; unsigned int col; - for (col = 0; col < self->mColumns; col++) { + for (col = size; col; ) { + --col; const double v = fabs(self_values[0][col]); if (v > maxValue) { maxValue = v; @@ -209,53 +285,61 @@ double matrix_det(const Matrix* self) return 0; } maxValue = self_values[0][maxColumn]; - Matrix* m = matrix_create_empty(self->mRows - 1, self->mColumns - 1); + Matrix* m = matrix_create_empty(size - 1, size - 1); + if (!m) { + return 0.0; + } double (*m_values)[m->mColumns] = m->mData; - for (col = 0; col < self->mColumns - 1; col++) { + for (col = size - 1; col; ) { + --col; const int sColumn = col < maxColumn ? col : col + 1; const double c = self_values[0][sColumn] / maxValue; unsigned int row; - for (row = 0; row < self->mRows - 1; row++) { - m_values[row][col] = self_values[row + 1][sColumn] - self_values[row + 1][maxColumn] * c; + for (row = size - 1; row; ) { + const double d = self_values[row][sColumn] - self_values[row][maxColumn] * c; + m_values[row--][col] = d; } } - double result = ((maxColumn & 1) == 0 ? 1 : -1) * maxValue * matrix_det(m); + const double result = ((maxColumn & 1) == 0 ? 1 : -1) * maxValue * matrix_det(m); matrix_destroy(m); return result; } -/** - * @return Inverted matrix. - */ Matrix* matrix_invert(const Matrix* self) { + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } if (self->mRows != self->mColumns) { - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("rows=%d <> columns=%d"), self->mRows, self->mColumns); + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix must be square but is (%u x %u)"), + self->mRows, self->mColumns); return NULL; } - const double (*self_values)[self->mColumns] = self->mData; - if (self->mColumns == 1) { + const unsigned int size = self->mRows; + const double (*self_values)[size] = self->mData; + if (size == 1) { const double det = matrix_det(self); if (det == 0) { LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Determinant is 0")); return NULL; } double temp[1][1] = { { 1.0 / det } }; - return matrix_create_from_values(self->mRows, self->mColumns, temp); + return matrix_create_from_values(size, size, temp); } - if (self->mColumns == 2) { + if (size == 2) { const double det = matrix_det(self); if (det == 0) { - return NULL; LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Determinant is 0")); + return NULL; } double temp[2][2] = { { self_values[1][1] / det, -self_values[0][1] / det }, { -self_values[1][0] / det, self_values[0][0] / det } }; - return matrix_create_from_values(self->mRows, self->mColumns, temp); + return matrix_create_from_values(size, size, temp); } - if (self->mColumns == 3) { + if (size == 3) { const double det = matrix_det(self); if (det == 0) { LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Determinant is 0")); @@ -275,50 +359,65 @@ Matrix* matrix_invert(const Matrix* self) (self_values[0][1] * self_values[2][0] - self_values[0][0] * self_values[2][1]) / det, (self_values[0][0] * self_values[1][1] - self_values[0][1] * self_values[1][0]) / det } }; - return matrix_create_from_values(self->mRows, self->mColumns, temp); + return matrix_create_from_values(size, size, temp); } - LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Not implemented for size=%d"), self->mColumns); + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Not implemented for size=%u"), size); return NULL; } -/** - * @return Transposed matrix. - */ Matrix* matrix_transpose(const Matrix* self) { - const double (*self_values)[self->mColumns] = self->mData; + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } Matrix* result = matrix_create_empty(self->mColumns, self->mRows); - double (*result_values)[result->mColumns] = result->mData; - unsigned int row; - for (row = self->mRows; row; ) { - --row; - unsigned int col; - for (col = self->mColumns; col; ) { - --col; - result_values[col][row] = self_values[row][col]; + if (result) { + const double (*self_values)[self->mColumns] = self->mData; + double (*result_values)[result->mColumns] = result->mData; + unsigned int row; + for (row = self->mRows; row; ) { + --row; + unsigned int col; + for (col = self->mColumns; col; ) { + --col; + result_values[col][row] = self_values[row][col]; + } } } return result; } -/** - * @return Symmetrized matrix. - */ Matrix* matrix_symmetrize(const Matrix* self) { - const double (*self_values)[self->mColumns] = self->mData; - Matrix* result = matrix_create_empty(self->mRows, self->mColumns); - double (*result_values)[result->mColumns] = result->mData; - unsigned int row; - for (row = self->mRows; row; ) { - --row; - result_values[row][row] = self_values[row][row]; - } - for (row = 0; row < self->mRows; row++) { - unsigned int col; - for (col = row + 1; col < self->mColumns; col++) { - result_values[row][col] = result_values[col][row] = - (self_values[row][col] + self_values[col][row]) / 2.0; + if (!self) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix is NULL")); + return NULL; + } + if (self->mRows != self->mColumns) { + LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Matrix must be square but is (%u x %u)"), + self->mRows, self->mColumns); + return NULL; + } + const unsigned int size = self->mRows; + Matrix* result = matrix_create_empty(size, size); + if (result) { + const double (*self_values)[size] = self->mData; + double (*result_values)[size] = result->mData; + unsigned int row; + for (row = size; row; ) { + --row; + result_values[row][row] = self_values[row][row]; + } + if (size >= 2) { + for (row = size - 1; row; --row) { + unsigned int col; + for (col = row; col; ) { + --col; + result_values[row][col] = result_values[col][row] = + (self_values[row][col] + self_values[col][row]) / 2.0; + } + } } } return result; diff --git a/lbs-server/src/fused/Matrix.h b/lbs-server/src/fused/Matrix.h index 474aa9f..dc0480e 100644 --- a/lbs-server/src/fused/Matrix.h +++ b/lbs-server/src/fused/Matrix.h @@ -42,14 +42,6 @@ Matrix* matrix_create_empty(const unsigned int rows, const unsigned int columns) Matrix* matrix_create_from_values(const unsigned int rows, const unsigned int columns, const void* in_values_pointer); -/** - * @brief Creates matrix and initializes it with given values. - * @param values Table with matrix rows. Can not be null. - * @param startColumn First column of matrix in values. - */ -Matrix* matrix_create_from_values_start_column(const unsigned int rows, const unsigned int columns, - const void* in_values_pointer, const unsigned int start_column); - /** * @brief Copy constructor. * @param in Source matrix. Can not be null. @@ -63,8 +55,6 @@ void matrix_destroy(Matrix *self); void matrix_set_from_values(Matrix *self, const void* in_values_pointer); -void matrix_set_from_copy(Matrix *self, const Matrix *in); - /** * @brief Returns value from matrix. * @param row Row number. @@ -82,10 +72,11 @@ Matrix* matrix_multiply_by_matrix(const Matrix *self, const Matrix *in); /** * @brief Multiplies matrix by given vector. - * @param vector Vector to multiply. Can not be null. + * @param vector Vector to multiply. + * Can not be null. It's size must be equal to number of matrix columns. * @return Product of matrix and vector. */ -double* matrix_multiply_by_vector(const Matrix *self, const double* vector, const unsigned int vector_length); +double* matrix_multiply_by_vector(const Matrix *self, const double* vector); /** * @brief Adds matrix to given one. @@ -102,11 +93,12 @@ Matrix* matrix_add_matrix(const Matrix *self, const Matrix *in); Matrix* matrix_sub_matrix(const Matrix *self, const Matrix *in); /** - * @brief Adds given vector to matrix diagonal. - * @param vector Vector to add. Can not be null. + * @brief Adds given vector to square matrix diagonal. + * @param vector Vector to add. + * Can not be null. It's size must be equal to number size of matrix. * @return Sum of matrix and vector. */ -Matrix* matrix_add_vector_diagonal(Matrix *self, const double* vector); +Matrix* matrix_add_vector_diagonal(const Matrix *self, const double* vector); /** * @return Determinant of matrix. diff --git a/lbs-server/src/fused/Orientation.c b/lbs-server/src/fused/Orientation.c index 1c1a5f4..f975482 100644 --- a/lbs-server/src/fused/Orientation.c +++ b/lbs-server/src/fused/Orientation.c @@ -142,41 +142,12 @@ bool orientation_new_angular_velocity(Orientation* self, double time, const_Vect 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_orientation_time)) { - const double dt = time - self->last_orientation_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); - Quaternion new_orient; - quaternion_multiply_quaternion(qwt, self->orientation, new_orient); - quaternion_clone(new_orient, self->orientation); - quaternion_normalize(self->orientation); - } - integral_value(&(self->heading), time, orientation_get_heading_rate(self)); - self->last_orientation_time = time; - } - } -} -*/ - bool orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acc) { - LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.9f, acc0=%.17e, acc1=%.17e, acc2=%.17e"), - time, acc[0], acc[1], acc[2]); + LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.9f, acc0=%.17e, acc1=%.17e, acc2=%.17e"), time, acc[0], acc[1], acc[2]); if (self->acc_weight < 100) { (self->acc_weight)++; } -/* orientation_predict_rw(self, time); */ Vector_3d gacc; quaternion_rotate_vector_3d(self->orientation, acc, gacc); LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), gacc0=%.17e, gacc1=%.17e, gacc2=%.17e, angle=%.17e"), @@ -185,7 +156,7 @@ bool orientation_new_acceleration(Orientation* self, double time, const_Vector_3 vector_3d_copy(gu, gacc); bool result = false; - /* update rotation matrix */ + /* update rotation quaternion */ double accLen = vector_3d_length(gu); if (accLen > 0) { Quaternion q; @@ -202,9 +173,10 @@ bool orientation_new_acceleration(Orientation* self, double time, const_Vector_3 quaternion_clone(new_orient, self->orientation); quaternion_normalize(self->orientation); } - if (self->orientation_time < time) { + + if (self->orientation_time < time) self->orientation_time = time; - } + result = orientation_calc_heading_rate(self); LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.9f, q=[%.17e, %.17e, %.17e, %.17e]"), self->orientation_time, @@ -247,9 +219,7 @@ void quaternion_normalize(Quaternion_ref self) double quaternion_get_rotation_angle(const_Quaternion_ref self) { return 2 * atan2(sqrt(SQUARE(self[QTR_X]) + SQUARE(self[QTR_Y]) + SQUARE(self[QTR_Z])), fabs(self[QTR_0])); -/* - return 2 * acos(fabs(self[QTR_0])); This may degenerate accuracy for quaternion near 1 -*/ +/* return 2 * acos(fabs(self[QTR_0])); //This may degenerate accuracy for quaternion near 1 */ } void quaternion_clone(const_Quaternion_ref self, Quaternion_ref out) @@ -393,8 +363,8 @@ void rotation_filter_reset(RotationFilter* self) self->rot_speed = 0; self->rot_speed_variance = self->def_rot_var; SET_UNKNOWN_TIME(self->rot_speed_time); - self->last_azimuth = 0; - SET_UNKNOWN_TIME(self->last_azimuth_time); + self->last_heading = 0; + SET_UNKNOWN_TIME(self->last_heading_time); } void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot_speed, double rot_speed_variance) @@ -426,20 +396,20 @@ void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot self->rot_speed_time = time; } -void rotation_filter_new_azimuth(RotationFilter* self, double time, double azimuth, double vhe, double speed) +void rotation_filter_new_heading(RotationFilter* self, double time, double heading, double vhe, double speed) { - if (speed > 0 && IS_KNOWN_TIME(self->last_azimuth_time)) { - const double dt = time - self->last_azimuth_time; + if (speed > 0 && IS_KNOWN_TIME(self->last_heading_time)) { + const double dt = time - self->last_heading_time; if (speed * vhe * dt > 0) { - const double rot_speed = correct_angle(-(azimuth - self->last_azimuth)) / dt; + const double rot_speed = correct_angle(-(heading - self->last_heading)) / 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; + self->last_heading = heading; + self->last_heading_time = time; } #endif diff --git a/lbs-server/src/fused/Orientation.h b/lbs-server/src/fused/Orientation.h index 3f6afe2..92df5d9 100644 --- a/lbs-server/src/fused/Orientation.h +++ b/lbs-server/src/fused/Orientation.h @@ -26,6 +26,7 @@ #include "parameters.h" #include "Vector.h" #include "math-ext.h" + #include #ifdef __cplusplus @@ -39,8 +40,8 @@ typedef struct { 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. */ + double last_heading; /* Last reported heading. */ + double last_heading_time; /* Time of last heading update. */ } RotationFilter; #endif @@ -96,7 +97,7 @@ extern void orientation_reset(Orientation* self); /** (double) */ #define orientation_get_heading_rate(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) +#define orientation_new_heading(self, time, heading, vhe, speed) rotation_filter_new_heading(&(self)->rotation_filter, time, heading, vhe, speed) #else @@ -251,8 +252,8 @@ extern void rotation_filter_reset(RotationFilter* self); 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); +extern void rotation_filter_new_heading(RotationFilter* self, + double time, double heading, double vhe, double speed); /** (double) Returns current rotation speed along axis */ #define rotation_filter_get_rot_speed(self) ((self)->rot_speed) diff --git a/lbs-server/src/fused/PositionFusion2D.c b/lbs-server/src/fused/PositionFusion2D.c index 1fdd70c..63b62f1 100644 --- a/lbs-server/src/fused/PositionFusion2D.c +++ b/lbs-server/src/fused/PositionFusion2D.c @@ -17,6 +17,7 @@ #include "PositionFusion2D.h" #include "math-ext.h" #include "debug_util.h" + #include /** Process noise standard deviation of linear acceleration in [m/s^2]. */ @@ -118,7 +119,7 @@ static const double PDR_MEASUREMENT_MATRIX[3][5] = { /** * Initialize PositionFusion2D object. */ -void position_fusion_2d_init(PositionFusion2D* self) +bool position_fusion_2d_init(PositionFusion2D* self) { self->positionMeasurementMatrix = matrix_create_from_values(2, 5, POSITION_MEASUREMENT_MATRIX); self->speedMeasurementMatrix = matrix_create_from_values(1, 5, SPEED_MEASUREMENT_MATRIX); @@ -127,20 +128,39 @@ void position_fusion_2d_init(PositionFusion2D* self) self->angularSpeedMeasurementMatrix = matrix_create_from_values(1, 5, ANGULAR_SPEED_MEASUREMENT_MATRIX); self->speedAngularSpeedMeasurementMatrix = matrix_create_from_values(2, 5, SPEED_ANGULAR_SPEED_MEASUREMENT_MATRIX); self->pdrMeasurementMatrix = matrix_create_from_values(3, 5, PDR_MEASUREMENT_MATRIX); - kalman_filter_init(&self->mKalmanFilter, STATE_VECTOR_SIZE); - position_fusion_2d_reset(self); + const bool initialized = kalman_filter_init(&self->mKalmanFilter, STATE_VECTOR_SIZE); + if (self->positionMeasurementMatrix && + self->speedMeasurementMatrix && + self->positionSpeedMeasurementMatrix && + self->speedDirectionMeasurementMatrix && + self->angularSpeedMeasurementMatrix && + self->speedAngularSpeedMeasurementMatrix && + self->pdrMeasurementMatrix && + initialized) { + position_fusion_2d_reset(self); + return true; + } + position_fusion_2d_destroy(self); + return false; } void position_fusion_2d_destroy(PositionFusion2D* self) { kalman_filter_destroy(&self->mKalmanFilter); matrix_destroy(self->positionMeasurementMatrix); + self->positionMeasurementMatrix = NULL; matrix_destroy(self->speedMeasurementMatrix); + self->speedMeasurementMatrix = NULL; matrix_destroy(self->positionSpeedMeasurementMatrix); + self->positionSpeedMeasurementMatrix = NULL; matrix_destroy(self->speedDirectionMeasurementMatrix); + self->speedDirectionMeasurementMatrix = NULL; matrix_destroy(self->angularSpeedMeasurementMatrix); + self->angularSpeedMeasurementMatrix = NULL; matrix_destroy(self->speedAngularSpeedMeasurementMatrix); + self->speedAngularSpeedMeasurementMatrix = NULL; matrix_destroy(self->pdrMeasurementMatrix); + self->pdrMeasurementMatrix = NULL; } void position_fusion_2d_reset(PositionFusion2D* self) @@ -298,9 +318,10 @@ void position_fusion_2d_predict(PositionFusion2D* self, Timestamp predictTimesta { 0, 0, 0, c33, c34 }, { 0, 0, 0, c34, c44 } }; - Matrix *processNoiseMatrix = matrix_create_from_values(5, 5, processNoise); - kalman_filter_predict(&self->mKalmanFilter, state_transition_function, stateTransitionMatrix, processNoiseMatrix); + if (stateTransitionMatrix && processNoiseMatrix) { + kalman_filter_predict(&self->mKalmanFilter, state_transition_function, stateTransitionMatrix, processNoiseMatrix); + } matrix_destroy(stateTransitionMatrix); matrix_destroy(processNoiseMatrix); } diff --git a/lbs-server/src/fused/PositionFusion2D.h b/lbs-server/src/fused/PositionFusion2D.h index d8ec712..5b43c7f 100644 --- a/lbs-server/src/fused/PositionFusion2D.h +++ b/lbs-server/src/fused/PositionFusion2D.h @@ -24,6 +24,8 @@ #include "KalmanFilter.h" +#include + #ifdef __cplusplus extern "C" { #endif @@ -52,7 +54,7 @@ typedef struct { } PositionFusion2D; /** Initialize PositionFusion2D object. */ -extern void position_fusion_2d_init(PositionFusion2D* self); +extern bool position_fusion_2d_init(PositionFusion2D* self); /** Deinitialize/destroy PositionFusion2D object . */ extern void position_fusion_2d_destroy(PositionFusion2D* self); diff --git a/lbs-server/src/fused/WgsConversion.c b/lbs-server/src/fused/WgsConversion.c index 7970316..26a2e12 100644 --- a/lbs-server/src/fused/WgsConversion.c +++ b/lbs-server/src/fused/WgsConversion.c @@ -22,6 +22,11 @@ #include void wgs_conversion_init(WgsConversion* self) +{ + wgs_conversion_reset(self); +} + +void wgs_conversion_reset(WgsConversion* self) { memset(self, 0, sizeof(*self)); } @@ -60,13 +65,16 @@ void wgs_conversion_wgs_to_local(const WgsConversion* self, double latitude, dou void wgs_conversion_local_bearing_to_wgs_azimuth(const WgsConversion* self, double x, double y, double bearing, double* azimuth) { LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("bearing=%.17e"), bearing); - *azimuth = M_PI / 2 - bearing; + *azimuth = to_degrees(correct_angle(M_PI / 2 - bearing)); + if (*azimuth < 0) { + *azimuth += 360; + } LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("azimuth=%.17e"), *azimuth); } void wgs_conversion_wgs_azimuth_to_local_bearing(const WgsConversion* self, double latitude, double longitude, double azimuth, double *bearing) { LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("azimuth=%.17e"), azimuth); - *bearing = M_PI / 2 - azimuth; + *bearing = correct_angle(M_PI / 2 - to_radians(azimuth)); LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("bearing=%.17e"), *bearing); } diff --git a/lbs-server/src/fused/WgsConversion.h b/lbs-server/src/fused/WgsConversion.h index bdf0f34..89c55e2 100644 --- a/lbs-server/src/fused/WgsConversion.h +++ b/lbs-server/src/fused/WgsConversion.h @@ -16,6 +16,7 @@ /** * @file WgsConversion.h + * Performs conversion between local (tangent) coordinate system and WGS coordinate system. */ #pragma once @@ -57,57 +58,51 @@ typedef struct { extern void wgs_conversion_init(WgsConversion* self); +extern void wgs_conversion_reset(WgsConversion* self); + extern bool wgs_conversion_is_valid(WgsConversion* 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 + * @brief Create coordinate map from WGS coordinate system to the tangent space. + * @param[in/out] latitude Latitude of center of the local frame in degrees. + * @param[in/out] longitude Longitude of center of the local frame in degrees. */ extern void wgs_conversion_create(WgsConversion* self, double latitude, double longitude); /** - * @brief Converts coordinates in local coordinate system to spherical coordinate system. - * @param[in] self Tangent coordinate system. - * @param[in] pos Coordinates of given point in tangent coordinate system. + * @brief Converts coordinates in local coordinate system to WGS coordinate system. + * @param[in] x X coordinate of a given point in tangent coordinate system in [m]. + * @param[in] y Y coordinate of a given point in tangent coordinate system in [m]. * @param[out] latitude Latitude in degrees. * @param[out] longitude Longitude in degrees. - * @param[out] altitude Altitude in meters. */ extern void wgs_conversion_local_to_wgs(const WgsConversion* self, double x, double y, double* latitude, double* longitude); /** - * @brief Converts coordinates in spherical coordinate system to local coordinate system. + * @brief Converts coordinates in WGS coordinate system to local 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 local coordinate system. + * @param[in] latitude Latitude of a given point in degrees. + * @param[in] longitude Longitude of a given point in degrees. + * @param[out] x X coordinate in tangent coordinate system in [m]. + * @param[out] y Y coordinate in tangent coordinate system in [m]. */ extern void wgs_conversion_wgs_to_local(const WgsConversion* self, double latitude, double longitude, double* x, double* y); /** - * @brief Converts speed in tangent coordinate system to spherical coordinate system. - * @param[in] self Tangent coordinate system. - * @param[in] pos Coordinates of location in local coordinate system. - * @param[in] vel Speed in given location in local 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). + * @brief Converts bearing in tangent coordinate system to azimuth in WGS coordinate system. + * @param[in] x X coordinate of a given point in tangent coordinate system in [m]. + * @param[in] y Y coordinate of a given point in tangent coordinate system in [m]. + * @param[in] bearing Bearing (angle between OX and speed direction anticlockwise) in [rad]. + * @param[out] azimuth Azimuth (angle between true North and speed direction clockwise) in degrees in the range [0..360). */ extern void wgs_conversion_local_bearing_to_wgs_azimuth(const WgsConversion* self, double x, double y, double bearing, double* azimuth); /** - * @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. + * @brief Converts azimuth in WGS coordinate system to bearing in tangent coordinate system. + * @param[in] latitude Latitude of a given point in degrees. + * @param[in] longitude Longitude of a given point in degrees. + * @param[in] azimuth Azimuth (angle between true North and speed direction clockwise) in degrees. + * @param[out] bearing Bearing (angle between OX and speed direction anticlockwise) in [rad] in the range [-PI, PI]. */ extern void wgs_conversion_wgs_azimuth_to_local_bearing(const WgsConversion* self, double latitude, double longitude, double azimuth, double *bearing); diff --git a/lbs-server/src/fused/math-ext.h b/lbs-server/src/fused/math-ext.h index 9de1c02..5974263 100644 --- a/lbs-server/src/fused/math-ext.h +++ b/lbs-server/src/fused/math-ext.h @@ -24,7 +24,14 @@ #define __MATH_EXT_H__ #define _USE_MATH_DEFINES +#ifdef __STRICT_ANSI__ +#undef __STRICT_ANSI__ +/* To include fmin, fmax etc. */ #include +#define __STRICT_ANSI__ +#else +#include +#endif #include #ifdef __cplusplus diff --git a/lbs-server/src/fused/parameters.h b/lbs-server/src/fused/parameters.h index 2dc12f4..d7d8079 100644 --- a/lbs-server/src/fused/parameters.h +++ b/lbs-server/src/fused/parameters.h @@ -26,19 +26,27 @@ /* Features */ /** 0 = off, 1 = Kalman-merging of angular velocities from GPS and gyroscope. */ -#define FL_KALMAN_ANGULAR_VELOCITY 0 +#define FL_KALMAN_ANGULAR_VELOCITY 0 /** 0 = off, 1 = Filtration of output location notifications */ -#define FL_MIN_NOTIFICATION_FILTRATION 1 -/** Minimal interval in seconds between consecutive output location notifications */ -#define FL_MIN_NOTIFICATION_INTERVAL 1 +#define FL_MAX_NOTIFICATION_INTERVAL_FROM_LAST_POS_FILTRATION_USE 1 /** Maximum time in seconds from last input location for sending output position to clients */ -#define FL_MAX_TIME_FROM_LAST_POS 300.0 +#define FL_MAX_NOTIFICATION_INTERVAL_FROM_LAST_POS 300.0 +/** 0 = off, 1 = Filtration of output location notifications */ +#define FL_MIN_NOTIFICATION_INTERVAL_FILTRATION_USE 1 +/** Minimal interval in seconds between consecutive output location notifications */ +#define FL_MIN_NOTIFICATION_INTERVAL 1.0 +/** 0 = off, 1 = Filtration pedometer false positives in driving case */ +#define FL_PEDOMETER_DURING_DRIVING_FILTRATION_USE 1 /** 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 +#define FL_PEDOMETER_SKIP_MAX_INTERVAL_FROM_LAST_GPS 60.0 +/** Min speed in [m/s] from last gps when pedometer skip may occur (filtration pedometer false positive in driving case) */ +#define FL_PEDOMETER_SKIP_MIN_SPEED_FROM_LAST_GPS 10.0 / 3.6 /** 10 km/h */ +/** 0 = off, 1 = Filter out pedometer events with low speed */ +#define FL_MIN_SPEED_FROM_PEDOMETER_FILTRATION_USE 1 +/** Min accepted speed in [m/s] from pedometer */ +#define FL_MIN_SPEED_FROM_PEDOMETER 0.001 /** 0.001 m/s */ /** 0 = off, 1 = fused devel logs on */ -#define FL_ENABLE_DEVEL_LOG 0 +#define FL_ENABLE_DEVEL_LOG 0 /* Free parameters */ @@ -58,5 +66,9 @@ #define FL_GYRO_SPIN_SIGMA_F 0.0004 /** Log-2 of the AEMA averaging constant used for clock synchronization. */ #define FL_TIMO_AEMA_PLATEAU_BITS 10 +/* 1 [km/h] = 1/3.6 [m/s] */ +#define KMPH_TO_MPS(kmph) ((kmph) / 3.6) +/* 1 [m/s] = 3.6 [km/h] */ +#define MPS_TO_KMPH(mps) ((mps) * 3.6) #endif /* __FL_PARAMETERS_H__ */ diff --git a/lbs-server/src/lbs_server.c b/lbs-server/src/lbs_server.c index 9e91f5e..83b1769 100644 --- a/lbs-server/src/lbs_server.c +++ b/lbs-server/src/lbs_server.c @@ -259,7 +259,6 @@ 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 /* * For fused location */ @@ -267,7 +266,6 @@ static void nps_update_position(lbs_server_s *lbs_server, NpsManagerPositionExt 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, lbs_server->wps.fields, lbs_server->wps.timestamp, lbs_server->wps.latitude, @@ -1419,7 +1417,6 @@ static void shutdown(gpointer userdata, gboolean *shutdown_arr) } } -#ifdef TIZEN_DEVICE /* * For fused location */ @@ -1439,7 +1436,6 @@ static void fused_update_position_cb(gint timestamp, gdouble latitude, gdouble l lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_FUSED, fields, timestamp, latitude, longitude, altitude, speed, direction, climb, accuracy); } -#endif static void gps_update_position_cb(pos_data_t *pos, gps_error_t error, void *user_data) { @@ -1466,7 +1462,6 @@ static void gps_update_position_cb(pos_data_t *pos, gps_error_t error, void *use gps_set_position(pos); -#ifdef TIZEN_DEVICE /* * For fused location */ @@ -1476,7 +1471,6 @@ static void gps_update_position_cb(pos_data_t *pos, gps_error_t error, void *use 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, pos->latitude, pos->longitude, pos->altitude, pos->speed, pos->bearing, 0.0, accuracy); @@ -1653,7 +1647,6 @@ static void lbs_server_init(lbs_server_s *lbs_server) lbs_server->optimized_batch_array[LBS_BATCH_PERIOD] = MAX_BATCH_PERIOD; #endif -#ifdef TIZEN_DEVICE /* * For fused location */ @@ -1661,7 +1654,6 @@ static void lbs_server_init(lbs_server_s *lbs_server) lbs_server->fused_interval_table = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, NULL); location_fused_init(fused_update_position_cb, lbs_server); } -#endif } static void nps_get_last_position(lbs_server_s *lbs_server) @@ -1870,7 +1862,6 @@ int main(int argc, char **argv) LOG_GPS(DBG_LOW, "lbs_server deamon Stop...."); -#ifdef TIZEN_DEVICE /* * For fused location */ @@ -1878,7 +1869,6 @@ int main(int argc, char **argv) location_fused_deinit(); g_hash_table_destroy(lbs_server->fused_interval_table); } -#endif gps_deinit_log(); diff --git a/packaging/lbs-server.spec b/packaging/lbs-server.spec index 00e361a..f72c42d 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.3.3 +Version: 1.3.4 Release: 1 Group: Location/Service License: Apache-2.0