#include <glib.h>
#include <libgen.h>
-#include "fused/parameters.h"
#ifdef __cplusplus
extern "C" {
#include "fused/FusionEngine.h"
#include "fused.h"
#include "debug_util.h"
+#include <pthread.h>
#define PTR2ENUM(ptr, type) ((type)(int)((char*)(ptr) - ((char*)0)))
#define ENUM2PTR(value) (((char*)0) + (int)(value))
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()
{
&& 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;
- }
}
}
&& 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;
- }
}
}
{
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",
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) {
} 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,
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");
+ }
}
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();
#include "CalibrationFilter.h"
#include "Time.h"
#include "math-ext.h"
+
#include <stdio.h>
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;
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;
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]);
}
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;
#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);
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)
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;
}
}
}
-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]);
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);
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;
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;
}
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;
}
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)
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 };
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);
#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 {
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);
/**
* @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.
* @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).
* @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.
* @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,
/**
* @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);
*/
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
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;
*/
#include "KalmanFilter.h"
+#include "debug_util.h"
#include <stdlib.h>
-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;
}
* 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];
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);
}
/**
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);
#include "Matrix.h"
+#include <stdbool.h>
+
#ifdef __cplusplus
extern "C" {
#endif
* @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.
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)
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;
}
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),
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),
/**
* @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);
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;
}
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;
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;
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]);
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;
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"));
(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;
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.
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.
/**
* @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.
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.
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"),
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;
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,
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)
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)
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
#include "parameters.h"
#include "Vector.h"
#include "math-ext.h"
+
#include <stdbool.h>
#ifdef __cplusplus
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
/** (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
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)
#include "PositionFusion2D.h"
#include "math-ext.h"
#include "debug_util.h"
+
#include <limits.h>
/** Process noise standard deviation of linear acceleration in [m/s^2]. */
/**
* 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);
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)
{ 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);
}
#include "KalmanFilter.h"
+#include <stdbool.h>
+
#ifdef __cplusplus
extern "C" {
#endif
} 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);
#include <memory.h>
void wgs_conversion_init(WgsConversion* self)
+{
+ wgs_conversion_reset(self);
+}
+
+void wgs_conversion_reset(WgsConversion* self)
{
memset(self, 0, sizeof(*self));
}
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);
}
/**
* @file WgsConversion.h
+ * Performs conversion between local (tangent) coordinate system and WGS coordinate system.
*/
#pragma once
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);
#define __MATH_EXT_H__
#define _USE_MATH_DEFINES
+#ifdef __STRICT_ANSI__
+#undef __STRICT_ANSI__
+/* To include fmin, fmax etc. */
#include <math.h>
+#define __STRICT_ANSI__
+#else
+#include <math.h>
+#endif
#include <stdlib.h>
#ifdef __cplusplus
/* 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 */
#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__ */
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
*/
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,
}
}
-#ifdef TIZEN_DEVICE
/*
* For fused location
*/
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)
{
gps_set_position(pos);
-#ifdef TIZEN_DEVICE
/*
* For fused location
*/
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);
lbs_server->optimized_batch_array[LBS_BATCH_PERIOD] = MAX_BATCH_PERIOD;
#endif
-#ifdef TIZEN_DEVICE
/*
* For fused location
*/
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)
LOG_GPS(DBG_LOW, "lbs_server deamon Stop....");
-#ifdef TIZEN_DEVICE
/*
* For fused location
*/
location_fused_deinit();
g_hash_table_destroy(lbs_server->fused_interval_table);
}
-#endif
gps_deinit_log();
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
projects / platform / core / location / lbs-server.git / commitdiff