DEV_SPATIAL_DIMENSION = GEO_DIMENSION
} fl_dev_spatial;
-
-/** Combined status flags and bit-field */
-typedef union {
- struct {
- unsigned started:1;
- unsigned paused:1;
- };
-} _fl_status;
-
/** Fused location data */
typedef struct {
- _fl_status status;
+ bool is_started;
WgsLocation last_location;
_fl_sensor sensors[SENSOR_SOURCE_COUNT];
gpointer lbs_server_handle;
calibration != NULL ? calibration : "EMPTY");
fusion_engine_set_calibration(&fused.fusion_engine, calibration);
- fused.status.started = TRUE;
+ fused.is_started = TRUE;
fusion_engine_start(&fused.fusion_engine);
unsigned int i;
for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
sensor_stop(i);
}
fusion_engine_stop(&fused.fusion_engine);
- fused.status.started = FALSE;
+ fused.is_started = FALSE;
char *calibration = fusion_engine_get_calibration(&fused.fusion_engine);
if (calibration) {
fused.sensors[PEDOMETER].type = SENSOR_HUMAN_PEDOMETER;
fused.fused_pos_cb = _fused_pos_cb;
fused.lbs_server_handle = lbs_server;
- fused.status.started = FALSE;
- fused.status.paused = FALSE;
+ fused.is_started = FALSE;
fused.last_gps_speed = -1.0;
fused.last_gps_timestamp = 0;
fused.last_wps_timestamp = 0;
unsigned long long boot_time_ns = get_current_boot_time();
fused.utc_offset_sec = (double)now - (double)boot_time_ns / 1000000000.0;
LOG_FUSED(LOG_DEBUG, "now = %lu, boot_time_ns = %llu, utc_offset_sec = %.10f", now, boot_time_ns, fused.utc_offset_sec);
- fusion_engine_init(&fused.fusion_engine, on_motion_state, fused.utc_offset_sec);
+ fusion_engine_init(&fused.fusion_engine, fused.utc_offset_sec);
}
void location_fused_deinit()
{
LOG_FUSED_FUNC;
- if (fused.status.started)
+ if (fused.is_started)
location_fused_stop();
sensors_destroy();
- fusion_engine_exit(&fused.fusion_engine);
+ fusion_engine_destroy(&fused.fusion_engine);
fused.fused_pos_cb = NULL;
fused.lbs_server_handle = NULL;
- fused.status.started = FALSE;
- fused.status.paused = FALSE;
+ fused.is_started = FALSE;
fused.last_gps_speed = -1.0;
fused.last_gps_timestamp = 0;
fused.last_wps_timestamp = 0;
{
// LOG_FUSED_FUNC;
- if (!fused.status.started)
+ if (!fused.is_started)
return;
fused.last_gps_timestamp = timestamp;
{
// LOG_FUSED_FUNC;
- if (!fused.status.started)
+ if (!fused.is_started)
return;
fused.last_wps_timestamp = timestamp;
fusion_engine_get_wgs_location(&fused.fusion_engine, &fused.last_location);
if (fused.fused_pos_cb)
- fused.fused_pos_cb((gint)fused.last_location.time,
+ 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,
case GYROSCOPE:
is_new_position = fusion_engine_gyro_event(&fused.fusion_engine,
(double)(long long)event->timestamp / 1000000.0,
- conversions_degrees_to_radians(event->values[DEV_SPATIAL_X]),
- conversions_degrees_to_radians(event->values[DEV_SPATIAL_Y]),
- conversions_degrees_to_radians(event->values[DEV_SPATIAL_Z]));
+ to_radians(event->values[DEV_SPATIAL_X]),
+ to_radians(event->values[DEV_SPATIAL_Y]),
+ to_radians(event->values[DEV_SPATIAL_Z]));
break;
case PEDOMETER:
LOG_FUSED(LOG_DEBUG, "[PEDOMETER] timestamp[%llu] accuracy[%d] Step[%.0f] Step[%.0f] [%.1f] Dist[%.3f m] [%.1f] Speed[%.5f m/s] [%.1f] [%.1f]",
} else
LOG_FUSED(LOG_ERROR, "event == NULL");
}
-
-#if (FL_MOTION_DETECTOR)
-static void location_fused_pause()
-{
- LOG_FUSED_FUNC;
- if (fused.status.paused == FALSE) {
- sensor_stop(GYROSCOPE);
- sensor_stop(PEDOMETER);
- }
- fused.status.paused = TRUE;
-}
-
-static void location_fused_resume()
-{
- LOG_FUSED_FUNC;
- if (fused.status.paused) {
- sensor_start(GYROSCOPE);
- sensor_start(PEDOMETER);
- }
- fused.status.paused = FALSE;
-}
-
-static void on_motion_state(MotionDetectorState state)
-{
- LOG_FUSED(LOG_DEBUG, "(state=%u)", state);
- if (fused.status.started) {
- switch (state) {
- case MOTI_MOVING:
- if (fused.status.paused) {
- location_fused_resume();
- }
- break;
-
- case MOTI_SLEEP:
- if (fused.status.paused == FALSE) {
- location_fused_pause();
- }
- break;
-
- default:
- /* MOTI_UNDECIDED: */
- /* MOTI_IMMOBILE: */
- break;
- }
- }
-}
-#endif
#ifndef __LOCATION_FUSED_H__
#define __LOCATION_FUSED_H__
-#if (FL_MOTION_DETECTOR)
-#include "fused/MotionDetector.h"
-#endif
#include <glib.h>
/** Labels for standard sensors */
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);
+
void location_fused_deinit();
+
void location_fused_start();
+
void location_fused_stop();
/**
*/
static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id);
-#if (FL_MOTION_DETECTOR)
-/** Enters power-saving mode by unsubscribing from location source, and and unused sensor(s)) */
-static void location_fused_pause();
-
-/** Resumes normal-power operation mode by subscribing to location source, and and used sensor(s) */
-static void location_fused_resume();
-
-/**
- * @brief Receive notifications about motion state changes, and pause/resume accordingly.
- * @param[in] state New motion state
- */
-static void on_motion_state(MotionDetectorState state);
-#else /* (FL_MOTION_DETECTOR) */
-/** Compilation stub */
- #define on_motion_state NULL
-#endif /* (FL_MOTION_DETECTOR) */
-
#endif /* defined(__LOCATION_FUSED_C__) */
#endif /* __LOCATION_FUSED_H__ */
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "AccelerometerFilter.h"
-#include "Conversions.h"
-#include "math-ext.h"
-#include "debug_util.h"
-
-#define _USE_MATH_DEFINES
-#include <math.h>
-
-#define DEFAULT_SAMPLING_FREQUENCY 10.0 /* [Hz] accelerometer sampling frequency */
-#define DEFAULT_ACCELERATION_SIGMA2 SQUARE(FL_ACCEL_NOISE_LEVEL) /* [(m/s^2)^2] */
-
-static const Vector_3d av = {0, 0, EARTH_GRAVITY};
-
-void accelerometer_filter_init(AccelerometerFilter* self,
- MotionDetectorStateChangeListener on_motion_state_changed)
-{
- LOG_FUSED_FUNC;
- frequency_estimator_init(&self->frequency_estimator, DEFAULT_SAMPLING_FREQUENCY);
- butterworth_3d_filter_init(&self->lp_3d_filter, FL_LP3D_CUTOFF_FREQUENCY);
- gravity_estimator_init(&self->gravity_estimator);
- motion_detector_init(&self->motion_detector, on_motion_state_changed);
-
- butterworth_3d_filter_reset_to(&self->lp_3d_filter, av);
- butterworth_3d_filter_set_sampling_frequency(&self->lp_3d_filter, DEFAULT_SAMPLING_FREQUENCY);
-
- LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
-}
-
-void accelerometer_filter_exit(AccelerometerFilter* self)
-{
- LOG_FUSED_FUNC;
- motion_detector_exit(&self->motion_detector);
- gravity_estimator_exit(&self->gravity_estimator);
- butterworth_3d_filter_exit(&self->lp_3d_filter);
-
- LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
-}
-
-void accelerometer_filter_reset(AccelerometerFilter* self, const_Vector_3d_ref av)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
- frequency_estimator_init(&self->frequency_estimator, DEFAULT_SAMPLING_FREQUENCY);
- motion_detector_reset(&self->motion_detector);
- butterworth_3d_filter_reset_to(&self->lp_3d_filter, av);
-}
-
-void accelerometer_filter_process(AccelerometerFilter* self,
- double time, const_Vector_3d_ref acc, Vector_3d_ref gu, double *w2gu)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.2f, acc=(% 5.2f, % 5.2f, % 5.2f))"), time, acc[X], acc[Y], acc[Z]);
-
- if (frequency_estimator_process(&self->frequency_estimator, time))
- butterworth_3d_filter_set_sampling_frequency(&self->lp_3d_filter, frequency_estimator_get_frequency(&self->frequency_estimator));
-
- const_Vector_3d_ref af = butterworth_3d_filter_process(&self->lp_3d_filter, acc);
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, af=(%.16g, %.16g, %.16g)"), time, af[0], af[1], af[2]);
-
- /* the z-value is passed through 2nd order Butterworth, then 1st order EMA */
- const double af_len = vector_3d_length(af);
- const double overlap = vector_3d_dot_product(af, acc);
- const double g2 = gravity_estimator_process(&self->gravity_estimator, overlap);
-
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=%.2f, |af|=%5.2f, <g>=%5.2f"), time, af_len, g2);
-
- if (af_len > FL_ACCEL_NOISE_LEVEL && g2 > 1) {
- const double norm_out = 1.0 / af_len;
- vector_3d_mul_number(af, norm_out, gu);
- *w2gu = self->gravity_estimator.base.decay_rate / (fl_square(gu[X]) + fl_square(gu[Y]) + fl_square(gu[Z] - 1) + GEO_DIMENSION * DEFAULT_ACCELERATION_SIGMA2);
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, decay=%.16g, gu=(%.16g, %.16g, %.16g), af_len=%.16g, overlap=%.16g, g2=%.16g"),
- time, self->gravity_estimator.base.decay_rate, gu[X], gu[Y], gu[Z], af_len, overlap, g2);
- } else {
- vector_3d_set(gu, 0, 0, 1); /* default */
- *w2gu = self->gravity_estimator.base.decay_rate / (GEO_DIMENSION * DEFAULT_ACCELERATION_SIGMA2);
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16g, decay=%.16g, gu=(%.16g, %.16g, %.16g), af_len=%.16g, overlap=%.16g, g2=%.16g"),
- time, self->gravity_estimator.base.decay_rate, gu[X], gu[Y], gu[Z], af_len, overlap, g2);
- }
-
- Vector_3d al; /* Linear acceleration in global frame. */
- const double g = sqrt(g2);
- if (g > 1)
- vector_3d_linear_combination(EARTH_GRAVITY / g, acc, -EARTH_GRAVITY, gu, al);
- else
- vector_3d_copy(al, acc);
-
- motion_detector_process(&self->motion_detector, time, al);
-}
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file AccelerometerFilter.h
- * @brief Accelerometer calibration (scale and sampling frequency), and
- * low-pass 3D filtering.
- */
-
-#pragma once
-#ifndef __ACCELEROMETER_FILTER_H__
-#define __ACCELEROMETER_FILTER_H__
-
-#include "FrequencyEstimator.h"
-#include "Butterworth3dFilter.h"
-#include "MotionDetector.h"
-#include "GravityEstimator.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/** Accelerometer calibration (scale and sampling frequency), and low-pass 3D filtering */
-typedef struct {
- FrequencyEstimator frequency_estimator;
- Butterworth3dFilter lp_3d_filter;
- GravityEstimator gravity_estimator;
- MotionDetector motion_detector;
-} AccelerometerFilter;
-
-/**
- * @brief Initialization of the AccelerometerFilter object (constructor).
- * @param[in] on_motion_state_changed Callback to be invoked when the detected state of motion changes.
- */
-void accelerometer_filter_init(AccelerometerFilter* self, MotionDetectorStateChangeListener on_motion_state_changed);
-
-void accelerometer_filter_exit(AccelerometerFilter* self);
-
-/**
- * @brief Reset of the internal state back to initial one.
- * This function is used for repetitive testing and module soft restarts.
- * @param[in] av Stationary acceleration state to be set (typically {0, 0, g}).
- */
-void accelerometer_filter_reset(AccelerometerFilter* self, const_Vector_3d_ref av);
-
-/**
- * @brief Processing of a single sample from the accelerometer. This function performs:
- * - estimation of the sampling frequency,
- * - estimation of the gravity direction via low-pass 3D filter (cf. LP3D unit),
- * - estimation of the gravity value (cf. GRES unit) and scale correction,
- * - estimation of the linear acceleration i.e. with subtracted gravity component,
- * - motion state detection (cf. MOTI unit)
- * @param[in] time Time of the event in seconds.
- * @param[in] acc Measured 3D acceleration vector in global frame.
- * @param[out] gu Unit vector along gravity direction oriented upwards.
- * @param[out] w2gu Estimated inverse covariance (squared weight) of a @gu.
- */
-void accelerometer_filter_process(AccelerometerFilter* self,
- double time, const_Vector_3d_ref acc, Vector_3d_ref gu, double *w2gu);
-
-/** @return double */
-#define accelerometer_filter_get_update_rate(self) gravity_estimator_get_update_rate(&(self)->gravity_estimator)
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __ACCELEROMETER_FILTER_H__ */
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
- /* custom logging threshold - keep undefined on the repo */
- /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */
-#endif
-
-#include "Butterworth3dFilter.h"
-#include "math-ext.h"
-#include "debug_util.h"
-
-#define LP3D_FORMAT "% 6.2f"
-
-void butterworth_3d_filter_init(Butterworth3dFilter* self, double f)
-{
- LOG_FUSED_DEV(DBG_LOW, INDENT("()"));
- self->cut_off_frequency = f;
- self->lp3d_G = 0;
- self->lp3d_B1 = -2;
- self->lp3d_B2 = 1;
- butterworth_3d_filter_reset(self);
- LOG_FUSED_DEV(DBG_LOW, UNINDENT("(): OK"));
-}
-
-void butterworth_3d_filter_exit(Butterworth3dFilter* self)\
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
-}
-
-void butterworth_3d_filter_set_sampling_frequency(Butterworth3dFilter* self, double f)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("frequency() f=%.16e, cut_off=%.16e"), f, self->cut_off_frequency);
-
- const double omegaC = tan(M_PI * self->cut_off_frequency / f);
- const double omegaC2 = fl_square(omegaC);
- const double _B0 = 1.0 / (1 + 2 * omegaC * cos(M_PI_4) + omegaC2);
- self->lp3d_B1 = _B0 * 2*(omegaC2 - 1);
- self->lp3d_B2 = _B0 * (1 - 2 * omegaC * cos(M_PI_4) + omegaC2);
- self->lp3d_G = _B0 * omegaC2;
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%g Hz): OK"), f);
-}
-
-void butterworth_3d_filter_reset(Butterworth3dFilter* self)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
- vector_3d_clear_array(self->lp3d_u);
- vector_3d_clear_array(self->lp3d_v);
-}
-
-void butterworth_3d_filter_reset_to(Butterworth3dFilter* self, const_Vector_3d_ref stationary)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_FORMAT ", " LP3D_FORMAT ", " LP3D_FORMAT "})"), stationary[0], stationary[1], stationary[2]);
- unsigned i = GEO_DIMENSION;
- do {
- --i;
- /* up-conversion */
- self->lp3d_u[TIME_SHIFT_1][i] = stationary[i];
- self->lp3d_u[TIME_SHIFT_2][i] = stationary[i];
- self->lp3d_v[TIME_SHIFT_1][i] = stationary[i];
- self->lp3d_v[TIME_SHIFT_2][i] = stationary[i];
- } while (i);
-}
-
-const_Vector_3d_ref butterworth_3d_filter_process(Butterworth3dFilter* self, const_Vector_3d_ref u)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("({" LP3D_FORMAT ", " LP3D_FORMAT ", " LP3D_FORMAT "})"), u[0], u[1], u[2]);
- unsigned i = GEO_DIMENSION;
- do {
- --i;
-#define u1 self->lp3d_u[TIME_SHIFT_1][i]
-#define u2 self->lp3d_u[TIME_SHIFT_2][i]
-#define v1 self->lp3d_v[TIME_SHIFT_1][i]
-#define v2 self->lp3d_v[TIME_SHIFT_2][i]
- const double u0 = u[i];
- const double v0 = self->lp3d_G * (u0 + 2 * u1 + u2) - self->lp3d_B1 * v1 - self->lp3d_B2 * v2;
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("process() i=%i, lp3d_G=%.16e, lp3d_B1=%.16e, u0=%.16e, u1=%.16e, u2=%.16e, v0=%.16e, v1=%.16e, v2=%.16e"),
- i, self->lp3d_G, self->lp3d_B1, u0, u1, u2, v0, v1, v2);
-
- u2 = u1;
- u1 = u0;
- v2 = v1;
- v1 = v0;
-#undef u1
-#undef u2
-#undef v1
-#undef v2
- } while (i);
- return self->lp3d_v[TIME_SHIFT_1];
-}
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file Butterworth3dFilter.h
- * @brief Low-pass 3D 2nd order Butterworth filter.
- */
-
-#pragma once
-#ifndef __BUTTERWORTH_3D_FILTER_H__
-#define __BUTTERWORTH_3D_FILTER_H__
-
-#include "Vector.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-typedef enum {
- TIME_SHIFT_1, /* t - 1 */
- TIME_SHIFT_2, /* t - 2 */
- TIME_SHIFT_COUNT
-} TimeShift;
-
-typedef struct {
- /* Filter coefficients. */
- double cut_off_frequency;
- double lp3d_G; /* omega^2/B_0 */
- double lp3d_B1; /* B_1/B_0 */
- double lp3d_B2; /* B_2/B_0 */
-
- /* Memory of past states. */
- Vector_3d lp3d_u[TIME_SHIFT_COUNT];
- Vector_3d lp3d_v[TIME_SHIFT_COUNT];
-} Butterworth3dFilter;
-
-void butterworth_3d_filter_init(Butterworth3dFilter* self, double f);
-
-void butterworth_3d_filter_exit(Butterworth3dFilter* self);
-
-void butterworth_3d_filter_reset(Butterworth3dFilter* self);
-
-/**
- * @brief Adjust internal parameters to the given sampling frequency and reset the the past state memory.
- * @param[in] f The sampling frequency in [Hz].
- */
-void butterworth_3d_filter_set_sampling_frequency(Butterworth3dFilter* self, double f);
-
-/**
- * @brief Reset to the given stationary state (input = output).
- * @param[in] stationary 3D vector of acceleration measured in tangent coordinate frame.
- */
-void butterworth_3d_filter_reset_to(Butterworth3dFilter* self, const_Vector_3d_ref stationary);
-
-/**
- * @brief Process single input sample through the 3D Butterworth filter.
- * @param[in] u 3D acceleration input vector.
- * @return 3D acceleration output vector.
- */
-const_Vector_3d_ref butterworth_3d_filter_process(Butterworth3dFilter* self, const_Vector_3d_ref u);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __BUTTERWORTH_3D_FILTER_H__ */
*/
#include "CalibrationFilter.h"
-#include "Conversions.h"
+#include "Time.h"
#include "math-ext.h"
#include <stdio.h>
-static Vector_3d zero = { 0, 0, 0 };
+static const Vector_3d ZERO = { 0, 0, 0 };
/** Minimum measurements in stable state to calculate new calibration parameters. */
static const int MIN_MEAS = 100;
calibration_filter_reset(self);
}
-void calibration_filter_exit(CalibrationFilter* self)
-{
-}
-
void calibration_filter_reset(CalibrationFilter* self)
{
self->calibrating = false;
- self->calibration_time = FL_UNDEFINED_TIME;
+ SET_UNKNOWN_TIME(self->calibration_time);
}
-Vector_3d_ref calibration_filter_filter(CalibrationFilter* self, double time,
+const_Vector_3d_ref calibration_filter_filter(CalibrationFilter* self, double time,
const_Vector_3d_ref data, bool is_stable)
{
if (self->autocalibration) {
self->sum[i] += data[i];
self->sum2[i] += fl_square(data[i]);
}
- return zero;
+ return ZERO;
} else {
double variance = self->sum2[i] / self->cal_nr - m * m;
std_dev[i] = variance > 0 ? sqrt(variance) : 0;
}
- double dt = (!KNOWN_TIME(self->calibration_time)) || time < self->calibration_time
+ double dt = (!IS_KNOWN_TIME(self->calibration_time)) || time < self->calibration_time
? DAY : time - self->calibration_time;
const double curPrec = self->cal_prec * exp(-dt / DT);
for (i = GEO_DIMENSION; i;) {
i--;
- self->bias[i] = self->bias[i] * alpha + mean[i] * alpha1;
+ self->bias[i] = self->bias[i] * alpha - mean[i] * alpha1;
}
self->cal_prec = newPrec + curPrec;
self->calibration_time = time;
}
}
}
- vector_3d_sub_vector_3d(data, self->bias, self->output);
+ vector_3d_add_vector_3d(data, self->bias, self->output);
return self->output;
}
void calibration_filter_clear_calibration(CalibrationFilter* self)
{
- self->calibration_time = FL_UNDEFINED_TIME;
+ SET_UNKNOWN_TIME(self->calibration_time);
self->cal_prec = 0;
vector_3d_clear(self->bias);
}
if (params != NULL) {
snprintf(params, PARAMS_LEN, "%i,%.9f,%.10f,%.10f,%.10f,%.10f",
PARAMS_VERSION,
- self->calibration_time == FL_UNDEFINED_TIME ? 0 :
- self->calibration_time + self->utc_offset,
+ 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]);
}
return params;
if (sscanf(params, "%i,%lf,%lf,%lf,%lf,%lf",
&version, &cal_time, &cal_prec, &bias0, &bias1, &bias2) == 6) {
if (version == PARAMS_VERSION) {
- self->calibration_time = cal_time == 0 ? FL_UNDEFINED_TIME : cal_time - self->utc_offset;
+ if (cal_time == 0) {
+ SET_UNKNOWN_TIME(self->calibration_time);
+ } else {
+ self->calibration_time = cal_time - self->utc_offset;
+ }
self->cal_prec = cal_prec;
self->bias[0] = bias0;
self->bias[1] = bias1;
#include <stdbool.h>
-
typedef struct {
double utc_offset; /* Difference between UTC time and monotonic time in [s]. */
double calibration_time;
} CalibrationFilter;
extern void calibration_filter_init(CalibrationFilter* self, double utcOffset);
-extern void calibration_filter_exit(CalibrationFilter* self);
+
extern void calibration_filter_reset(CalibrationFilter* self);
-extern Vector_3d_ref calibration_filter_filter(CalibrationFilter* self, double time, const_Vector_3d_ref data, bool is_stable);
+extern const_Vector_3d_ref calibration_filter_filter(CalibrationFilter* self, double time, const_Vector_3d_ref data, bool is_stable);
extern void calibration_filter_set_enable_autocalibration(CalibrationFilter* self, bool enable);
extern void calibration_filter_clear_calibration(CalibrationFilter* self);
+
extern char* calibration_filter_get_calibration(CalibrationFilter* self);
+
extern void calibration_filter_set_calibration(CalibrationFilter* self, const char* params);
#endif /* __CALIBRATIONFILTER_H_ */
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "Conversions.h"
-
-double conversions_radians_to_positive_degrees(double radians)
-{
- double d = (180 / M_PI) * radians;
- /* When the value only occasionally enters adjacent branches this is more
- efficient a way than division modulo. */
- if (d < 0)
- do {
- d += 360;
- } while (d < 0);
- else if (d >= 360)
- do {
- d -= 360;
- } while (d >= 360);
- return d;
-}
-
-double conversions_radians_to_balanced_degrees(double radians)
-{
- double d = (180 / M_PI) * radians;
- /* When the value only ocassionally enters adjacent branches this is more
- efficient a way than division modulo. */
- if (d < -180)
- do {
- d += 360;
- } while (d < -180);
- else if (d >= 180)
- do {
- d -= 360;
- } while (d >= 180);
- return d;
-}
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file Conversions.h
- * @brief Units conversion functions
- */
-
-#pragma once
-#ifndef __CONVERSIONS_H__
-#define __CONVERSIONS_H__
-
-#include "math-ext.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#define FL_UNDEFINED_TIME -1e+48
-#define KNOWN_TIME(t) (t > FL_UNDEFINED_TIME)
-
-/**
- * @brief Convert degrees to radians.
- * @param[in] degrees Value in degrees.
- * @return Corresponding value in radians.
- */
-#define conversions_degrees_to_radians(degrees) ((M_PI / 180) * (degrees))
-
-/**
- * @brief Convert radians to degrees.
- * @param[in] radians Value in radians.
- * @return Corresponding value in degrees.
- */
-#define conversions_radians_to_degrees(radians) ((180 / M_PI) * (radians))
-
-/**
- * @brief Convert radians to degrees in the [0,360) range
- * @param[in] radians Arbitrary value in radians.
- * @return Value in degrees projected onto the [0,360) branch.
- */
-double conversions_radians_to_positive_degrees(double radians);
-
-/**
- * @brief Convert radians to degrees in the [-180,180) range.
- * @param[in] radians Arbitrary value in radians.
- * @return Value in degrees projected onto the [-180,180) branch.
- */
-double conversions_radians_to_balanced_degrees(double radians);
-
-/** Convert radians to latitude degrees [-90,90] */
-#define conversions_radians_to_latitude conversions_radians_to_balanced_degrees
-
-/** Convert radians to longitude degrees [0, 360) */
-#define conversions_radians_to_longitude conversions_radians_to_positive_degrees
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __CONVERSIONS_H_ */
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "FrequencyEstimator.h"
-#include "Conversions.h"
-#include "debug_util.h"
-
-#define FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD (1.0 * 1.0)
-
-void frequency_estimator_init(FrequencyEstimator* self, double initial_frequency)
-{
- self->frequency = initial_frequency;
- self->frequency_update_time = FL_UNDEFINED_TIME;
- self->samples = 0;
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("frequency_estimator_process() last_t=%.16e, last_f=%.16e"), self->frequency_update_time, self->frequency);
-}
-
-bool frequency_estimator_process(FrequencyEstimator* self, double time)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("t=%.16e, last_t=%.16e, f=%.16e, c=%i"), time, self->frequency_update_time, self->frequency, self->samples);
-
- bool frequency_updated = false;
- if (KNOWN_TIME(self->frequency_update_time)) {
- ++(self->samples);
- const double dt = time - self->frequency_update_time;
-
- if (dt > 0) {
- const double f = self->samples / dt;
- const double df = fabs(f - self->frequency);
- if (dt * df >= FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD) {
- self->samples = 0;
- self->frequency = f;
- self->frequency_update_time = time;
- frequency_updated = true;
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("last_t=%.16e, f=%.16e"), time, self->frequency);
- }
- }
-
- } else {
- self->frequency_update_time = time;
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("last_t=%.16e, f=%.16e"), time, self->frequency);
- }
-
- return frequency_updated;
-}
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file FrequencyEstimator.h
- * @brief Estimates frequency of arbitrary timed events.
- */
-
-#pragma once
-#ifndef __FREQUENCYESTIMATOR_H_
-#define __FREQUENCYESTIMATOR_H_
-
-#include <stdbool.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-typedef struct {
- double frequency; /* Current estimated frequency. */
- double frequency_update_time; /* Time of last frequency update. */
- unsigned samples; /* Number of samples from last frequency update. */
-} FrequencyEstimator;
-
-/**
- * @brief Initialization of the FrequencyEstimator object (constructor).
- * @param[in] initial_frequency Initial frequency.
- */
-extern void frequency_estimator_init(FrequencyEstimator* self, double initial_frequency);
-
-/** (double) Returns current frequency. */
-#define frequency_estimator_get_frequency(self) ((self)->frequency)
-
-/**
- * @brief Called when new event happens.
- * @param[in] time Time of event.
- * @return Is new frequency calculated.
- */
-extern bool frequency_estimator_process(FrequencyEstimator* self, double time);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __FREQUENCYESTIMATOR_H_ */
* limitations under the License.
*/
-#include "GyroscopeFilter.h"
-#include "Conversions.h"
#include "FusionEngine.h"
+#include "HeadingRateCalculator.h"
+#include "GyroscopeFilter.h"
+#include "math-ext.h"
#include "debug_util.h"
+#include "Time.h"
-#define TIME_FORMAT "%.3fs"
+/** Default deviation of heading change rate in [rad/s]. */
+#define HEADING_RATE_DEV (5 * M_PI / 180)
-/* the bigger the default standard deviations, the swifter response to first measurements */
-#define INITIAL_POSITION_SIGMA2 SQUARE(1024 * M_2PI * EARTH_RADIUS)
-#define INITIAL_VELOCITY_SIGMA2 (1024 * 1024 * SQUARE(FL_DEFAULT_VELOCITY_SIGMA))
-#define INITIAL_ACCELERATION_SIGMA2 DEFAULT_ACCELERATION_SIGMA2
+#define DEFAULT_ROTATION_SIGMA2 SQUARE(1.0 / 256) /* [(rad/s)^2] */
-void fusion_engine_init(FusionEngine* self, MotionDetectorStateChangeListener on_motion_state_changed, double utcOffset)
+void fusion_engine_init(FusionEngine* self, double utcOffset)
{
- LOG_FUSED_DEV(DBG_LOW, INDENT("(motion_cb=%p)"), on_motion_state_changed);
- orientation_init(&self->orientation, on_motion_state_changed, DEFAULT_ROTATION_SIGMA2);
+ LOG_FUSED_DEV(DBG_LOW, INDENT(""));
+ 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);
+ time_offset_init(&self->timestamp_offset, utcOffset);
self->engine_started = false;
self->enable_gyro_filter = false;
peace_detector_init(&self->acc_peace_detector, 0.2, 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);
LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
}
-void fusion_engine_exit(FusionEngine* self)
+void fusion_engine_destroy(FusionEngine* self)
{
LOG_FUSED_DEV(DBG_LOW, INDENT("()"));
- time_offset_exit(&self->timestamp_offset);
- gyroscope_filter_exit(&self->gyro_bias);
- orientation_exit(&self->orientation);
+ location_filter_destroy(&self->location_filter);
LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
}
void fusion_engine_reset(FusionEngine* self)
{
LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
- self->last_notification_time = FL_UNDEFINED_TIME;
+ SET_UNKNOWN_TIME(self->last_notification_time);
self->was_position = false;
- location_filter_init(&self->kalf, 0, 0, EARTH_RADIUS, INITIAL_POSITION_SIGMA2, INITIAL_VELOCITY_SIGMA2, INITIAL_ACCELERATION_SIGMA2);
+ location_filter_reset(&self->location_filter);
self->fix_status = LOCATION_STATUS_NO_FIX;
- tangent_frame_init(&self->__te);
- time_offset_reset(&self->timestamp_offset);
orientation_reset(&self->orientation);
+ gyroscope_filter_reset(&self->gyro_bias);
+ 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);
calibration_filter_reset(&self->calibration_filter);
-}
-
-static void fusion_engine_spherical_to_tangent2d(FusionEngine* self,
- double latitude, double longitude, double sigma_of_horizontal_pos, Vector_3d_ref pm, Vector_3d_ref s2pm)
-{
- self->fix_status = LOCATION_STATUS_2D_FIX;
- double radius = location_filter_get_position(&self->kalf)[Z];
- tangent_frame_spherical_to_tangent(&self->__te, latitude, longitude, radius, pm);
-
- s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(sigma_of_horizontal_pos);
-}
-
-static void fusion_engine_spherical_to_tangent3d(FusionEngine* self,
- double latitude, double longitude, double altitude, double hor_accuracy, double ver_accuracy,
- double speed, double sigma_of_speed, double direction, double climb, double sigma_of_climb,
- Vector_3d_ref pm, Vector_3d_ref vm, Vector_3d_ref s2pm, Vector_3d_ref s2vm)
-{
- self->fix_status = LOCATION_STATUS_3D_FIX;
- double radius = EARTH_RADIUS + altitude;
- tangent_frame_spherical_to_tangent(&self->__te, latitude, longitude, radius, pm);
-
- double cd = pm[Z] / radius;
- double cd2 = SQUARE(cd);
- double sd2 = 1 - cd2;
- if (sd2 < 0) {
- cd2 = 1;
- sd2 = 0;
- }
-
- /* Notice: in principle we should rotate the diagonal sigma matrix here (two matrix multiplications).
- This approximation assumes the old and new points are close. */
- if (ver_accuracy > 0) {
- double s2h = fl_square(hor_accuracy);
- double s2v = fl_square(ver_accuracy);
- s2pm[X] = s2pm[Y] = cd2 * s2h + sd2 * s2v;
- s2pm[Z] = sd2 * s2h + cd2 * s2v;
- } else {
- s2pm[X] = s2pm[Y] = s2pm[Z] = fl_square(hor_accuracy);
- }
-
- tangent_frame_spherical_to_tangent_speed(&self->__te, pm, speed, direction, climb, vm);
- if (sigma_of_climb > 0) {
- s2vm[X] = s2vm[Y] = cd2 * fl_square(sigma_of_speed) + sd2 * fl_square(sigma_of_climb);
- s2vm[Z] = sd2 * fl_square(sigma_of_speed) + cd2 * fl_square(sigma_of_climb);
- } else {
- s2vm[X] = s2vm[Y] = s2vm[Z] = fl_square(sigma_of_speed);
- }
+ hr_calculator_reset(&self->hr_calculator);
}
/**
*/
static bool fusion_engine_signal_updated_location(FusionEngine* self, double time)
{
- LOG_FUSED_DEV(DBG_ERR, "t=%g, fixing=%g", time, self->last_notification_time);
+ 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) {
- time += time_offset_get(&self->timestamp_offset);
- if (!KNOWN_TIME(self->last_notification_time) ||
+ if (!IS_KNOWN_TIME(self->last_notification_time) ||
time - self->last_notification_time >= FL_MIN_NOTIFICATION_INTERVAL) {
self->last_notification_time = time;
return true;
return false;
}
-/** Filter input: Enter the raw position measurement */
+static void create_wgs_conversion(WgsConversion* self, double latitude, double longitude)
+{
+ if (wgs_conversion_is_valid(self) == false)
+ wgs_conversion_create(self, latitude, longitude);
+}
+
+static void update_wgs_conversion(FusionEngine* self)
+{
+ Vector_3d pos;
+ location_filter_get_position(&self->location_filter, pos);
+ LOG_FUSED_DEV(DBG_ERR, "x=%.17e, y=%.17e", pos[X], pos[Y]);
+
+ if (fl_square(pos[X]) + fl_square(pos[Y]) > SQUARE(1000.0)) {
+ double latitude;
+ double longitude;
+ wgs_conversion_local_to_wgs(&self->wgs_conversion, pos[X], pos[Y], &latitude, &longitude);
+ wgs_conversion_create(&self->wgs_conversion, latitude, longitude);
+ wgs_conversion_wgs_to_local(&self->wgs_conversion, latitude, longitude, &pos[X], &pos[Y]);
+ LOG_FUSED_DEV(DBG_ERR, "new x=%.17e, new y=%.17e", pos[X], pos[Y]);
+ location_filter_set_predicted_state(&self->location_filter, pos);
+ }
+}
+
bool fusion_engine_position_2d_event(FusionEngine* self, double time,
double latitude, double longitude, double sigma_of_horizontal_pos)
{
- double t = time_offset_correct_time(&self->timestamp_offset, time);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g, %g), sp=%gm)"),
- t, latitude, longitude, sigma_of_horizontal_pos);
- Vector_3d pm; /* p-measurement */
- Vector_3d s2pm; /* p-measurement variance */
+ 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->fix_status = LOCATION_STATUS_2D_FIX;
+ create_wgs_conversion(&self->wgs_conversion, latitude, longitude);
- fusion_engine_spherical_to_tangent2d(self, latitude, longitude, sigma_of_horizontal_pos, pm, s2pm);
- location_filter_kalman_p(&self->kalf, t, pm, s2pm);
+ Vector_3d pm; /* pos measurement */
+ 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);
+
+ 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);
self->was_position = true;
- tangent_frame_create(&self->__te, location_filter_get_position(&self->kalf), location_filter_get_velocity(&self->kalf));
- location_filter_update_state(&self->kalf);
+ update_wgs_conversion(self);
+ 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)
+{
+ 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->fix_status = LOCATION_STATUS_2D_FIX;
+ Vector_3d pm; /* pos measurement */
+ 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);
+
+ 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);
+ self->was_position = true;
return fusion_engine_signal_updated_location(self, t);
}
-/** Filter input: Enter the raw position measurement */
bool fusion_engine_position_3d_event(FusionEngine* self, double time,
double latitude, double longitude, double altitude,
double hor_accuracy, double ver_accuracy, double speed, double sigma_of_speed,
double direction, double climb, double sigma_of_climb)
{
double t = time_offset_correct_time(&self->timestamp_offset, time);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g, %g, %g), dir=%g, spd=%g, climb=%g, acc=%g, sv=%gm/s)"),
- time, latitude, longitude, altitude, direction, speed, climb, hor_accuracy, sigma_of_speed);
-
- Vector_3d pm; /* p-measurement */
- Vector_3d vm; /* v-measurement */
- Vector_3d s2pm; /* p-measurement variance */
- Vector_3d s2vm; /* v-measurement variance */
+ 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);
- fusion_engine_spherical_to_tangent3d(self, latitude, longitude, altitude,
- hor_accuracy, ver_accuracy, speed, sigma_of_speed,
- direction, climb, sigma_of_climb, pm, vm, s2pm, s2vm);
+ create_wgs_conversion(&self->wgs_conversion, latitude, longitude);
+ self->fix_status = LOCATION_STATUS_3D_FIX;
+ Vector_3d pm; /* pos measurement */
+ 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 vhe = vector_2d_length(location_filter_get_velocity(&self->kalf));
- double azimuth = conversions_degrees_to_radians(direction);
+ 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);
- location_filter_set_z_rot_speed(&self->kalf, orientation_get_z_rot_speed(&self->orientation));
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("GYROO(), t=%.16e, wze=%.16e"), t, orientation_get_z_rot_speed(&self->orientation));
+ location_filter_update_heading_rate(&self->location_filter, t,
+ orientation_get_heading_rate(&self->orientation), HEADING_RATE_DEV);
#endif
- location_filter_kalman_pv(&self->kalf, t, pm, vm, s2pm, s2vm);
+ location_filter_update_position_speed(&self->location_filter, t, pm, hor_accuracy, speed, sigma_of_speed);
self->was_position = true;
- tangent_frame_create(&self->__te, location_filter_get_position(&self->kalf), location_filter_get_velocity(&self->kalf));
- location_filter_update_state(&self->kalf);
+ 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)
+{
+ 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);
+
+ Vector_3d pm; /* pos measurement */
+
+ self->fix_status = LOCATION_STATUS_3D_FIX;
+ pm[X] = x;
+ pm[Y] = y;
+ pm[Z] = altitude;
+
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+ 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);
+ 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);
+ self->was_position = true;
return fusion_engine_signal_updated_location(self, t);
}
void fusion_engine_get_wgs_location(FusionEngine* self, WgsLocation* location)
{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("WGS I(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
- self->last_notification_time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
- self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
-
if (location == NULL) {
LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(), NULL pointer"));
return;
}
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(), last_notification_time=%gs"), self->last_notification_time);
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(), last_notification_time=%.17es"), self->last_notification_time);
- const const_Vector_3d_ref pos = location_filter_get_predicted_position(&self->kalf);
location->time = self->last_notification_time;
+ location->utc_time = self->last_notification_time + time_offset_get(&self->timestamp_offset);
+
+ const double x = location_filter_get_x(&self->location_filter);
+ const double y = location_filter_get_y(&self->location_filter);
+ const double z = location_filter_get_z(&self->location_filter);
+ wgs_conversion_local_to_wgs(&self->wgs_conversion, x, y, &location->latitude, &location->longitude);
+ location->altitude = z;
+
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("POS (), t=%.9f, x=%.17e, y=%.17e, z=%.17e => lat=%.17e, lon=%.17e, alt=%.17e"),
+ location->time, x, y, z, location->latitude, location->longitude, location->altitude);
+
+ location->speed = location_filter_get_horizontal_speed(&self->location_filter);
+ const double heading = location_filter_get_heading(&self->location_filter);
+ wgs_conversion_local_bearing_to_wgs_azimuth(&self->wgs_conversion, x, y, heading, &location->direction);
+ location->climb = location_filter_get_vertical_speed(&self->location_filter);
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPD (), t=%.9f, heading=%.17e, speed=%.17e, dir=%.17e, climb=%.17e"),
+ location->time, heading, location->speed, location->direction, location->climb);
+
+ location->hor_dev = location_filter_get_hor_dev(&self->location_filter);
+ location->ver_dev = location_filter_get_ver_dev(&self->location_filter);
+ location->speed_dev = location_filter_get_speed_dev(&self->location_filter);
+ location->climb_dev = location_filter_get_climb_dev(&self->location_filter);
+}
- tangent_frame_tangent_to_spherical(&self->__te, pos, &location->latitude, &location->longitude, &location->altitude);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("POS (), t=%.16e, p0=%.16e, p1=%.16e, p2=%.16e => lat=%.16e, lon=%.16e, alt=%.16e"),
- location->time, pos[0], pos[1], pos[2], location->latitude, location->longitude, location->altitude);
-
- const const_Vector_3d_ref vel = location_filter_get_predicted_velocity(&self->kalf);
- tangent_frame_tangent_to_spherical_speed(&self->__te, pos, vel, &location->speed, &location->direction, &location->climb);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPD (), t=%.16e, v0=%.16e, v1=%.16e, v2=%.16e, speed=%.16e, dir=%.16e, climb=%.16e"),
- location->time, vel[0], vel[1], vel[2], location->speed, location->direction, location->climb);
-
- const const_fl_kalf_sigma_cube_ref s2x = location_filter_get_predicted_covariance(&self->kalf);
- location->hor_dev = sqrt((s2x[X][POS][POS] + s2x[Y][POS][POS]) * 0.5);
- location->ver_dev = s2x[Z][POS][POS];
- location->speed_dev = sqrt((s2x[X][VEL][VEL] + s2x[Y][VEL][VEL]) * 0.5);
- location->climb_dev = s2x[Z][VEL][VEL];
+void fusion_engine_get_local_position(FusionEngine* self, WgsLocation* location)
+{
+ if (location == NULL) {
+ LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("(), NULL pointer"));
+ return;
+ }
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(), last_notification_time=%.17es"), self->last_notification_time);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("WGS O(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
- self->last_notification_time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
- self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+ location->time = self->last_notification_time;
+ location->utc_time = self->last_notification_time + time_offset_get(&self->timestamp_offset);
+
+ location->latitude = location_filter_get_y(&self->location_filter);
+ location->longitude = location_filter_get_x(&self->location_filter);
+ location->altitude = location_filter_get_z(&self->location_filter);
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("POS (), t=%.9f, lat=%.17e, lon=%.17e, alt=%.17e"),
+ location->time, location->latitude, location->longitude, location->altitude);
+
+ location->speed = 0;
+ location->direction = 0;
+ location->climb = 0;
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPD (), t=%.9f, speed=%.17e, dir=%.17e, climb=%.17e"),
+ location->time, location->speed, location->direction, location->climb);
+
+ location->hor_dev = location_filter_get_hor_dev(&self->location_filter);
+ location->ver_dev = location_filter_get_ver_dev(&self->location_filter);
+ location->speed_dev = location_filter_get_speed_dev(&self->location_filter);
+ 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)
{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("SPEED(), t=%.16e, speed=%.16e, sigma=%.16e"), time, speed, sigma_of_speed);
- location_filter_update_speed(&self->kalf, time, speed, sigma_of_speed);
-
- tangent_frame_create(&self->__te, location_filter_get_position(&self->kalf), location_filter_get_velocity(&self->kalf));
- location_filter_update_state(&self->kalf);
+ 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);
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));
+ if (hr_calculator_new_heading(&self->hr_calculator,
+ orientation_get_heading_time(&self->orientation),
+ orientation_get_heading(&self->orientation)
+ )) {
+ location_filter_update_heading_rate(&self->location_filter,
+ hr_calculator_get_timestamp(&self->hr_calculator),
+ hr_calculator_get_heading_rate(&self->hr_calculator),
+ hr_calculator_get_heading_rate_deviation(&self->hr_calculator));
+ }
+}
+
bool fusion_engine_acc_event(FusionEngine* self, double time, double x, double y, double z)
{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC I(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
- time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
- self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+ 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);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", RAC, a=(%g, %g, %g)[m/s^2])"), time, x, y, z);
- /* float, math system -> double, geographic system, rotated into local (tangent) frame */
Vector_3d acc = { x, y, z };
peace_detector_new_data(&self->acc_peace_detector, time, acc);
- orientation_new_acceleration(&self->orientation, time, acc);
- /* process linear acceleration */
- location_filter_clear_acc(&self->kalf, time);
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC O(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
- time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
- self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+ if (orientation_new_acceleration(&self->orientation, time, acc))
+ on_new_heading(self);
return fusion_engine_signal_updated_location(self, time);
}
bool fusion_engine_gyro_event(FusionEngine* self, double time, double x, double y, double z)
{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("GYROI(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
- time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
- self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
-
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%.9f, a=(%.17e, %.17e, %.17e))"), time, x, y, z);
time_offset_reference_time(&self->timestamp_offset, time);
- const Vector_3d angular_velocity = { -x, -y, -z };
+ const Vector_3d angular_velocity = { x, y, z };
peace_detector_new_data(&self->gyro_peace_detector, time, angular_velocity);
const_Vector_3d_ref av = angular_velocity;
+
if (self->enable_static_calibration) {
av = calibration_filter_filter(&self->calibration_filter, time, av,
peace_detector_is_stable(&self->gyro_peace_detector) &&
if (self->enable_gyro_filter)
av = gyroscope_filter_process(&self->gyro_bias, time, av);
- if (orientation_new_angular_velocity(&self->orientation, time, av, DEFAULT_ROTATION_SIGMA2)) {
- location_filter_set_z_rot_speed(&self->kalf, orientation_get_z_rot_speed(&self->orientation));
- location_filter_update_acc(&self->kalf, time);
- }
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("GYROO(), t=%.16e, sp0=%.16e, sp1=%.16e, sp2=%.16e, se0=%.16e, se1=%.16e, se2=%.16e"),
- time, self->kalf.prior.x.p[0], self->kalf.prior.x.p[1], self->kalf.prior.x.p[2],
- self->kalf.posterior.x.p[0], self->kalf.posterior.x.p[1], self->kalf.posterior.x.p[2]);
+ if (orientation_new_angular_velocity(&self->orientation, time, av))
+ on_new_heading(self);
return false;
}
+bool fusion_engine_heading_event(FusionEngine* self, double time, double heading)
+{
+ time_offset_reference_time(&self->timestamp_offset, time);
+ if (orientation_update_heading(&self->orientation, time, heading))
+ on_new_heading(self);
+
+ return fusion_engine_signal_updated_location(self, time);
+}
+
+bool fusion_engine_heading_rate_event(FusionEngine* self, double time, double heading_rate, double heading_rate_deviation)
+{
+ time_offset_reference_time(&self->timestamp_offset, time);
+ location_filter_update_heading_rate(&self->location_filter, time, heading_rate, heading_rate_deviation);
+ return fusion_engine_signal_updated_location(self, time);
+}
+
+bool fusion_engine_speed_heading_rate_event(FusionEngine* self, double time,
+ double speed, double speed_deviation, double heading_rate, double heading_rate_deviation)
+{
+ time_offset_reference_time(&self->timestamp_offset, time);
+ location_filter_update_speed_heading_rate(&self->location_filter, time, speed, speed_deviation, heading_rate, heading_rate_deviation);
+
+ return fusion_engine_signal_updated_location(self, time);
+}
+
/**
* For ips_devel test
*/
void fusion_engine_get_orientation(FusionEngine* self, double* o)
{
orientation_get_orientation(&self->orientation, o);
+ o[8] = hr_calculator_get_heading(&self->hr_calculator);
+ o[9] = location_filter_get_heading(&self->location_filter);
}
/**
/**
* @file FusionEngine.h
- * @brief Implementation of a 4D (2D position + 2D velocity) Kalman filter
+ * @brief Fuses different measurements to obtain smooth position estimate.
*/
#pragma once
#ifndef __FUSION_ENGINE_H__
#define __FUSION_ENGINE_H__
-#include <stdbool.h>
#include "parameters.h"
-#include "MotionDetector.h"
-#include "Conversions.h"
-#include "Matrix.h"
-#include "TangentFrame.h"
+#include "WgsConversion.h"
#include "LocationFilter.h"
-#include "AccelerometerFilter.h"
#include "GyroscopeFilter.h"
#include "TimeOffset.h"
#include "Orientation.h"
#include "PeaceDetector.h"
#include "CalibrationFilter.h"
+#include "HeadingRateCalculator.h"
+#include <stdbool.h>
#include <memory.h>
#ifdef __cplusplus
extern "C" {
#endif
-#define DEFAULT_ACCELERATION_SIGMA2 SQUARE(FL_ACCEL_NOISE_LEVEL) /* [(m/s^2)^2] */
-#define DEFAULT_ROTATION_SIGMA2 SQUARE(1.0 / 256) /* [(rad/s)^2] */
-
-/** position on the Earth surface in Cartesian coordinates */
-typedef double position_vector[GEO_DIMENSION];
-
-typedef struct {
- double time;
- position_vector position;
-} fl_prediction;
-
typedef enum {
LOCATION_STATUS_NO_FIX = 0, /*/< No fix status. */
LOCATION_STATUS_2D_FIX, /*/< 2D fix status (latitude/longitude/speed/direction). */
bool enable_static_calibration;
double last_notification_time;
TimeOffset timestamp_offset;
- TangentFrame __te; /* tangent frame map */
+ WgsConversion wgs_conversion;
Orientation orientation;
- LocationFilter kalf; /* Kalman filter */
- LocationStatus fix_status; /* pass-through */
+ LocationFilter location_filter;
+ LocationStatus fix_status;
GyroscopeFilter gyro_bias;
PeaceDetector acc_peace_detector;
PeaceDetector gyro_peace_detector;
CalibrationFilter calibration_filter;
+ HeadingRateCalculator hr_calculator;
} 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]. */
/**
* @brief Initialization of the engine.
- * @param[in] on_motion_state_changed
- * Callback to be invoked when the detected state of motion changes. This argument is optional, and can be NULL.
*/
-extern void fusion_engine_init(FusionEngine* self,
- MotionDetectorStateChangeListener on_motion_state_changed, double utcOffset);
+extern void fusion_engine_init(FusionEngine* self, double utcOffset);
/** Cleanup of the singleton unit (static destructor). This corresponds to service unload. */
-extern void fusion_engine_exit(FusionEngine* self);
+extern void fusion_engine_destroy(FusionEngine* self);
/**
* @brief Start processing the location and sensory inputs, and sending back notifications.
*/
extern void fusion_engine_reset(FusionEngine* self);
-/*******************************************************************************
- * Process 2D (horizontal) position without change of the altitude or speed (WPS).
- * Kalman merging is carried out in tangent frame, and the origin is moved
- * after each measurement so that the predictions always start at (0, 0, R + h).
- * - Clocks synchronization
- * - Kalman data blending
- * - Repositioning of the tangent frame
- *
- * @param[in] time Internal time.
+/**
+ * @brief Process 2D (horizontal) position without change of the altitude or speed (WPS).
+ * After Kalman merging the origin of local frame is moved so that the current local position estimate is (0, 0).
+ * @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].
*/
extern bool fusion_engine_position_2d_event(FusionEngine* self, double time, double latitude, double longitude, double sigma);
-/*******************************************************************************
- * Process 3D position along with 3D velocity (GPS).
- * Kalman merging is carried out in tangent frame, and the origin is moved after each measurement
- * so that the predictions always start at (0, 0, R + h).
- * - Clocks synchronization
- * - Kalman data blending
- * - Repositioning of the tangent frame
+/**
+ * Used by IpsDevel for development.
+ * @brief Process 2D (horizontal) position without change of the altitude or speed.
+ * @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].
+ */
+extern bool fusion_engine_local_position_2d_event(FusionEngine* self, double time, double x, double y, double sigma);
+
+/**
+ * @brief Process 3D position along with 3D velocity (GPS).
+ * After Kalman merging the origin of local frame is moved so that the current local position estimate is (0, 0).
* If FL_KALMAN_ANGULAR_VELOCITY is enabled the angular velocity is derived
* from consecutive measurements of linear velocities and merged with gyroscope readings.
- *
- * @param[in] time Internal time.
+ * @param[in] time Internal time in [s].
* @param[in] latitude Latitude in degrees.
* @param[in] longitude Longitude in degrees.
* @param[in] altitude Altitude in [m].
* @param[in] sigma_of_climb Standard deviation of vertical speed in [m/s].
*/
extern bool fusion_engine_position_3d_event(FusionEngine* self, double time,
- double latitude, double longitude, double altitude,
- double hor_accuracy, double ver_accuracy,
+ double 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);
+
+/**
+ * Used by IpsDevel for development.
+ * @brief Process 3D position along with 3D velocity.
+ * @param[in] time Internal time in [s].
+ * @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].
+ */
+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);
/**
* @brief Process speed measurement events.
- * @param[in] time Internal time.
+ * @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].
*/
extern bool fusion_engine_speed_event(FusionEngine* self, double time, double speed, double sigma_of_speed);
/**
- * @brief Process accelerometer events. The time offset is corrected if necessary.
- * - Clocks synchronization
- * - Device-to-tangent frame orientation
- * - Prediction
+ * @brief Process pedometer events. The time offset is corrected if necessary.
* @param[in] time Sensory time-stamp
- * @param[in] x X coordinate of acceleration.
- * @param[in] y Y coordinate of acceleration.
- * @param[in] z Z coordinate of acceleration.
+ * @param[in] speed Speed in [m/s].
+ * @param[in] sigma_of_speed Speed deviation 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.
- * - Clocks synchronization
- * - Device-to-tangent frame orientation
- * - Prediction
- * @param[in] time Sensory time-stamp
- * @param[in] x X coordinate of angular rotation velocity vector.
- * @param[in] y Y coordinate of angular rotation velocity vector.
- * @param[in] z Z coordinate of angular rotation velocity vector.
+ * @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].
*/
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] heading Heading in [rad] anticlockwise.
+ */
+extern bool fusion_engine_heading_event(FusionEngine* self,
+ double time, double heading);
+
+/**
+ * @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.
+ */
+extern bool fusion_engine_heading_rate_event(FusionEngine* self,
+ double time, double heading_rate, double heading_rate_deviation);
+
+/**
+ * @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.
+ */
+bool fusion_engine_speed_heading_rate_event(FusionEngine* self, double time,
+ double speed, double speed_deviation, double heading_rate, double heading_rate_deviation);
+
/**
* @brief Retrieve the last processed location, typically during the on_location_changed notification.
* @param[out] location Current WGS location
*/
extern void fusion_engine_get_wgs_location(FusionEngine* self, WgsLocation* location);
+/**
+ * Used by IpsDevel for development.
+ * @brief Retrieve the last processed position in local coordinates, typically during the on_location_changed notification.
+ * @param[out] location Current local position
+ */
+extern void fusion_engine_get_local_position(FusionEngine* self, WgsLocation* location);
+
/**
* For ips_devel test
* @brief Retrieve current orientation.
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file GravityEstimator.h
- * @brief Estimator of the gravity value used for accelerometer scale calibration.
- */
-
-#pragma once
-#ifndef __GRAVITY_ESTIMATOR_H__
-#define __GRAVITY_ESTIMATOR_H__
-
-#include "MovingAverageFilters.h"
-#include "parameters.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-typedef MovingAverage1dFilter GravityEstimator;
-
-#define gravity_estimator_init(self) moving_average_1d_filter_init(self, (1 << FL_GRES_AEMA_PLATEAU_BITS))
-
-#define gravity_estimator_exit(self) moving_average_1d_filter_exit(self)
-
-/**
- * @brief Bring the internal state back to initial one. This function is used for repetitive testing and module soft restarts.
- */
-#define gravity_estimator_reset(self) moving_average_1d_filter_reset(self)
-
-/**
- * @brief Processing of a single data sample.
- * @param[in] g2 Modulus squared of the measured acceleration projected onto z-axis.
- * @return[double] Estimated value of the Earth gravity.
- */
-#define gravity_estimator_process(self, g2) moving_average_1d_filter_process(self, g2)
-
-/**
- * @brief Bring the filter to a stationary state (EMA mode, input = output). This method is used for testing.
- * @param[in] g2 Modulus squared value of Earth gravity measured by the accelerometer.
- */
-#define gravity_estimator_set_estimate(self, g2) moving_average_1d_filter_set_estimate(self, g2)
-
-/**
- * @brief Fetch the current gravity estimate.
- * @param[in] self Object pointer
- * @return[double] Output value after last processed sample.
- */
-
-/** Retrieve current estimate */
-#define gravity_estimator_get_estimate(self) moving_average_1d_filter_get_estimate(self)
-
-#define gravity_estimator_get_update_rate(self) moving_average_get_update_rate(&(self)->base)
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __GRAVITY_ESTIMATOR_H__ */
* limitations under the License.
*/
-#if !defined(LOG_THRESHOLD)
- /* custom logging threshold - keep undefined on the repo */
- /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */
-#endif
-
#include "GyroscopeFilter.h"
-#include "GravityEstimator.h"
-#include "math-ext.h"
-#include "debug_util.h"
-
-#define _USE_MATH_DEFINES
-#include <math.h>
+#include "parameters.h"
#define GYRO_PLATEAU_EVIDENCE (1 << FL_GYRO_AEMA_PLATEAU_BITS)
#define GYRO_SCALE2_I (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_I)))
void gyroscope_filter_init(GyroscopeFilter* self)
{
- LOG_FUSED_DEV(DBG_LOW, INDENT("()"));
gyroscope_filter_reset(self);
- LOG_FUSED_DEV(DBG_LOW, UNINDENT("()"));
-}
-
-void gyroscope_filter_exit(GyroscopeFilter* self)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
}
void gyroscope_filter_reset(GyroscopeFilter* self)
{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
waema3d_estimator_reset(self, (SQUARE(FL_GYRO_SPIN_SIGMA_I / FL_GYRO_SPIN_SIGMA_F) - 1) / SQUARE(GYRO_PLATEAU_EVIDENCE), GYRO_PLATEAU_EVIDENCE, GYRO_SCALE2_I);
}
const_Vector_3d_ref gyroscope_filter_process(GyroscopeFilter* self, double time, const_Vector_3d_ref wm)
{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs)"), time);
return waema3d_estimator_filter(self, wm);
}
extern "C" {
#endif
-
typedef Waema3dEstimator GyroscopeFilter;
-
void gyroscope_filter_init(GyroscopeFilter* self);
-void gyroscope_filter_exit(GyroscopeFilter* self);
+
void gyroscope_filter_reset(GyroscopeFilter* self);
/**
--- /dev/null
+/*
+ * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "HeadingRateCalculator.h"
+#include "math-ext.h"
+#include "Time.h"
+
+void hr_calculator_init(HeadingRateCalculator* self)
+{
+ self->report_rate = 0.3;
+ self->heading_rate_deviation = 5 * M_PI / 180;
+ hr_calculator_reset(self);
+}
+
+void hr_calculator_set_report_rate(HeadingRateCalculator* self, double rate)
+{
+ self->report_rate = rate;
+}
+
+void hr_calculator_set_heading_rate_deviation(HeadingRateCalculator* self, double deviation)
+{
+ self->heading_rate_deviation = deviation;
+}
+
+bool hr_calculator_new_heading(HeadingRateCalculator* self, double timestamp, double heading)
+{
+ 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) {
+ return false;
+ }
+ const double headingChange = correct_angle(heading - self->heading);
+ self->heading = heading;
+ const double dt = timestamp - self->timestamp;
+ self->timestamp = timestamp;
+ self->heading_rate = headingChange / dt;
+ return true;
+}
+
+double hr_calculator_get_timestamp(HeadingRateCalculator* self)
+{
+ return self->timestamp;
+}
+
+double hr_calculator_get_heading(HeadingRateCalculator* self)
+{
+ return self->heading;
+}
+
+double hr_calculator_get_heading_rate(HeadingRateCalculator* self)
+{
+ return self->heading_rate;
+}
+
+double hr_calculator_get_heading_rate_deviation(HeadingRateCalculator* self)
+{
+ return self->heading_rate_deviation;
+}
+
+void hr_calculator_reset(HeadingRateCalculator* self)
+{
+ SET_UNKNOWN_TIME(self->timestamp);
+ self->heading = 0;
+ self->heading_rate = 0;
+}
--- /dev/null
+/*
+ * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file HeadingRateCalculator.h
+ * @brief Calculates heading rate from heading.
+ */
+
+#pragma once
+#ifndef __HR_CALCULATOR_H__
+#define __HR_CALCULATOR_H__
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+ double report_rate; /* Minimum rate heading rate events should be reported in [s]. */
+ double heading_rate_deviation; /* Heading rate deviation in [rad/s]. */
+ double timestamp; /* Timestamp of last heading report in [s]. */
+ double heading; /* Last heading reported in [rad] anti-clockwise. */
+ double heading_rate; /* Last heading rate reported in [rad/s]. */
+} HeadingRateCalculator;
+
+void hr_calculator_init(HeadingRateCalculator* self);
+
+void hr_calculator_set_report_rate(HeadingRateCalculator* self, double rate);
+
+void hr_calculator_set_heading_rate_deviation(HeadingRateCalculator* self, double deviation);
+
+bool hr_calculator_new_heading(HeadingRateCalculator* self, double timestamp, double heading);
+
+double hr_calculator_get_timestamp(HeadingRateCalculator* self);
+
+double hr_calculator_get_heading(HeadingRateCalculator* self);
+
+double hr_calculator_get_heading_rate(HeadingRateCalculator* self);
+
+double hr_calculator_get_heading_rate_deviation(HeadingRateCalculator* self);
+
+void hr_calculator_reset(HeadingRateCalculator* self);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __HR_CALCULATOR_H__ */
--- /dev/null
+/*
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "KalmanFilter.h"
+
+#include <stdlib.h>
+
+void 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);
+}
+
+void kalman_filter_destroy(KalmanFilter *self)
+{
+ if (self->stateVector != NULL) {
+ free(self->stateVector);
+ self->stateVector = NULL;
+ }
+ if (self->mCovariance != NULL) {
+ matrix_destroy(self->mCovariance);
+ self->mCovariance = NULL;
+ }
+}
+
+void kalman_filter_set_state(KalmanFilter *self, const double* state, const void* covariance)
+{
+ unsigned int i;
+ for (i = self->mStateVectorSize; i ; ) {
+ i--;
+ self->stateVector[i] = state[i];
+ }
+ matrix_set_from_values(self->mCovariance, covariance);
+}
+
+double kalman_filter_get_variable_value(const KalmanFilter *self, const unsigned int index)
+{
+ return self->stateVector[index];
+}
+
+void kalman_filter_set_variable_value(KalmanFilter *self, const unsigned int index, double value)
+{
+ self->stateVector[index] = value;
+}
+
+double kalman_filter_get_variable_variance(const KalmanFilter *self, const unsigned int index)
+{
+ return matrix_get(self->mCovariance, index, index);
+}
+
+/**
+ * Makes state prediction. For current state vector V and covariance matrix C the following is performed
+ * V := stateTransitionFunction(V)
+ * C := stateTransitionMatrix * C * stateTransitionMatrix^T + noiseMatrix
+ */
+void kalman_filter_predict(KalmanFilter *self,
+ kalman_filter_state_transition_function stateTransitionFunction,
+ const Matrix *stateTransitionMatrix, const Matrix *noiseMatrix)
+{
+ double newStateVector[self->mStateVectorSize];
+ stateTransitionFunction(self->stateVector, newStateVector);
+ Matrix *m1 = matrix_multiply_by_matrix(stateTransitionMatrix, self->mCovariance);
+ Matrix *m2 = matrix_transpose(stateTransitionMatrix);
+ Matrix *m3 = matrix_multiply_by_matrix(m1, m2);
+ Matrix *m4 = matrix_add_matrix(m3, noiseMatrix);
+
+ kalman_filter_set_state(self, newStateVector, m4->mData);
+
+ matrix_destroy(m1);
+ matrix_destroy(m2);
+ matrix_destroy(m3);
+ matrix_destroy(m4);
+}
+
+/**
+ * Makes state correction. For current state vector V and covariance matrix C the following is performed
+ * S := measurementMatrix * C * measurementMatrix^T + noiseVector
+ * K := C * measurementMatrix^T * S^-1
+ * V += K * innovation
+ * C -= K * measurementMatrix * C
+ */
+void kalman_filter_correct(KalmanFilter *self, const Matrix *measurementMatrix, const double *innovation, const double *noiseVector)
+{
+ Matrix *mc = matrix_multiply_by_matrix(measurementMatrix, self->mCovariance);
+ Matrix *mmt = matrix_transpose(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_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);
+
+ 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);
+ matrix_destroy(mc);
+}
--- /dev/null
+/*
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file KalmanFilter.h
+ * @brief Implements Kalman filter for multidimensional state.
+ */
+#pragma once
+#ifndef __KALMAN_FILTER_H__
+#define __KALMAN_FILTER_H__
+
+#include "Matrix.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+ unsigned int mStateVectorSize; /** Size of state vector. */
+ double *stateVector; /** State vector. */
+ Matrix* mCovariance; /** State covariance matrix. */
+} KalmanFilter;
+
+typedef void (*kalman_filter_state_transition_function)(const double* in, double* out);
+
+/**
+ * @brief Initialize KalmanFilter.
+ * @param stateVectorSize Size of state vector.
+ */
+extern void kalman_filter_init(KalmanFilter *self, const unsigned int stateVectorSize);
+
+/**
+ * @brief Deinitialize/destroy KalmanFilter.
+ */
+extern void kalman_filter_destroy(KalmanFilter *self);
+
+/**
+ * @brief Sets new state vector and covariance matrix with provided values.
+ * Called every time when new state is calculated.
+ * @param state State vector. Must have size declared in constructor.
+ * @param covariance Covariance matrix. Must be square with size declared in constructor.
+ */
+extern void kalman_filter_set_state(KalmanFilter *self, const double* state, const void* covariance);
+
+/**
+ * @brief Returns estimated value of state variable.
+ * @param index Index of variable in state vector.
+ * @return Estimated value of state variable.
+ */
+extern double kalman_filter_get_variable_value(const KalmanFilter *self, const unsigned int index);
+
+/**
+ * @brief Sets new value of state variable.
+ * @param index Index of variable in state vector.
+ * @param value New value of state variable.
+ */
+extern void kalman_filter_set_variable_value(KalmanFilter *self, const unsigned int index, double value);
+
+/**
+ * @brief Returns estimated variance of state variable.
+ * @param index Index of variable in state vector.
+ * @return Estimated variance of state variable.
+ */
+extern double kalman_filter_get_variable_variance(const KalmanFilter *self, const unsigned int index);
+
+/**
+ * @brief Makes state prediction. For current state vector V and covariance matrix C the following is performed
+ * V := stateTransitionFunction(V)
+ * C := stateTransitionMatrix * C * stateTransitionMatrix^T + noiseMatrix
+ * @param stateTransitionFunction Function representing state transition from current state to next one.
+ * @param stateTransitionMatrix Matrix representing state transition from current state to next one.
+ * Jacobian of stateTransitionFunction in current state.
+ * @param noiseMatrix Represents noise of transition.
+ */
+extern void kalman_filter_predict(KalmanFilter *self,
+ kalman_filter_state_transition_function stateTransitionFunction,
+ const Matrix *stateTransitionMatrix, const Matrix *noiseMatrix);
+
+/**
+ * @brief Makes state correction. For current state vector V and covariance matrix C the following is performed
+ * S := measurementMatrix * C * measurementMatrix^T + noiseVector
+ * K := C * measurementMatrix^T * S^-1
+ * V += K * innovation
+ * C -= K * measurementMatrix * C
+ * @param measurementMatrix Measurement matrix.
+ * @param innovation Innovation vector (difference between measured values and expected in current state).
+ * @param noiseVector Represents noise of measurement.
+ */
+extern void kalman_filter_correct(KalmanFilter *self, const Matrix *measurementMatrix,
+ const double *innovation, const double *noiseVector);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __KALMAN_FILTER_H__ */
* See the License for the specific language governing permissions and
* limitations under the License.
*/
-
#include "LocationFilter.h"
-#include "Matrix.h"
-#include "Conversions.h"
-#include "debug_util.h"
-
-#include <memory.h>
-
-#define TIME_FORMAT "%.9fs"
-
-#define NEAR_ZERO 1.0e-200
+#include "Time.h"
-static bool location_filter_update_heading(_fl_kalf_state_m* self)
+/**
+ * Convert time in seconds to location time-stamp. The necessary up-rounding is half the time-stamp unit.
+ * @param[in] timestamp Time-stamp received from the location source (GPS, WPS).
+ * @return Corresponding value in seconds
+ */
+static Timestamp seconds_to_timestamp(double time)
{
- const double vx = self->v[X];
- const double vy = self->v[Y];
- if (fabs(vx) > 0.001 || fabs(vy) > 0.001) {
- self->heading = atan2(vy, vx);
- return true;
- }
- return false;
+ return IS_KNOWN_TIME(time) ? (Timestamp)(time * 1e9 + 0.5) : UNKNOWN_TIMESTAMP;
}
-void location_filter_init(LocationFilter* self, double x, double y, double z, double var_p, double var_v, double var_a)
+void location_filter_init(LocationFilter* self)
{
- self->default_var_a = var_a;
- self->last_correction_time = FL_UNDEFINED_TIME;
- memset(&self->prior, 0, sizeof(self->prior));
- memset(&self->posterior, 0, sizeof(self->posterior));
- self->z_rot_speed = 0;
- self->prior.x.p[X] = self->posterior.x.p[X] = x;
- self->prior.x.p[Y] = self->posterior.x.p[Y] = y;
- self->prior.x.p[Z] = self->posterior.x.p[Z] = z;
- unsigned i;
- for (i = GEO_DIMENSION; i;) {
- --i;
- /* diagonal terms */
- self->prior.s2x[i][POS][POS] = self->posterior.s2x[i][POS][POS] = var_p;
- self->prior.s2x[i][VEL][VEL] = self->posterior.s2x[i][VEL][VEL] = var_v;
- self->prior.s2x[i][ACC][ACC] = self->posterior.s2x[i][ACC][ACC] = var_a;
- }
- location_filter_update_heading(&self->posterior.x);
- self->prior.x.heading = self->posterior.x.heading;
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+ position_fusion_1d_init(&self->fusion1D);
+ position_fusion_2d_init(&self->fusion2D);
+ location_filter_reset(self);
}
-/**
- * @brief Check for and correct the numerical errors which cause the covariance matrix
- * to cease being positive-definite. This operation has to be performed regularly,
- * or otherwise magic ensues, and every strange result becomes possible.
- * @param[in, out] src The state object to be rectified.
- * This is either the predicted (prior), or estimated (posterior) one.
- */
-static void __rectify_state(_fl_kalf_sigma_cube s2x)
+void location_filter_destroy(LocationFilter* self)
{
- unsigned i;
- for (i = GEO_DIMENSION; i--;) {
- Matrix_3d_ref s2xi = s2x[i];
- /* diagonal elements */
-#define CHECK(u) \
- if (s2xi[u][u] < 0) { \
- /*LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): src.s2x[%u][" #u "][" #u "] = %g < 0"), src, i, s2xi[u][u]);*/ \
- s2xi[u][u] = 0; \
- }
- CHECK(POS);
- CHECK(VEL);
- CHECK(ACC);
-#undef CHECK
-
- /* partial determinants */
- double uuvv, uvvu;
-#define CHECK(u, v) \
- uuvv = s2xi[u][u] * s2xi[v][v]; \
- uvvu = s2xi[u][v] * s2xi[v][u]; \
- if (uuvv < uvvu) { \
- /*LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(src=%p): det(src.s2x[%u], " #u ", " #v ") = %g < 0"), src, i, uuvv - uvvu);*/ \
- s2xi[u][v] = \
- s2xi[v][u] *= sqrt(uuvv / uvvu); \
- }
- CHECK(POS, VEL);
- CHECK(POS, ACC);
- CHECK(ACC, VEL);
-#undef CHECK
- }
+ position_fusion_1d_destroy(&self->fusion1D);
+ position_fusion_2d_destroy(&self->fusion2D);
}
-/**
- * @brief Copy of the state (3x3D position & covariance) object.
- * @param[out] dst Destination state object
- * @param[out] src Source state object
- */
-static void __copy_state(_fl_kalf_state *dst, _fl_kalf_state *src)
+void location_filter_reset(LocationFilter* self)
{
- __rectify_state(src->s2x);
- memcpy(dst, src, sizeof(*dst));
+ self->was_position = false;
+ position_fusion_2d_reset(&self->fusion2D);
}
-void location_filter_update_state(LocationFilter* self)
+void location_filter_get_position(const LocationFilter* self, Vector_3d_ref pos)
{
- memcpy(&self->prior.x, &self->posterior.x, sizeof(self->prior.x));
+ pos[X] = location_filter_get_x(self);
+ pos[Y] = location_filter_get_y(self);
+ pos[Z] = location_filter_get_z(self);
}
-/**
- * @brief Calculates prior from posterior, i.e. extrapolates position,
- * velocity, and the covariance-matrices up to the given time.
- * We assume rotation is at constant angular rate (z_rot_speed) and speed (norm of velocity) is also constant.
- * Therefore motion equations are:
- * Vx = vx cos(wt) - vy sin(wt)
- * Vy = vy cos(wt) + vx sin(wt)
- * Vz = vz
- * X = x + vx sin(wt)/w + vy(cos(wt) - 1)/w (by integrating)
- * Y = y + vy sin(wt)/w - vx(cos(wt) - 1)/w
- * Z = z + vz t
- * Ax = -vy w (differentiating at t=0)
- * Ay = vx w
- * Az = 0
- * And transition matrix is:
- * | 1 0 0 sin(wt)/w (cos(wt) - 1)/w 0 0 0 0|
- * | 0 1 0 -(cos(wt) - 1)/w sin(wt)/w 0 0 0 0|
- * | 0 0 1 0 0 t 0 0 0|
- * | 0 0 0 cos(wt) -sin(wt) 0 0 0 0|
- * | 0 0 0 sin(wt) -cos(wt) 0 0 0 0|
- * | 0 0 0 0 0 1 0 0 0|
- * | 0 0 0 0 -w 0 0 0 0|
- * | 0 0 0 w 0 0 0 0 0|
- * | 0 0 0 0 0 0 0 0 0|
- * so for small wt:
- * | 1 0 0 t wt^2/2 0 0 0 0|
- * | 0 1 0 -wt^2/2 t 0 0 0 0|
- * | 0 0 1 0 0 t 0 0 0|
- * | 0 0 0 1 - w^2t^2/2 -t 0 0 0 0|
- * | 0 0 0 t -1 + w^2t^2/2 0 0 0 0|
- * | 0 0 0 0 0 1 0 0 0|
- * | 0 0 0 0 -w 0 0 0 0|
- * | 0 0 0 w 0 0 0 0 0|
- * | 0 0 0 0 0 0 0 0 0|
- * We assume covariance matrix can be represented as
- * 3 independent covariance matrixes, one for each coordinate,
- * and assume transition matrix used for covariance update
- * for each coordinate is:
- * | 1 t t^2/2 |
- * | 0 1 t |
- * | 0 0 1 |
- * Process noise matrix ???
- *
- * @param[in] time Internal time
- */
-static void location_filter_predict(LocationFilter* self, double time)
+double location_filter_get_x(const LocationFilter* self)
{
- if (!KNOWN_TIME(self->last_correction_time)) {
- /* no prior data */
- return;
- }
- double dt = time - self->last_correction_time;
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT "), dt=" TIME_FORMAT), time, dt);
- if (dt <= 0) {
- return;
- }
- const _fl_kalf_state_m* post = &self->posterior.x;
- _fl_kalf_state_m* prior = &self->prior.x;
- double z_rot_speed = self->z_rot_speed;
- double rot = z_rot_speed * dt;
- if (fabs(rot) > NEAR_ZERO) {
- double cos_rot = cos(rot);
- double sin_rot = sin(rot);
- double inv_z_rot_speed = 1.0 / z_rot_speed;
- double cos_rot_inv = (cos_rot - 1) * inv_z_rot_speed;
- double sin_rot_inv = sin_rot * inv_z_rot_speed;
-
- prior->p[X] = post->p[X] + post->v[X] * sin_rot_inv + post->v[Y] * cos_rot_inv;
- prior->p[Y] = post->p[Y] + post->v[Y] * sin_rot_inv - post->v[X] * cos_rot_inv;
- prior->v[X] = post->v[X] * cos_rot - post->v[Y] * sin_rot;
- prior->v[Y] = post->v[Y] * cos_rot + post->v[X] * sin_rot;
- if (!location_filter_update_heading(prior)) {
- prior->heading = post->heading + rot;
- }
- } else {
- prior->p[X] = post->p[X] + dt * post->v[X];
- prior->p[Y] = post->p[Y] + dt * post->v[Y];
- prior->v[X] = post->v[X];
- prior->v[Y] = post->v[Y];
- prior->heading = post->heading;
- }
- prior->p[Z] = post->p[Z] + dt * post->v[Z];
- prior->v[Z] = post->v[Z];
-
- prior->a[X] = -prior->v[Y] * z_rot_speed;
- prior->a[Y] = prior->v[X] * z_rot_speed;
- prior->a[Z] = 0;
-
- const_fl_kalf_sigma_cube_ref post_s = self->posterior.s2x;
- _fl_kalf_sigma_cube_ref prior_s = self->prior.s2x;
- unsigned i;
- for (i = GEO_DIMENSION; i;) {
- --i;
-
- const_Matrix_3d_ref post_si = post_s[i];
- Matrix_3d_ref prior_si = prior_s[i];
-
- prior_si[ACC][ACC] = post_si[ACC][ACC];
- prior_si[ACC][VEL] = post_si[ACC][VEL] + post_si[ACC][ACC] * dt;
- prior_si[ACC][POS] = post_si[ACC][POS]
- + (post_si[ACC][VEL] + post_si[ACC][ACC] * dt / 2) * dt;
-
- prior_si[VEL][ACC] = prior_si[ACC][VEL]; /* by symmetry */
- prior_si[VEL][VEL] = post_si[VEL][VEL]
- + (post_si[VEL][ACC] * 2 + post_si[ACC][ACC] * dt) * dt;
- prior_si[VEL][POS] = post_si[VEL][POS]
- + (post_si[VEL][VEL] + post_si[POS][ACC]
- + (post_si[VEL][ACC] * 3
- + post_si[ACC][ACC] * dt) / 2 * dt) * dt;
-
- prior_si[POS][ACC] = prior_si[ACC][POS]; /* by symmetry */
- prior_si[POS][VEL] = prior_si[VEL][POS]; /* by symmetry */
- prior_si[POS][POS] = post_si[POS][POS]
- + (post_si[POS][VEL] * 2
- + (post_si[VEL][VEL] + post_si[ACC][POS]
- + (post_si[ACC][VEL]
- + post_si[ACC][ACC] * dt / 4) * dt)
- * dt) * dt;
- }
- /* process noise */
- prior_s[X][ACC][ACC] += fl_square(prior->a[X] - post->a[X]);
- prior_s[Y][ACC][ACC] += fl_square(prior->a[Y] - post->a[Y]);
- __rectify_state(prior_s);
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+ return position_fusion_2d_get_x(&self->fusion2D);
}
-/**
- * @brief Update of the 2D position
- * Observation matrix for each coordinate is:
- * | 1 0 0|
- *
- * @param[in] time Internal time
- * @param[in] pos Measured position
- * @param[in] pos_ac Measured position deviation (squared standard deviation)
- */
-void location_filter_kalman_p(LocationFilter* self, double time, const_Vector_3d_ref pos, const_Vector_3d_ref pos_ac)
+double location_filter_get_y(const LocationFilter* self)
{
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=%g, x0=%g, x1=%g, x2=%g, s0=%g, s1=%g, s2=%g)"),
- time, pos[0], pos[1], pos[2], pos_ac[0], pos_ac[1], pos_ac[2]);
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("input (t=%g, p0=%g, p1=%g, p2=%g, v0=%g, v1=%g, v2=%g)"),
- time, self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2], self->prior.x.v[0], self->prior.x.v[1], self->prior.x.v[2]);
-
- location_filter_predict(self, time);
- unsigned i;
- /* @note WPS-specific: restrict the update to horizontal position */
- for (i = PLANAR_DIMENSION; i;) {
- --i;
- const double s2mi = pos_ac[i];
- Matrix_3d_ref post_si = self->posterior.s2x[i];
- const_Matrix_3d_ref prior_si = self->prior.s2x[i];
- const_Vector_3d_ref s2pis = prior_si[POS];
- const double s2piss = s2pis[POS];
- const double innovation_covariance = s2mi + s2piss;
- if (innovation_covariance > 0) {
- const double inv_inn_cov = 1.0 / innovation_covariance;
- const double innovation = pos[i] - self->prior.x.p[i];
-
- /* reduced Joseph form in special 3x1 case: subtract scaled projection */
-#define SET_FIELD(a, b) post_si[a][b] = inv_inn_cov * ((prior_si[a][b] * s2piss - s2pis[a] * s2pis[b]) + prior_si[a][b] * s2mi)
-#define CPY_FIELD(a, b) post_si[a][b] = post_si[b][a]
- SET_FIELD(POS, POS);
- SET_FIELD(POS, VEL);
- SET_FIELD(POS, ACC);
- CPY_FIELD(VEL, POS);
- SET_FIELD(VEL, VEL);
- SET_FIELD(VEL, ACC);
- CPY_FIELD(ACC, POS);
- CPY_FIELD(ACC, VEL);
- SET_FIELD(ACC, ACC);
-#undef CPY_FIELD
-#undef SET_FIELD
-
- self->posterior.x.p[i] = self->prior.x.p[i] + innovation * inv_inn_cov * s2pis[POS];
- self->posterior.x.v[i] = self->prior.x.v[i] + innovation * inv_inn_cov * s2pis[VEL];
- self->posterior.x.a[i] = self->prior.x.a[i] + innovation * inv_inn_cov * s2pis[ACC];
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(i=%i, t=%g, prev=%g, dx=%g, inv_s2k=%g, s2pis[POS]=%g, cur=%g)"),
- i, time, self->prior.x.p[i], innovation, inv_inn_cov, s2piss, self->posterior.x.p[i]);
- }
- }
- /* the third coordinate is of `special care': we want to preserve it in the tangent frame, along with its sigma */
- self->posterior.x.p[Z] = pos[Z];
- location_filter_update_heading(&self->posterior.x);
- __copy_state(&self->prior, &self->posterior);
- self->last_correction_time = time;
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+ return position_fusion_2d_get_y(&self->fusion2D);
}
-/**
- * @brief Update of the 3D position & velocity
- * Observation matrix for each coordinate is:
- * | 1 0 0|
- * | 0 1 0|
- *
- * @param[in] time Internal time
- * @param[in] pm Measured position
- * @param[in] vm Measured velocity
- * @param[in] s2pm Measured position deviation (squared standard deviation)
- * @param[in] s2vm Measured velocity deviation (squared standard deviation)
- */
-void location_filter_kalman_pv(LocationFilter* self, double time,
- const_Vector_3d_ref pm, const_Vector_3d_ref vm, const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm)
+double location_filter_get_z(const LocationFilter* self)
{
- location_filter_predict(self, time);
- unsigned i;
- for (i = GEO_DIMENSION; i;) {
- --i;
+ return position_fusion_1d_get_position(&self->fusion1D);
+}
- /* shortcuts */
- const_Matrix_3d_ref prior_cov = self->prior.s2x[i];
+double location_filter_get_horizontal_speed(const LocationFilter* self)
+{
+ return position_fusion_2d_get_speed(&self->fusion2D);
+}
- /* Sk = Sp + Sm */
- Matrix_2d innovation_covariance;
- innovation_covariance[POS][POS] = prior_cov[POS][POS] + s2pm[i];
- innovation_covariance[POS][VEL] = /* == prior_cov[POS][VEL] */
- innovation_covariance[VEL][POS] = prior_cov[VEL][POS];
- innovation_covariance[VEL][VEL] = prior_cov[VEL][VEL] + s2vm[i];
+double location_filter_get_vertical_speed(const LocationFilter* self)
+{
+ return position_fusion_1d_get_speed(&self->fusion1D);
+}
- Matrix_2d inv_inn_cov;
- if (matrix_2d_invert(innovation_covariance, inv_inn_cov)) {
- Matrix_2d k0;
- matrix_3d_multiply_matrix_2d(prior_cov, inv_inn_cov, k0); /* K = Sp.Si */
- /* 1 - K = Sm.Si, use positive form */
- Matrix_2d k1;
- k1[POS][POS] = s2pm[i] * inv_inn_cov[POS][POS];
- k1[POS][VEL] = s2pm[i] * inv_inn_cov[POS][VEL];
- k1[VEL][POS] = s2vm[i] * inv_inn_cov[VEL][POS];
- k1[VEL][VEL] = s2vm[i] * inv_inn_cov[VEL][VEL];
- /* state update */
- self->posterior.x.p[i] = k0[POS][POS] * pm[i]
- + k0[POS][VEL] * vm[i]
- + k1[POS][POS] * self->prior.x.p[i]
- + k1[POS][VEL] * self->prior.x.v[i];
+void location_filter_get_velocity(const LocationFilter* self, Vector_3d_ref vel)
+{
+ vel[X] = position_fusion_2d_get_speed_x(&self->fusion2D);
+ vel[Y] = position_fusion_2d_get_speed_y(&self->fusion2D);
+ vel[Z] = position_fusion_1d_get_speed(&self->fusion1D);
+}
- self->posterior.x.v[i] = k0[VEL][POS] * pm[i]
- + k0[VEL][VEL] * vm[i]
- + k1[VEL][POS] * self->prior.x.p[i]
- + k1[VEL][VEL] * self->prior.x.v[i];
+void location_filter_set_predicted_state(LocationFilter* self, const_Vector_3d_ref pos)
+{
+ position_fusion_2d_set_position(&self->fusion2D, pos[X], pos[Y]);
+ position_fusion_1d_set_position(&self->fusion1D, pos[Z]);
+}
- /* covariance update Se = K1,K1.Sp + K,K.Sm */
- Matrix_3d_ref s2ei = self->posterior.s2x[i];
+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_1d_update_position(&self->fusion1D, pm[Z], s2pm[Z]);
+ self->was_position = true;
+}
-#define SET_S2E(j, k) s2ei[j][k] = \
- k1[j][POS] * k1[k][POS] * prior_cov[POS][POS] + k0[j][POS] * k0[k][POS] * s2pm[i] \
- + k1[j][POS] * k1[k][VEL] * prior_cov[POS][VEL] \
- + k1[j][VEL] * k1[k][POS] * prior_cov[VEL][POS] \
- + k1[j][VEL] * k1[k][VEL] * prior_cov[VEL][VEL] + k0[j][VEL] * k0[k][VEL] * s2vm[i]
+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_1d_update_position(&self->fusion1D, pm[Z], pos_dev);
+ self->was_position = true;
+}
- SET_S2E(POS, POS);
- SET_S2E(POS, VEL);
- SET_S2E(VEL, POS);
- SET_S2E(VEL, VEL);
-#undef SET_S2E
- }
- /* upon arrival of new position-velocity pair (a maximally uncertain) the pair (p, v) decorrelates from a: */
- self->posterior.s2x[i][POS][ACC] = 0;
- self->posterior.s2x[i][VEL][ACC] = 0;
- self->posterior.s2x[i][ACC][POS] = 0;
- self->posterior.s2x[i][ACC][VEL] = 0;
- self->posterior.s2x[i][ACC][ACC] = self->prior.s2x[i][ACC][ACC];
- }
- location_filter_update_heading(&self->posterior.x);
- __copy_state(&self->prior, &self->posterior);
- self->last_correction_time = time;
+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)
+{
+ position_fusion_2d_update_position_speed(&self->fusion2D, seconds_to_timestamp(time),
+ pm[X], pm[Y], sqrt((s2pm[X] + s2pm[Y]) / 2),
+ sqrt(fl_square(vm[X]) + fl_square(vm[Y])), sqrt((s2vm[X] + s2vm[Y]) / 2));
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+ position_fusion_1d_update_position_speed(&self->fusion1D, pm[Z], s2pm[Z], s2vm[Z], s2vm[Z]);
+ self->was_position = true;
}
-/**
- * @brief Update of the 2D position using speed measurement
- * Observation matrix is:
- * | 0 vx/v 0 0 vy/v 0 0 0 0|
- * Where v = sqrt(vx^2 + vy^2)
- *
- * @param[in] time Internal time
- * @param[in] speed Measured horizontal speed
- * @param[in] sigma_of_speed Measured horizontal speed deviation (squared standard deviation)
- */
void location_filter_update_speed(LocationFilter* self, double time, double speed, double sigma_of_speed)
{
- location_filter_predict(self, time);
- const double vx = self->prior.x.v[X];
- const double vy = self->prior.x.v[Y];
- const double v = sqrt(vx * vx + vy * vy);
- double ch;
- double sh;
- if (v != 0) {
- ch = vx/v;
- sh = vy/v;
- } else {
- const double heading = self->prior.x.heading;
- ch = cos(heading);
- sh = sin(heading);
+ if (self->was_position) {
+ position_fusion_2d_update_speed(&self->fusion2D, seconds_to_timestamp(time), speed, sigma_of_speed);
}
- const double ch2 = ch * ch;
- const double sh2 = sh * sh;
- const double innovation_covariance =
- ch2 * self->prior.s2x[X][VEL][VEL] + sh2 * self->prior.s2x[Y][VEL][VEL] + SQUARE(sigma_of_speed);
- const double innovation = speed - v;
- /* Update state estimate */
- const double vxk = ch * innovation / innovation_covariance;
- self->posterior.x.p[X] = self->prior.x.p[X] + self->prior.s2x[X][POS][VEL] * vxk;
- self->posterior.x.v[X] = self->prior.x.v[X] + self->prior.s2x[X][VEL][VEL] * vxk;
- self->posterior.x.a[X] = self->prior.x.a[X] + self->prior.s2x[X][ACC][VEL] * vxk;
-
- const double vyk = sh * innovation / innovation_covariance;
- self->posterior.x.p[Y] = self->prior.x.p[Y] + self->prior.s2x[Y][POS][VEL] * vyk;
- self->posterior.x.v[Y] = self->prior.x.v[Y] + self->prior.s2x[Y][VEL][VEL] * vyk;
- self->posterior.x.a[Y] = self->prior.x.a[Y] + self->prior.s2x[Y][ACC][VEL] * vyk;
-
- self->posterior.x.p[Z] = self->prior.x.p[Z];
- self->posterior.x.v[Z] = self->prior.x.v[Z];
- self->posterior.x.a[Z] = self->prior.x.a[Z];
+}
- /* Update covariance estimate */
- Vector_2d vv;
- vv[0] = ch2 / innovation_covariance;
- vv[1] = sh2 / innovation_covariance;
- unsigned i;
- for (i = PLANAR_DIMENSION; i;) {
- --i;
- const_Matrix_3d_ref prior_cov = self->prior.s2x[i];
- Matrix_3d_ref post_cov = self->posterior.s2x[i];
- unsigned s1;
- for (s1 = STATE_DIMENSION; s1;) {
- s1--;
- unsigned s2;
- for (s2 = s1 + 1; s2;) {
- s2--;
- post_cov[s2][s1] = post_cov[s1][s2] =
- prior_cov[s1][s2] - prior_cov[s1][VEL] * prior_cov[VEL][s2] * vv[i];
- }
- }
+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);
}
- matrix_3d_copy(self->posterior.s2x[Z], self->prior.s2x[Z]);
- location_filter_update_heading(&self->posterior.x);
- __copy_state(&self->prior, &self->posterior);
- self->last_correction_time = time;
}
-void location_filter_clear_acc(LocationFilter* self, double time)
+void location_filter_update_speed_heading_rate(LocationFilter* self,
+ double time, double speed, double speed_deviation,
+ double heading_rate, double heading_rate_deviation)
{
- unsigned i;
- for (i = GEO_DIMENSION; i--;) {
- self->posterior.x.a[i] = 0;
- self->posterior.s2x[i][ACC][ACC] = self->default_var_a;
+ if (self->was_position) {
+ position_fusion_2d_update_speed_angular_speed(&self->fusion2D, seconds_to_timestamp(time),
+ speed, speed_deviation, heading_rate, heading_rate_deviation);
}
-
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
-
- location_filter_predict(self, time);
}
-void location_filter_update_acc(LocationFilter* self, double time)
+double location_filter_get_heading(LocationFilter* self)
{
- location_filter_predict(self, time);
- self->posterior.s2x[X][ACC][ACC] = fl_square(self->prior.x.a[X] - self->posterior.x.a[X]);
- self->posterior.s2x[Y][ACC][ACC] = fl_square(self->prior.x.a[Y] - self->posterior.x.a[Y]);
- __copy_state(&self->posterior, &self->prior);
- self->last_correction_time = time;
-
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(p0=%g, p1=%g, p2=%g)"), self->prior.x.p[0], self->prior.x.p[1], self->prior.x.p[2]);
+ return position_fusion_2d_get_direction(&self->fusion2D);
}
* @file LocationFilter.h
* @brief Implementation of a 4D (2D position + 2D velocity) Kalman filter
*/
-
#pragma once
#ifndef __LOCATION_FILTER_H_
#define __LOCATION_FILTER_H_
-#include "parameters.h"
+#include "PositionFusion2D.h"
+#include "PositionFusion1D.h"
#include "Vector.h"
-#include "Matrix.h"
+
+#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif
-typedef enum {
- POS, /* position [m] */
- VEL, /* velocity [m/s] */
- ACC, /* acceleration [m/s^2] */
- STATE_DIMENSION
-} _fl_kalf_state_component;
-
-typedef struct {
- Vector_3d p; /* Position */
- Vector_3d v; /* Velocity */
- Vector_3d a; /* Acceleration */
- double heading; /* Heading */
-} _fl_kalf_state_m;
-
-typedef Matrix_3d _fl_kalf_sigma_cube[GEO_DIMENSION];
-typedef Matrix_3d(*_fl_kalf_sigma_cube_ref);
-typedef const Matrix_3d* const_fl_kalf_sigma_cube_ref;
-
-typedef struct {
- _fl_kalf_state_m x; /* state vector */
- _fl_kalf_sigma_cube s2x; /* covariance matrix */
-} _fl_kalf_state;
-
typedef struct {
- double default_var_a;
- _fl_kalf_state prior; /* predicted (a priori) state */
- _fl_kalf_state posterior; /* corrected (a posteriori) state */
- double last_correction_time; /* update of posterior */
- double z_rot_speed; /* reference angular z-velocity (horizontal plane) */
+ PositionFusion2D fusion2D; /* 2D fusion of speed, heading and position. */
+ PositionFusion1D fusion1D; /* 1D fusion of height and climbing speed. */
+ bool was_position; /* true if some position was delivered. */
} LocationFilter;
/**
* @brief Initialization of the filter (constructor).
- * @param[in] x X coordinate of the initial position.
- * @param[in] y Y coordinate of the initial position.
- * @param[in] z Z coordinate of the initial position.
- * @param[in] var_p Initial variance of position.
- * @param[in] var_v Initial variance of speed.
- * @param[in] var_a Initial variance of acceleration.
*/
-extern void location_filter_init(LocationFilter* self,
- double x, double y, double z, double var_p, double var_v, double var_a);
+extern void location_filter_init(LocationFilter* self);
+
+extern void location_filter_destroy(LocationFilter* self);
+
+extern void location_filter_reset();
/**
- * @brief Sets z rotation speed.
- * @param[in] z_rot_speed Z rotation speed in [rad/sec].
+ * @brief Kalman blending of heading rate.
+ * @param[in] heading_rate Heading change rate in [rad/s].
+ * @param[in] heading_rate_deviation Heading change rate deviation in [rad/s].
*/
-#define location_filter_set_z_rot_speed(self, _z_rot_speed) ((self)->z_rot_speed = (_z_rot_speed))
+extern void location_filter_update_heading_rate(LocationFilter* self,
+ double time, double heading_rate, double heading_rate_deviation);
-extern void location_filter_update_state(LocationFilter* self);
+/**
+ * @brief Kalman blending of speed and heading rate.
+ * @param[in] speed Speed in [m/s].
+ * @param[in] speed_deviation Speed deviation in [m/s].
+ * @param[in] heading_rate Heading change rate in [rad/s].
+ * @param[in] heading_rate_deviation Heading change rate deviation in [rad/s].
+ */
+extern void location_filter_update_speed_heading_rate(LocationFilter* self,
+ double time, double speed, double speed_deviation,
+ double heading_rate, double heading_rate_deviation);
/**
* @brief Planar (2D) Kalman blending of position.
* @param[in] pm Measured position.
* @param[in] s2pm Position variances.
*/
-extern void location_filter_kalman_p(LocationFilter* self,
+extern void location_filter_update_position(LocationFilter* self,
double time, const_Vector_3d_ref pm, const_Vector_3d_ref s2pm);
/**
* @param[in] s2pm Position variances.
* @param[in] s2vm Velocity variances.
*/
-extern void location_filter_kalman_pv(LocationFilter* self, double time,
- const_Vector_3d_ref pm, const_Vector_3d_ref vm, const_Vector_3d_ref s2pm, const_Vector_3d_ref s2vm);
+extern void location_filter_update_position_speed(LocationFilter* self,
+ double time, const_Vector_3d_ref pm, double pos_dev, double speed, double speed_dev);
+
+/**
+ * @brief Spatial (3D) Kalman blending of position and velocity.
+ * @param[in] time Internal time.
+ * @param[in] pm Measured position.
+ * @param[in] vm Measured velocity.
+ * @param[in] s2pm Position variances.
+ * @param[in] s2vm Velocity variances.
+ */
+extern void location_filter_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);
/**
* @brief Kalman blending of speed.
*/
extern void location_filter_update_speed(LocationFilter* self, double time, double speed, double sigma_of_speed);
-extern void location_filter_clear_acc(LocationFilter* self, double time);
-extern void location_filter_update_acc(LocationFilter* self, double time);
+double location_filter_get_x(const LocationFilter* self);
+
+double location_filter_get_y(const LocationFilter* self);
+
+double location_filter_get_z(const LocationFilter* self);
+
+/** Returns current position estimate. */
+void location_filter_get_position(const LocationFilter* self, Vector_3d_ref pos);
+
+double location_filter_get_horizontal_speed(const LocationFilter* self);
+
+double location_filter_get_vertical_speed(const LocationFilter* self);
+
+/** Returns current velocity estimate. */
+void location_filter_get_velocity(const LocationFilter* self, Vector_3d_ref vel);
+
+extern void location_filter_set_predicted_state(LocationFilter* self, const_Vector_3d_ref pos);
-/** (Vector_3d_ref) Returns current position estimate */
-#define location_filter_get_position(self) ((self)->posterior.x.p)
+#define location_filter_get_hor_dev(self) (position_fusion_2d_get_position_deviation(&(self)->fusion2D))
-/** (Vector_3d_ref) Returns current velocity estimate */
-#define location_filter_get_velocity(self) ((self)->posterior.x.v)
+#define location_filter_get_ver_dev(self) (position_fusion_1d_get_position_deviation(&(self)->fusion1D))
-/** (const_fl_kalf_sigma_cube_ref) */
-#define location_filter_get_predicted_covariance(self) ((self)->prior.s2x)
+#define location_filter_get_speed_dev(self) (position_fusion_2d_get_speed_deviation(&(self)->fusion2D))
-/** (const_Vector_3d_ref) Returns predicted position estimate */
-#define location_filter_get_predicted_position(self) ((self)->prior.x.p)
+#define location_filter_get_climb_dev(self) (position_fusion_1d_get_speed_deviation(&(self)->fusion1D))
-/** (const_Vector_3d_ref) Returns predicted velocity estimate */
-#define location_filter_get_predicted_velocity(self) ((self)->prior.x.v)
+extern double location_filter_get_heading(LocationFilter* self);
#ifdef __cplusplus
}
/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
*/
#include "Matrix.h"
-#include "math-ext.h"
+#include "debug_util.h"
-void matrix_3d_transpose(Matrix_3d_ref self)
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+
+Matrix* matrix_create_empty(const unsigned int rows, const unsigned int columns)
{
- int i, j;
+ Matrix* self = malloc(sizeof(Matrix));
+ if (self != NULL) {
+ self->mRows = rows;
+ self->mColumns = columns;
+ self->mData = malloc(rows * columns * sizeof(double));
+ }
+ return self;
+}
- for (i = GEO_DIMENSION - 1; i >= 0; i--) {
- for (j = i - 1; j >= 0; j--) {
- double v = self[i][j];
- self[i][j] = self[j][i];
- self[j][i] = v;
- }
+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));
}
+ return self;
}
-void matrix_3d_multiply_matrix_3d(const_Matrix_3d_ref m1, const_Matrix_3d_ref m2, Matrix_3d_ref out)
+Matrix* matrix_create_from_copy(const Matrix* in)
{
- int i, j;
+ Matrix* self = matrix_create_from_values(in->mRows, in->mColumns, in->mData);
+ return self;
+}
- for (i = GEO_DIMENSION - 1; i >= 0; i--) {
- for (j = GEO_DIMENSION - 1; j >= 0; j--) {
- out[i][j] = m1[i][X] * m2[X][j] + m1[i][Y] * m2[Y][j] + m1[i][Z] * m2[Z][j];
+void matrix_destroy(Matrix* self)
+{
+ if (self != NULL) {
+ if (self->mData != NULL) {
+ free(self->mData);
}
+ free(self);
+ self = NULL;
}
}
-void matrix_3d_multiply_vector_3d(const_Matrix_3d_ref m, const_Vector_3d_ref v, Vector_3d_ref out)
+void matrix_set_from_values(Matrix* self, const void* in_values_pointer)
{
- out[X] = vector_3d_dot_product(m[X], v);
- out[Y] = vector_3d_dot_product(m[Y], v);
- out[Z] = vector_3d_dot_product(m[Z], v);
+ memcpy(self->mData, in_values_pointer, self->mRows * self->mColumns * sizeof(double));
}
-void vector_3d_multiply_matrix_3d(const_Vector_3d_ref v, const_Matrix_3d_ref m, Vector_3d_ref out)
+void matrix_set_from_copy(Matrix* self, const Matrix* in)
{
- out[X] = m[X][X] * v[X] + m[Y][X] * v[Y] + m[Z][X] * v[Z];
- out[Y] = m[X][Y] * v[X] + m[Y][Y] * v[Y] + m[Z][Y] * v[Z];
- out[Z] = m[X][Z] * v[X] + m[Y][Z] * v[Y] + m[Z][Z] * v[Z];
+ matrix_set_from_values(self, in->mData);
}
-void matrix_3d_from_vector_3d(const_Vector_3d_ref p, Matrix_3d_ref out,
- double* lat, double* lon, double* sin_lat, double* cos_lat)
+double matrix_get(const Matrix* self, const unsigned int row, const unsigned int column)
{
- double cos_rx, sin_rx;
- double cos_ry, sin_ry;
- double yz2 = fl_square(p[Y]) + fl_square(p[Z]);
- double r2s = sqrt(yz2 + fl_square(p[X]));
- if (r2s > 0) {
- double _r = 1.0 / r2s;
- double yz = sqrt(yz2);
-
- *lat = atan2(p[X], yz);
- *sin_lat = sin_rx = _r * p[X];
- *cos_lat = cos_rx = _r * yz;
- if (yz > 0) {
- double _yz = 1.0 / yz;
+ double (*self_values)[self->mColumns] = self->mData;
+ return self_values[row][column];
+}
- *lon = atan2(p[Y], p[Z]);
- sin_ry = _yz * p[Y];
- cos_ry = _yz * p[Z];
- } else {
- *lon = 0;
- sin_ry = 0;
- cos_ry = 1;
+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;
+ 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];
+ }
+ result_values[row][col] = v;
}
- } else {
- *lat = 0;
- *lon = 0;
- *sin_lat = sin_rx = 0;
- *cos_lat = cos_rx = 1;
- sin_ry = 0;
- cos_ry = 1;
}
- out[X][X] = cos_rx;
- out[Y][X] = 0;
- out[Z][X] = sin_rx;
-
- out[X][Y] = -sin_ry * sin_rx;
- out[Y][Y] = cos_ry;
- out[Z][Y] = sin_ry * cos_rx;
-
- out[X][Z] = -cos_ry * sin_rx;
- out[Y][Z] = -sin_ry;
- out[Z][Z] = cos_ry * cos_rx;
+ return result;
}
-void matrix_2d_multiply_matrix_2d(const_Matrix_2d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out)
+double* matrix_multiply_by_vector(const Matrix* self, const double* vector, const unsigned int vector_length)
{
- int i, j;
-
- for (i = PLANAR_DIMENSION - 1; i >= 0; i--) {
- for (j = PLANAR_DIMENSION - 1; j >= 0; j--) {
- out[i][j] = m1[i][PLANAR_X] * m2[PLANAR_X][j] + m1[i][PLANAR_Y] * m2[PLANAR_Y][j];
+ const double (*self_values)[self->mColumns] = self->mData;
+ double *result = (double *)malloc(self->mRows * sizeof(double));
+ if (result != NULL) {
+ unsigned int row;
+ for (row = self->mRows; row; ) {
+ --row;
+ double v = 0;
+ unsigned int col;
+ for (col = self->mColumns; col; ) {
+ --col;
+ v += self_values[row][col] * vector[col];
+ }
+ result[row] = v;
}
}
+ return result;
}
-void matrix_3d_multiply_matrix_2d(const_Matrix_3d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out)
+Matrix* matrix_add_matrix(const Matrix* self, const Matrix* in)
{
- int i, j;
+ const double (*self_values)[self->mColumns] = self->mData;
+ const double (*in_values)[in->mColumns] = in->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;
+ unsigned int col;
+ for (col = self->mColumns; col; ) {
+ --col;
+ result_values[row][col] = self_values[row][col] + in_values[row][col];
+ }
+ }
+ return result;
+}
- for (i = PLANAR_DIMENSION - 1; i >= 0; i--) {
- for (j = PLANAR_DIMENSION - 1; j >= 0; j--) {
- out[i][j] = m1[i][PLANAR_X] * m2[PLANAR_X][j] + m1[i][PLANAR_Y] * m2[PLANAR_Y][j];
+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;
+ 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];
}
}
+ return result;
}
-void matrix_2d_multiply_vector_2d(const_Matrix_2d_ref m, const_Vector_2d_ref v, Vector_2d_ref out)
+Matrix* matrix_add_vector_diagonal(Matrix* self, const double* vector)
{
- out[PLANAR_X] = vector_2d_dot_product(m[PLANAR_X], v);
- out[PLANAR_Y] = vector_2d_dot_product(m[PLANAR_Y], v);
+ 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];
+ }
+ return result;
}
-void vector_2d_multiply_matrix_2d(const_Vector_2d_ref v, const_Matrix_2d_ref m, Vector_2d_ref out)
+/**
+ * @return Determinant of matrix.
+ */
+double matrix_det(const Matrix* self)
{
- out[PLANAR_X] = m[PLANAR_X][PLANAR_X] * v[PLANAR_X] + m[PLANAR_Y][PLANAR_X] * v[PLANAR_Y];
- out[PLANAR_Y] = m[PLANAR_X][PLANAR_Y] * v[PLANAR_X] + m[PLANAR_Y][PLANAR_Y] * v[PLANAR_Y];
+ const double (*self_values)[self->mColumns] = self->mData;
+ if (self->mColumns == 1) {
+ return self_values[0][0];
+ }
+ if (self->mColumns == 2) {
+ return self_values[0][0] * self_values[1][1] - self_values[1][0] * self_values[0][1];
+ }
+ if (self->mColumns == 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++) {
+ const double v = fabs(self_values[0][col]);
+ if (v > maxValue) {
+ maxValue = v;
+ maxColumn = col;
+ }
+ }
+ if (maxValue == 0) {
+ return 0;
+ }
+ maxValue = self_values[0][maxColumn];
+ Matrix* m = matrix_create_empty(self->mRows - 1, self->mColumns - 1);
+ double (*m_values)[m->mColumns] = m->mData;
+ for (col = 0; col < self->mColumns - 1; 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;
+ }
+ }
+ double result = ((maxColumn & 1) == 0 ? 1 : -1) * maxValue * matrix_det(m);
+ matrix_destroy(m);
+ return result;
}
-double matrix_2d_det(const_Matrix_2d_ref self)
+/**
+ * @return Inverted matrix.
+ */
+Matrix* matrix_invert(const Matrix* self)
{
- return self[PLANAR_X][PLANAR_X] * self[PLANAR_Y][PLANAR_Y] - self[PLANAR_Y][PLANAR_X] * self[PLANAR_X][PLANAR_Y];
+ if (self->mRows != self->mColumns) {
+ LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("rows=%d <> columns=%d"), self->mRows, self->mColumns);
+ return NULL;
+ }
+ const double (*self_values)[self->mColumns] = self->mData;
+ if (self->mColumns == 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);
+ }
+ if (self->mColumns == 2) {
+ const double det = matrix_det(self);
+ if (det == 0) {
+ return NULL;
+ LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Determinant is 0"));
+ }
+ 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);
+ }
+ if (self->mColumns == 3) {
+ const double det = matrix_det(self);
+ if (det == 0) {
+ LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Determinant is 0"));
+ return NULL;
+ }
+ double temp[3][3] = {
+ {
+ (self_values[1][1] * self_values[2][2] - self_values[1][2] * self_values[2][1]) / det,
+ (self_values[0][2] * self_values[2][1] - self_values[0][1] * self_values[2][2]) / det,
+ (self_values[0][1] * self_values[1][2] - self_values[0][2] * self_values[1][1]) / det },
+ {
+ (self_values[1][2] * self_values[2][0] - self_values[1][0] * self_values[2][2]) / det,
+ (self_values[0][0] * self_values[2][2] - self_values[0][2] * self_values[2][0]) / det,
+ (self_values[0][2] * self_values[1][0] - self_values[0][0] * self_values[1][2]) / det },
+ {
+ (self_values[1][0] * self_values[2][1] - self_values[1][1] * self_values[2][0]) / det,
+ (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);
+ }
+ LOG_FUSED_DEV(DBG_ERR, FUNCTION_PREFIX("Not implemented for size=%d"), self->mColumns);
+ return NULL;
}
-bool matrix_2d_invert(const_Matrix_2d_ref self, Matrix_2d_ref out)
+/**
+ * @return Transposed matrix.
+ */
+Matrix* matrix_transpose(const Matrix* self)
{
- const double det = matrix_2d_det(self);
- if (det == 0) {
- return false;
+ const double (*self_values)[self->mColumns] = self->mData;
+ 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];
+ }
}
+ return result;
+}
- const double denom = 1.0 / det;
- out[PLANAR_X][PLANAR_X] = self[PLANAR_Y][PLANAR_Y] * denom;
- out[PLANAR_X][PLANAR_Y] = -self[PLANAR_Y][PLANAR_X] * denom;
- out[PLANAR_Y][PLANAR_X] = -self[PLANAR_X][PLANAR_Y] * denom;
- out[PLANAR_Y][PLANAR_Y] = self[PLANAR_X][PLANAR_X] * denom;
- return true;
+/**
+ * @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;
+ }
+ }
+ return result;
}
/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* limitations under the License.
*/
-/**
- * @file Matrix.h
- * @brief matrix
- */
-
#pragma once
-#ifndef __MATRIX_H_
-#define __MATRIX_H_
-
-#include "Vector.h"
-#include <stdbool.h>
-#include <memory.h>
+#ifndef __MATRIX_H__
+#define __MATRIX_H__
#ifdef __cplusplus
extern "C" {
#endif
-
-typedef double Matrix_3d[GEO_DIMENSION][GEO_DIMENSION];
-typedef double(*Matrix_3d_ref)[GEO_DIMENSION];
-typedef const double(*const_Matrix_3d_ref)[GEO_DIMENSION];
-
-typedef double Matrix_2d[PLANAR_DIMENSION][PLANAR_DIMENSION];
-typedef double(*Matrix_2d_ref)[PLANAR_DIMENSION];
-typedef const double(*const_Matrix_2d_ref)[PLANAR_DIMENSION];
+typedef struct {
+ unsigned int mRows; /** Number of rows in matrix. */
+ unsigned int mColumns; /** Number of columns in matrix. */
+ void* mData; /** Coefficients of matrix. */
+} Matrix;
/**
- * @brief Clears content of provided 3D matrix.
- * @param[in] self 3D matrix to clear.
+ * @brief Creates empty matrix with given dimensions.
+ * @param rows Number of rows.
+ * @param columns Number of columns.
*/
-#define matrix_3d_clear(self) memset(self, 0, sizeof(Matrix_3d))
+Matrix* matrix_create_empty(const unsigned int rows, const unsigned int columns);
/**
- * @brief Copy content of one 3D matrix to another one.
- * @param[in] dst Destination 3D matrix.
- * @param[in] src Source 3D matrix.
+ * @brief Creates matrix and initializes it with given values.
+ * @param in_values_pointer Table with matrix rows. Can not be null.
*/
-#define matrix_3d_copy(dst, src) memcpy(dst, src, sizeof(Matrix_3d))
+Matrix* matrix_create_from_values(const unsigned int rows, const unsigned int columns,
+ const void* in_values_pointer);
/**
- * @brief Transposition of provided 3D matrix.
- * @param[in] self 3D matrix to transpose.
+ * @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.
*/
-extern void matrix_3d_transpose(Matrix_3d_ref 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);
/**
- * @brief Multiplies two 3D matrixes.
- * @param[in] m1 First 3D matrix.
- * @param[in] m2 Second 3D matrix.
- * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2
+ * @brief Copy constructor.
+ * @param in Source matrix. Can not be null.
*/
-extern void matrix_3d_multiply_matrix_3d(const_Matrix_3d_ref m1, const_Matrix_3d_ref m2, Matrix_3d_ref out);
+Matrix* matrix_create_from_copy(const Matrix *in);
/**
- * @brief Multiplies 3D matrix with 3D vector.
- * @param[in] m 3D matrix.
- * @param[in] v 3D vector.
- * @param[out] out Multiplication of provided matrix and vector, i.e. out = m * v
+ * Deinitialize/destroy matrix.
*/
-extern void matrix_3d_multiply_vector_3d(const_Matrix_3d_ref m, const_Vector_3d_ref v, Vector_3d_ref out);
+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 Multiplies 3D vector with 3D matrix.
- * @param[in] v 3D vector.
- * @param[in] m 3D matrix.
- * @param[out] out Multiplication of provided vector and matrix, i.e. out = v * m
+ * @brief Returns value from matrix.
+ * @param row Row number.
+ * @param column Column number.
+ * @return Value in matrix on given position.
*/
-extern void vector_3d_multiply_matrix_3d(const_Vector_3d_ref v, const_Matrix_3d_ref m, Vector_3d_ref out);
+double matrix_get(const Matrix *self, const unsigned int row, const unsigned int column);
-extern void matrix_3d_from_vector_3d(const_Vector_3d_ref p, Matrix_3d_ref out,
- double *lat, double *lon, double* sin_lat, double* cos_lat);
+/**
+ * @brief Multiplies matrix by given one.
+ * @param m Matrix to multiply. Can not be null.
+ * @return Product of matrixes.
+ */
+Matrix* matrix_multiply_by_matrix(const Matrix *self, const Matrix *in);
/**
- * @brief Inlined left-action of a linear operator on a 3D covector.
- * @param[in] m 3x3D matrix.
- * @param[in] v 3D vector.
- * @param[out] out Left-action of the linear operator @a m on the vector @a v, i.e. out = m * v
- */
-#define MATRIX_DOT_VECTOR_3D(m, v, out)\
- do {\
- (out)[X] = (m)[X][X] * (v)[X] + (m)[X][Y] * (v)[Y] + (m)[X][Z] * (v)[Z];\
- (out)[Y] = (m)[Y][X] * (v)[X] + (m)[Y][Y] * (v)[Y] + (m)[Y][Z] * (v)[Z];\
- (out)[Z] = (m)[Z][X] * (v)[X] + (m)[Z][Y] * (v)[Y] + (m)[Z][Z] * (v)[Z];\
- } while (0)
+ * @brief Multiplies matrix by given vector.
+ * @param vector Vector to multiply. Can not be null.
+ * @return Product of matrix and vector.
+ */
+double* matrix_multiply_by_vector(const Matrix *self, const double* vector, const unsigned int vector_length);
/**
- * @brief Inlined right-action of a linear operator on a 3D.
- * In Euclidean metric this is the same as left action of the transposed operator on the vector.
- * @param[in] v 3D vector.
- * @param[in] m 3x3D matrix.
- * @param[out] out Right-action of the linear operator @a m on the covector @a v, i.e. out = v * m
- */
-#define VECTOR_DOT_MATRIX_3D(v, m, out)\
- do {\
- (out)[X] = (v)[X] * (m)[X][X] + (v)[Y] * (m)[Y][X] + (v)[Z] * (m)[Z][X];\
- (out)[Y] = (v)[X] * (m)[X][Y] + (v)[Y] * (m)[Y][Y] + (v)[Z] * (m)[Z][Y];\
- (out)[Z] = (v)[X] * (m)[X][Z] + (v)[Y] * (m)[Y][Z] + (v)[Z] * (m)[Z][Z];\
- } while (0)
+ * @brief Adds matrix to given one.
+ * @param m Matrix to add. Can not be null.
+ * @return Sum of matrixes.
+ */
+Matrix* matrix_add_matrix(const Matrix *self, const Matrix *in);
/**
- * @brief Multiplies two 2D matrixes.
- * @param[in] m1 First 2D matrix.
- * @param[in] m2 Second 2D matrix.
- * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2
+ * @brief Subtracts given matrix from this one.
+ * @param m Matrix to subtract. Can not be null.
+ * @return Difference of matrixes.
*/
-extern void matrix_2d_multiply_matrix_2d(const_Matrix_2d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out);
+Matrix* matrix_sub_matrix(const Matrix *self, const Matrix *in);
/**
- * @brief Multiplies submatrix of 3D matrix with 2D matrixes.
- * @param[in] m1 3D matrix. For multiplication 2D matrix consisting of first 2 rows and first 2 columns is taken.
- * @param[in] m2 2D matrix.
- * @param[out] out Multiplication of provided matrixes, i.e. out = m1 * m2
+ * @brief Adds given vector to matrix diagonal.
+ * @param vector Vector to add. Can not be null.
+ * @return Sum of matrix and vector.
*/
-extern void matrix_3d_multiply_matrix_2d(const_Matrix_3d_ref m1, const_Matrix_2d_ref m2, Matrix_2d_ref out);
+Matrix* matrix_add_vector_diagonal(Matrix *self, const double* vector);
/**
- * @brief Multiplies 2D matrix with 2D vector.
- * @param[in] m 2D matrix.
- * @param[in] v 2D vector.
- * @param[out] out Multiplication of provided matrix and vector, i.e. out = m * v
+ * @return Determinant of matrix.
*/
-extern void matrix_2d_multiply_vector_2d(const_Matrix_2d_ref m, const_Vector_2d_ref v, Vector_2d_ref out);
+double matrix_det(const Matrix *self);
/**
- * @brief Multiplies 2D vector with 2D matrix.
- * @param[in] v 2D vector.
- * @param[in] m 2D matrix.
- * @param[out] out Multiplication of provided vector and matrix, i.e. out = v * m
+ * @return Inverted matrix.
*/
-extern void vector_2d_multiply_matrix_2d(const_Vector_2d_ref v, const_Matrix_2d_ref m, Vector_2d_ref out);
+Matrix* matrix_invert(const Matrix *self);
/**
- * @brief Determinant of 2D matrix.
- * @param[in] m 2D matrix.
- * @return Determinant of the matrix.
+ * @return Transposed matrix.
*/
-extern double matrix_2d_det(const_Matrix_2d_ref self);
+Matrix* matrix_transpose(const Matrix *self);
/**
- * @brief Inverts 2D matrix.
- * @param[in] m 2D matrix.
- * @param[out] out Inverted matrix. Not modified if result of a function is false (determinant is 0).
- * @return
- * - true - If inverted matrix was calculated.
- * - false - If matrix can not be inverted (determinant is 0).
+ * @return Symmetrized matrix.
*/
-extern bool matrix_2d_invert(const_Matrix_2d_ref m, Matrix_2d_ref out);
+Matrix* matrix_symmetrize(const Matrix *self);
#ifdef __cplusplus
}
#endif
-#endif /* __MATRIX_H_ */
+#endif /* __MATRIX_H__ */
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#if !defined(LOG_THRESHOLD)
- /* custom logging threshold - keep undefined on the repo */
- /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */
-#endif
-
-#include "MotionDetector.h"
-#include "math-ext.h"
-#include "debug_util.h"
-
-#define _USE_MATH_DEFINES
-#include <math.h>
-
-#if (FL_MOTION_DETECTOR)
-
-void motion_detector_init(MotionDetector* self, MotionDetectorStateChangeListener on_motion_state_changed)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(motion_cb=%p)"), on_motion_state_changed);
- Aema1dEstimator_init(&self->moti_al2, 1 << FL_MOTI_AEMA_PLATEAU_BITS);
- self->moti_immobile_time = 0;
- self->moti_state = MOTI_UNDECIDED;
- self->moti_last_notification = MOTI_UNDECIDED;
- self->moti_on_motion_state_changed = on_motion_state_changed;
-}
-
-void motion_detector_exit(MotionDetector* self)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
- self->moti_on_motion_state_changed = NULL;
- self->moti_last_notification = MOTI_UNDECIDED;
- self->moti_state = MOTI_UNDECIDED;
- self->moti_immobile_time = 0;
- Aema1dEstimator_exit(&self->moti_al2);
-}
-
-void motion_detector_reset(MotionDetector* self)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("()"));
- self->moti_immobile_time = 0;
- self->moti_state = MOTI_UNDECIDED;
- self->moti_last_notification = MOTI_UNDECIDED;
- Aema1dEstimator_reset(&self->moti_al2);
-}
-
-/**
- * @brief Change the state indicator and notify the user.
- * @param[in] state New state value (does not have to be different than present one).
- */
-static void motion_detector_set_state(MotionDetector* self, MotionDetectorState state)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("(%u->%u)"), self->moti_state, state);
- self->moti_state = state;
- if (self->moti_on_motion_state_changed && self->moti_last_notification != state) {
- self->moti_on_motion_state_changed(state);
- self->moti_last_notification = state;
- }
-}
-
-void motion_detector_process(MotionDetector* self, double t, const_Vector_3d_ref al)
-{
- double al2 = Aema1dEstimator_process(&self->moti_al2, vector_3d_length2(al));
- if (al2 > SQUARE(FL_MOTI_MOTION_LEVEL)) {
- switch (self->moti_state) {
- case MOTI_UNDECIDED:
- case MOTI_IMMOBILE:
- case MOTI_SLEEP:
- MotionDetector_set_state(self, MOTI_MOVING);
- break;
-
- default: /* MOTI_MOVING */
- break;
- }
- } else if (al2 < SQUARE(FL_MOTI_NOISE_LEVEL)) {
- switch (self->moti_state) {
- case MOTI_UNDECIDED:
- case MOTI_MOVING:
- MotionDetector_set_state(self, MOTI_IMMOBILE);
- self->moti_immobile_time = t;
- break;
-
- case MOTI_IMMOBILE:
- if (t - self->moti_immobile_time > FL_MOTI_IMMOBILITY_INTERVAL)
- MotionDetector_set_state(self, MOTI_SLEEP);
- break;
-
- default:
- /* MOTI_SLEEP: */
- break;
- }
- }
-}
-
-#endif
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file MotionDetector.h
- * @brief Accelerometer-based detector of significant motion.
- */
-
-#pragma once
-#ifndef __MOTION_DETECTOR_H__
-#define __MOTION_DETECTOR_H__
-
-#include "MovingAverageFilters.h"
-#include "Vector.h"
-#include "parameters.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-typedef enum {
- MOTI_UNDECIDED, /* initial */
- MOTI_MOVING, /* acceleration above MOTION_LEVEL */
- MOTI_IMMOBILE, /* acceleration below NOISE_LEVEL */
- MOTI_SLEEP, /* immobile for IMMOBILITY_INTERVAL */
-} MotionDetectorState;
-
-/**
- * @brief Callback invoked on every change in motion state.
- * @param[in] state New motion state.
- * @param[in] handler Arbitrary pointer to user data specified during the init() call.
- */
-typedef void (*MotionDetectorStateChangeListener)(MotionDetectorState state);
-
-typedef struct {
- MovingAverage1dFilter moti_al2; /**< Linear acceleration low-pass filter */
- double moti_immobile_time; /**< Start time of immobility */
- MotionDetectorState moti_state; /**< Current state */
- MotionDetectorState moti_last_notification; /**< Last notified state */
- MotionDetectorStateChangeListener moti_on_motion_state_changed; /**< User callback */
-} MotionDetector;
-
-#if (FL_MOTION_DETECTOR)
-
-/**
- * @brief Initialization of the singleton unit (static constructor).
- * @param[in] on_motion_state_changed Callback to be invoked when the detected state of motion changes.
- * @param[in] handler Arbitrary user parameter passed to the callback.
- */
-void motion_detector_init(MotionDetector* self, MotionDetectorStateChangeListener on_motion_state_changed);
-
-/** Cleanup of the singleton unit (static destructor) */
-void motion_detector_exit(MotionDetector* self);
-
-/**
- * @brief Reset of the internal state back to initial one. This function is
- * used for repetitive testing and module soft restarts.
- */
-void motion_detector_reset(MotionDetector* self);
-
-/**
- * @biref Processing of a single sample from the accelerometer. This function
- * implments a hysteresis in the 2D space of linear acceleration modulus |al|^2
- * vs motion state:
- * - Crossing the FL_MOTI_MOTION_LEVEL from below changes state to MOVING;
- * - Crossing the FL_MOTI_NOISE_LEVEL from above changes state to IMMOBILE;
- * - Staying IMMOBILE for longer than FL_MOTI_IMMOBILITY_INTERVAL changes state to SLEEP.
- *
- * @param[in] time Time of the event in seconds.
- * @param[in] al 3D vector of linear acceleration.
- */
-void motion_detector_process(MotionDetector* self, double time, const_Vector_3d_ref al);
-
-#else
- #define motion_detector_init(self, on_motion_state_changed)
- #define motion_detector_exit(self)
- #define motion_detector_reset(self)
- #define motion_detector_process(self, t, al)
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __MOTION_DETECTOR_H__ */
#include "MovingAverage.h"
#include "debug_util.h"
-#include <math.h>
-
void moving_average_init(MovingAverage* self, unsigned plateau_samples)
{
LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](bits=%u)"), self, plateau_samples);
moving_average_reset(self);
}
-void moving_average_exit(MovingAverage *self)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self);
-}
-
void moving_average_reset(MovingAverage* self)
{
LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self);
*/
extern void moving_average_init(MovingAverage *self, unsigned plateau_samples);
-/** Cleanup of the base class (destructor) */
-extern void moving_average_exit(MovingAverage *self);
-
/**
* @brief Reset the AEMA object back to initial state.
* This function is used for repetitive testing and module soft restarts.
#include "MovingAverageFilters.h"
#include "debug_util.h"
-/* Labels for an axial (or cylindrical without phase) 2D space. */
-typedef enum {
- AXIAL_H,
- AXIAL_V,
- AXIAL_DIMENSION
-} fl_axial;
-
-void moving_average_1d_filter_init(MovingAverage1dFilter* self, unsigned plateau_samples)
+void moving_average_1d_filter_init(MovingAverage1dFilter* self, unsigned plateau_samples, double initial_value)
{
moving_average_init(&self->base, plateau_samples);
- self->value = 0;
-}
-
-void moving_average_1d_filter_exit(MovingAverage1dFilter *self)
-{
- moving_average_exit(&self->base);
+ self->value = initial_value;
+ self->initial_value = initial_value;
}
void moving_average_1d_filter_reset(MovingAverage1dFilter* self)
{
moving_average_reset(&self->base);
- self->value = 0;
+ self->value = self->initial_value;
}
double moving_average_1d_filter_process(MovingAverage1dFilter* self, double value)
moving_average_set_estimate(&self->base);
self->value = value;
}
-
-void moving_average_2d_filter_init(MovingAverage2dFilter* self, unsigned plateau_samples)
-{
- moving_average_init(&self->base, plateau_samples);
- self->value[AXIAL_H] = 0;
- self->value[AXIAL_V] = 0;
-}
-
-void moving_average_2d_filter_exit(MovingAverage2dFilter *self)
-{
- moving_average_exit(&self->base);
-}
-
-void moving_average_2d_filter_reset(MovingAverage2dFilter* self)
-{
- moving_average_reset(&self->base);
- self->value[PLANAR_X] = 0;
- self->value[PLANAR_Y] = 0;
-}
-
-const_Vector_2d_ref moving_average_2d_filter_process(MovingAverage2dFilter* self, double value_H, double value_V)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V);
- moving_average_process(&self->base);
- moving_average_filter(&self->base, self->value[AXIAL_H], value_H);
- moving_average_filter(&self->base, self->value[AXIAL_V], value_V);
- return self->value;
-}
-
-void moving_average_2d_filter_set_estimate(MovingAverage2dFilter* self, double value_H, double value_V)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V);
- moving_average_set_estimate(&self->base);
- self->value[AXIAL_H] = value_H;
- self->value[AXIAL_V] = value_V;
-}
-
-void moving_average_3d_filter_init(MovingAverage3dFilter* self, unsigned plateau_samples)
-{
- moving_average_init(&self->base, plateau_samples);
- vector_3d_clear(self->value);
-}
-
-void moving_average_3d_filter_exit(MovingAverage3dFilter *self)
-{
- moving_average_exit(&self->base);
-}
-
-void moving_average_3d_filter_reset(MovingAverage3dFilter* self)
-{
- moving_average_reset(&self->base);
- vector_3d_clear(self->value);
-}
-
-const_Vector_3d_ref moving_average_3d_filter_process(MovingAverage3dFilter* self, const_Vector_3d_ref value)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[X], value[Y], value[Z]);
- moving_average_process(&self->base);
- moving_average_filter(&self->base, self->value[X], value[X]);
- moving_average_filter(&self->base, self->value[Y], value[Y]);
- moving_average_filter(&self->base, self->value[Z], value[Z]);
- return self->value;
-}
-
-void moving_average_3d_filter_set_estimate(MovingAverage3dFilter* self, const_Vector_3d_ref value)
-{
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[X], value[Y], value[Z]);
- moving_average_set_estimate(&self->base);
- vector_3d_copy(self->value, value);
-}
extern "C" {
#endif
-
/** Moving Average (1st order) 1D filter */
typedef struct {
MovingAverage base; /* base object */
double value; /* filter output (estimated value) */
+ double initial_value; /*initial filter output*/
} MovingAverage1dFilter;
/**
* @param[in] self Object pointer
* @param[in] plateau_samples Number of samples to collect before switching to standard EMA mode.
*/
-void moving_average_1d_filter_init(MovingAverage1dFilter *self, unsigned plateau_samples);
-void moving_average_1d_filter_exit(MovingAverage1dFilter *self);
+void moving_average_1d_filter_init(MovingAverage1dFilter *self, unsigned plateau_samples, double initial_value);
+
void moving_average_1d_filter_reset(MovingAverage1dFilter *self);
/**
*/
#define moving_average_1d_filter_get_estimate(self) ((self)->value)
-
-/** Moving Average (1st order) 2D filter */
-typedef struct {
- MovingAverage base; /* base object */
- Vector_2d value; /* filter output (estimated value) */
-} MovingAverage2dFilter;
-
-/**
- * @brief Initialization of the 2D AEMA object (class constructor).
- * @param[in] self Object pointer
- * @param[in] plateau_samples Number of samples to collect before switching to standard EMA mode.
- */
-void moving_average_2d_filter_init(MovingAverage2dFilter *self, unsigned plateau_samples);
-void moving_average_2d_filter_exit(MovingAverage2dFilter *self);
-void moving_average_2d_filter_reset(MovingAverage2dFilter *self);
-
-/**
- * @brief Processing of a double data sample.
- * @param[in] self Object pointer
- * @param[in] value_H First (horizontal) input value to the filter.
- * @param[in] value_V Second (vertical) input value to the filter.
- * @return Output value of the filter after processing.
- * This is the same as returned by fl_aema2d_get_estimate(self).
- */
-const_Vector_2d_ref moving_average_2d_filter_process(MovingAverage2dFilter *self, double value_X, double value_Y);
-
-/**
- * @brief [TEST] Bring the filter to a stationary state (EMA mode, input = output).
- * @param[in] self Object pointer
- * @param[in] value_H First (horizontal) i/o value of the filter.
- * @param[in] value_V Second (vertical) i/o value of the filter.
- */
-void moving_average_2d_filter_set_estimate(MovingAverage2dFilter *self, double value_X, double value_Y);
-
-/**
- * @brief (const_Vector_2d_ref) Fetch the current filter output
- * @param[in] self Object pointer
- * @return Output 2D vector after last processed sample.
- */
-#define moving_average_2d_filter_get_estimate(self) ((self)->value)
-
-
-/** Moving Average (1st order) 3D filter */
-typedef struct {
- MovingAverage base; /* base object */
- Vector_3d value; /* filter output (estimated value) */
-} MovingAverage3dFilter;
-
-/**
- * @brief Initialization of the 3D AEMA object (class constructor).
- * @param[in] self Object pointer
- * @param[in] plateau_samples Number of samples to collect before switching to standard EMA mode.
- */
-void moving_average_3d_filter_init(MovingAverage3dFilter *self, unsigned plateau_samples);
-void moving_average_3d_filter_exit(MovingAverage3dFilter *self);
-void moving_average_3d_filter_reset(MovingAverage3dFilter *self);
-
-/**
- * @brief Processing of a triple data sample.
- * @param[in] self Object pointer
- * @param[in] value Input 3D vector value to the filter.
- * @return Output value of the filter after processing.
- * This is the same as returned by fl_aema3d_get_estimate(self).
- */
-const_Vector_3d_ref moving_average_3d_filter_process(MovingAverage3dFilter *self, const_Vector_3d_ref value);
-
-/**
- * @brief [TEST]Bring the filter to a stationary state (EMA mode, input = output).
- * @param[in] self Object pointer
- * @param[in] value I/o 3D vector value of the filter.
- */
-void moving_average_3d_filter_set_estimate(MovingAverage3dFilter *self, const_Vector_3d_ref value);
-
-/**
- * (const_Vector_3d_ref) Fetch the current filter output.
- * @param[in] self Object pointer
- * @return Output 3D vector after last processed sample.
- */
-#define moving_average_3d_filter_get_estimate(self) ((self)->value)
-
#ifdef __cplusplus
}
#endif
*/
#include "Orientation.h"
-#include "Conversions.h"
+#include "Time.h"
#include "debug_util.h"
#include "parameters.h"
+#include "math-ext.h"
-#define DEFAULT_ROTATION_SIGMA2 SQUARE(1.0 / 256) /* [(rad/s)^2] */
-#define TIME_FORMAT "%.3fs"
+static const Vector_3d GRAVITY_VECTOR = {0, 0, 1};
-#if (FL_ENABLE_DEVEL_LOG)
-static const Vector_3d gravity = {0, 0, 1};
+static const Vector_3d AXIS = {1, 0, 0};
+
+static double calculate_heading(const_Quaternion_ref rot)
+{
+ Vector_3d rot_axis;
+ quaternion_rotate_vector_3d(rot, AXIS, rot_axis);
+ Vector_3d finalV;
+ vector_3d_cross_vector_3d(rot_axis, GRAVITY_VECTOR, finalV);
+ return atan2(finalV[Y], finalV[X]) + M_PI / 2;
+}
+
+bool orientation_update_heading(Orientation* self, double time, double heading)
+{
+ bool result = false;
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.9f, h=%.17e"), time, heading);
+
+ if (IS_KNOWN_TIME(self->heading_rate_update_time)) {
+ const double dt = time - self->heading_rate_update_time;
+ if (dt > 0) {
+ const double heading_rate = correct_angle(heading - self->heading_rate_heading) / dt;
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+ rotation_filter_new_rot_speed(&self->rotation_filter, time, heading_rate, SQUARE(FL_DEFAULT_SPIN_SIGMA));
+#else
+ self->heading_rate = heading_rate;
#endif
+ self->heading_rate_update_time = time;
+ self->heading_rate_heading = heading;
+ integral_value(&(self->z_rot), time, orientation_get_heading_rate(self));
+ result = true;
+ }
+ } else {
+ integral_initialize(&(self->z_rot), time, -heading);
+ self->heading_rate_update_time = time;
+ self->heading_rate_heading = heading;
+ result = true;
+ }
+ self->heading = heading;
+ self->heading_update_time = time;
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.9f, hr=%.17e, h=%.17e"),
+ self->heading_update_time, orientation_get_heading_rate(self), integral_get_integral(&(self->z_rot)));
-void orientation_init(Orientation* self,
- MotionDetectorStateChangeListener on_motion_state_changed, double rot_var)
+ return result;
+}
+
+static bool orientation_calc_heading_rate(Orientation* self)
{
- accelerometer_filter_init(&self->acc_filter, on_motion_state_changed);
- rotation_filter_init(&self->rotation_filter, rot_var);
- orientation_reset(self);
+ return orientation_update_heading(self, self->orientation_time, calculate_heading(self->orientation));
}
-void orientation_exit(Orientation* self)
+void orientation_init(Orientation* self, double rot_var)
{
- accelerometer_filter_exit(&self->acc_filter);
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+ rotation_filter_init(&self->rotation_filter, rot_var);
+#endif
+ orientation_reset(self);
}
void orientation_reset(Orientation* self)
{
- matrix_3d_clear(&self->rotation);
- unsigned i;
- for (i = GEO_DIMENSION; i;) {
- --i;
- self->rotation[i][i] = 1;
- }
- self->last_angular_velocity_time = FL_UNDEFINED_TIME;
- self->weight2 = M_1_PI / 3;
+ self->orientation[QTR_0] = 1;
+ self->orientation[QTR_X] = 0;
+ self->orientation[QTR_Y] = 0;
+ self->orientation[QTR_Z] = 0;
+ SET_UNKNOWN_TIME(self->orientation_time);
+ SET_UNKNOWN_TIME(self->last_angular_velocity_time);
+ self->acc_weight = 0;
#if (FL_KALMAN_ANGULAR_VELOCITY)
rotation_filter_reset(&self->rotation_filter);
#else
- self->z_rot_speed = 0;
+ self->heading_rate = 0;
#endif
+ integral_reset(&(self->z_rot));
+ self->heading = 0;
+ SET_UNKNOWN_TIME(self->heading_update_time);
+ self->heading_rate_heading = 0;
+ SET_UNKNOWN_TIME(self->heading_rate_update_time);
}
-bool orientation_new_angular_velocity(Orientation* self, double time, const_Vector_3d_ref angular_velocity, double s2wm)
+bool orientation_new_angular_velocity(Orientation* self, double time, const_Vector_3d_ref angular_velocity)
{
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.9f, vx=%.17e, vy=%.17e, vz=%.17e"),
+ time, angular_velocity[X], angular_velocity[Y], angular_velocity[Z]);
+
bool result = false;
- if (KNOWN_TIME(self->last_angular_velocity_time)) {
+ if (IS_KNOWN_TIME(self->last_angular_velocity_time)) {
+ Vector_3d mean_av;
+ vector_3d_linear_combination(0.5, self->last_angular_velocity, 0.5, angular_velocity, mean_av);
const double dt = time - self->last_angular_velocity_time;
- if (dt > 0) {
- Vector_3d mean_av;
- vector_3d_linear_combination(0.5, self->last_angular_velocity, 0.5, angular_velocity, mean_av);
- const double z_rot_speed = vector_3d_dot_product(self->rotation[Z], mean_av);
-
-#if (FL_KALMAN_ANGULAR_VELOCITY)
- rotation_filter_new_rot_speed(&self->rotation_filter, time, z_rot_speed, SQUARE(FL_DEFAULT_SPIN_SIGMA));
-#else
- self->z_rot_speed = z_rot_speed;
-#endif
- result = true;
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, dt=%.16e, wz=%.16e, wze=%.16e, m0=%.16e, m1=%.16e, m2=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"),
- time, dt, z_rot_speed, orientation_get_z_rot_speed(self),
- mean_av[0], mean_av[1], mean_av[2], self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]);
-
- if (vector_3d_length(mean_av) > 0) {
- /* Integrate over dt: We use the fact that for any scalar t the quaternions commute.
- * [exp(w), exp(tw)] = 0
- */
- Quaternion qwt;
- quaternion_from_rotation_vector(qwt, mean_av, dt);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e"),
- time, qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z]);
-
- Vector_3d v;
- vector_3d_cross_vector_3d(self->last_angular_velocity, angular_velocity, v);
- const double dt2 = dt * dt / 24;
- qwt[QTR_X] += dt2 * v[X];
- qwt[QTR_Y] += dt2 * v[Y];
- qwt[QTR_Z] += dt2 * v[Z];
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, v0=%.16e, v1=%.16e, v2=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e, ql=%.16e"),
- time, v[0], v[1], v[2], qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z], quaternion_norm(qwt));
-
- quaternion_normalize(qwt);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, qw=%.16e, qx=%.16e, qy=%.16e, qz=%.16e"),
+ Quaternion qwt;
+ quaternion_from_rotation_vector(qwt, mean_av, dt);
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.9f, qw=%.17e, qx=%.17e, qy=%.17e, qz=%.17e"),
time, qwt[QTR_0], qwt[QTR_X], qwt[QTR_Y], qwt[QTR_Z]);
- Matrix_3d dsr;
- quaternion_to_rotation_matrix(qwt, dsr);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, r00=%.16e, r01=%.16e, r02=%.16e, r10=%.16e, r11=%.16e, r12=%.16e, r20=%.16e, r21=%.16e, r22=%.16e"),
- time, dsr[0][0], dsr[0][1], dsr[0][2], dsr[1][0], dsr[1][1], dsr[1][2], dsr[2][0], dsr[2][1], dsr[2][2]);
-
- Matrix_3d new_rot;
- matrix_3d_multiply_matrix_3d(self->rotation, dsr, new_rot);
- matrix_3d_copy(self->rotation, new_rot);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ANGVEL(), t=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"), time, self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]);
- }
+ Vector_3d v;
+ vector_3d_cross_vector_3d(angular_velocity, self->last_angular_velocity, v);
+ const double dt2 = dt * dt / 24;
+ qwt[QTR_X] += dt2 * v[X];
+ qwt[QTR_Y] += dt2 * v[Y];
+ qwt[QTR_Z] += dt2 * v[Z];
+
+ Quaternion new_orient;
+ quaternion_multiply_quaternion(self->orientation, qwt, new_orient);
+ quaternion_clone(new_orient, self->orientation);
+ quaternion_normalize(self->orientation);
+ if (self->orientation_time < time) {
+ self->orientation_time = time;
}
+ result = orientation_calc_heading_rate(self);
+ result = true;
}
vector_3d_copy(self->last_angular_velocity, angular_velocity);
self->last_angular_velocity_time = time;
+
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.9f, q=[%.17e, %.17e, %.17e, %.17e]"),
+ self->orientation_time, self->orientation[QTR_0], self->orientation[QTR_X], self->orientation[QTR_Y], self->orientation[QTR_Z]);
+
return result;
}
* @brief Extrapolate the system rotation (__sp) to the given time assuming constant angular velocity.
* @param[in] t Internal time
*/
+/*
static void orientation_predict_rw(Orientation* self, double time)
{
- if (KNOWN_TIME(self->last_angular_velocity_time)) {
- const double dt = time - self->last_angular_velocity_time;
-
+ if (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);
- Matrix_3d dsr;
- quaternion_to_rotation_matrix(qwt, dsr);
- Matrix_3d new_rot;
- matrix_3d_multiply_matrix_3d(self->rotation, dsr, new_rot);
- matrix_3d_copy(self->rotation, new_rot);
+ 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;
}
}
}
+*/
-void orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acc)
+bool orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acc)
{
- orientation_predict_rw(self, time);
- Vector_3d gacc; /* Acceleration in global (tangent) frame */
- matrix_3d_multiply_vector_3d(self->rotation, acc, gacc);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.16e, acc0=%.16e, acc1=%.16e, acc2=%.16e, gacc0=%.16e, gacc1=%.16e, gacc2=%.16e, angle=%.16e"),
- time, acc[0], acc[1], acc[2], gacc[0], gacc[1], gacc[2], vector_3d_angle(gacc, gravity));
+ 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"),
+ gacc[0], gacc[1], gacc[2], vector_3d_angle(gacc, GRAVITY_VECTOR));
Vector_3d gu;
vector_3d_copy(gu, gacc);
-#if (FL_ENABLE_DEVEL_LOG)
- Vector_3d gn; /* Gravity in device frame */
- /* rotate back to device frame */
- VECTOR_DOT_MATRIX_3D(gu, self->rotation, gn);
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), t=%.16e, gn0=%.16e, gn1=%.16e, gn2=%.16e"),
- time, gn[0], gn[1], gn[2]);
-#endif
+ bool result = false;
/* update rotation matrix */
- Matrix_3d new_rot;
double accLen = vector_3d_length(gu);
if (accLen > 0) {
Quaternion q;
quaternion_normalize(q);
Vector_3d rot_vect;
quaternion_get_rotation_vector(q, rot_vect);
- quaternion_from_rotation_vector(q, rot_vect, 0.01);
- Matrix_3d m;
- quaternion_to_rotation_matrix(q, m);
- matrix_3d_transpose(m);
- matrix_3d_multiply_matrix_3d(m, self->rotation, new_rot);
- matrix_3d_copy(self->rotation, new_rot);
+ quaternion_from_rotation_vector(q, rot_vect, 1.0 / self->acc_weight);
+ Quaternion new_orient;
+ quaternion_multiply_quaternion(q, self->orientation, new_orient);
+ quaternion_clone(new_orient, self->orientation);
+ quaternion_normalize(self->orientation);
}
-
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("ACC(), time=%.16e, s0=%.16e, s1=%.16e, s2=%.16e"),
- time, self->rotation[Z][0], self->rotation[Z][1], self->rotation[Z][2]);
+ 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,
+ self->orientation[QTR_0], self->orientation[QTR_X], self->orientation[QTR_Y], self->orientation[QTR_Z]);
+ return result;
}
void orientation_get_orientation(Orientation* self, double* o)
{
- Quaternion q;
- quaternion_from_rotation_matrix(q, self->rotation);
- if (q[QTR_0] < 0) {
- o[0] = q[QTR_X];
- o[1] = q[QTR_Y];
- o[2] = q[QTR_Z];
+ o[0] = self->orientation_time;
+ if (self->orientation[QTR_0] < 0) {
+ o[1] = -self->orientation[QTR_0];
+ o[2] = -self->orientation[QTR_X];
+ o[3] = -self->orientation[QTR_Y];
+ o[4] = -self->orientation[QTR_Z];
} else {
- o[0] = -q[QTR_X];
- o[1] = -q[QTR_Y];
- o[2] = -q[QTR_Z];
+ o[1] = self->orientation[QTR_0];
+ o[2] = self->orientation[QTR_X];
+ o[3] = self->orientation[QTR_Y];
+ o[4] = self->orientation[QTR_Z];
}
+ o[5] = orientation_get_heading_rate(self);
+ o[6] = self->heading;
+ o[7] = integral_get_integral(&(self->z_rot));
}
void quaternion_normalize(Quaternion_ref self)
double quaternion_get_rotation_angle(const_Quaternion_ref self)
{
- return 2 * acos(fabs(self[QTR_0]));
+ 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
+*/
+}
+
+void quaternion_clone(const_Quaternion_ref self, Quaternion_ref out)
+{
+ out[QTR_0] = self[QTR_0];
+ out[QTR_X] = self[QTR_X];
+ out[QTR_Y] = self[QTR_Y];
+ out[QTR_Z] = self[QTR_Z];
+}
+
+extern void quaternion_conjugate(const_Quaternion_ref self, Quaternion_ref out)
+{
+ out[QTR_0] = self[QTR_0];
+ out[QTR_X] = -self[QTR_X];
+ out[QTR_Y] = -self[QTR_Y];
+ out[QTR_Z] = -self[QTR_Z];
+}
+
+void quaternion_multiply_quaternion(const_Quaternion_ref a, const_Quaternion_ref b, Quaternion_ref out)
+{
+ out[QTR_0] = a[QTR_0] * b[QTR_0] - a[QTR_X] * b[QTR_X] - a[QTR_Y] * b[QTR_Y] - a[QTR_Z] * b[QTR_Z];
+ out[QTR_X] = a[QTR_0] * b[QTR_X] + a[QTR_X] * b[QTR_0] + a[QTR_Y] * b[QTR_Z] - a[QTR_Z] * b[QTR_Y];
+ out[QTR_Y] = a[QTR_0] * b[QTR_Y] - a[QTR_X] * b[QTR_Z] + a[QTR_Y] * b[QTR_0] + a[QTR_Z] * b[QTR_X];
+ out[QTR_Z] = a[QTR_0] * b[QTR_Z] + a[QTR_X] * b[QTR_Y] - a[QTR_Y] * b[QTR_X] + a[QTR_Z] * b[QTR_0];
+}
+
+void quaternion_multiply_vector_3d(const_Quaternion_ref self, const_Vector_3d_ref v, Quaternion_ref out)
+{
+ out[QTR_0] = - self[QTR_X] * v[X] - self[QTR_Y] * v[Y] - self[QTR_Z] * v[Z];
+ out[QTR_X] = self[QTR_0] * v[X] + self[QTR_Y] * v[Z] - self[QTR_Z] * v[Y];
+ out[QTR_Y] = self[QTR_0] * v[Y] + self[QTR_Z] * v[X] - self[QTR_X] * v[Z];
+ out[QTR_Z] = self[QTR_0] * v[Z] + self[QTR_X] * v[Y] - self[QTR_Y] * v[X];
+}
+
+void quaternion_rotate_vector_3d(const_Quaternion_ref self, const_Vector_3d_ref v, Vector_3d_ref out)
+{
+ Quaternion qc;
+ quaternion_conjugate(self, qc);
+ Quaternion qm;
+ quaternion_multiply_vector_3d(self, v, qm);
+ Quaternion q;
+ quaternion_multiply_quaternion(qm, qc, q);
+ out[X] = q[QTR_X];
+ out[Y] = q[QTR_Y];
+ out[Z] = q[QTR_Z];
+}
+
+void quaternion_invrotate_vector_3d(const_Quaternion_ref self, const_Vector_3d_ref v, Vector_3d_ref out)
+{
+ Quaternion qc;
+ quaternion_conjugate(self, qc);
+ Quaternion qm;
+ quaternion_multiply_vector_3d(qc, v, qm);
+ Quaternion q;
+ quaternion_multiply_quaternion(qm, self, q);
+ out[X] = q[QTR_X];
+ out[Y] = q[QTR_Y];
+ out[Z] = q[QTR_Z];
}
void quaternion_from_rotation_vector(Quaternion_ref self, const_Vector_3d_ref rot_vector, double scale)
}
}
-void quaternion_to_rotation_matrix(const_Quaternion_ref self, Matrix_3d_ref rot_matrix)
+void integral_init(Integral* self)
{
- double xx = 2 * self[QTR_X] * self[QTR_X];
- double xy = 2 * self[QTR_X] * self[QTR_Y];
- double xz = 2 * self[QTR_X] * self[QTR_Z];
- double xw = 2 * self[QTR_X] * self[QTR_0];
- double yy = 2 * self[QTR_Y] * self[QTR_Y];
- double yz = 2 * self[QTR_Y] * self[QTR_Z];
- double yw = 2 * self[QTR_Y] * self[QTR_0];
- double zz = 2 * self[QTR_Z] * self[QTR_Z];
- double zw = 2 * self[QTR_Z] * self[QTR_0];
- rot_matrix[X][X] = 1 - yy - zz;
- rot_matrix[X][Y] = xy + zw;
- rot_matrix[X][Z] = xz - yw;
-
- rot_matrix[Y][X] = xy - zw;
- rot_matrix[Y][Y] = 1 - xx - zz;
- rot_matrix[Y][Z] = yz + xw;
-
- rot_matrix[Z][X] = xz + yw;
- rot_matrix[Z][Y] = yz - xw;
- rot_matrix[Z][Z] = 1 - xx - yy;
+ integral_reset(self);
}
-void quaternion_from_rotation_matrix(Quaternion_ref self, const_Matrix_3d_ref m)
+void integral_reset(Integral* self)
{
- double qw;
- double qx;
- double qy;
- double qz;
- double tr = m[0][0] + m[1][1] + m[2][2];
-
- if (tr > 0) {
- double s = sqrt(tr + 1.0) * 2; /* s = 4 * qw */
- qw = 0.25 * s;
- qx = (m[2][1] - m[1][2]) / s;
- qy = (m[0][2] - m[2][0]) / s;
- qz = (m[1][0] - m[0][1]) / s;
- } else if ((m[0][0] > m[1][1]) & (m[0][0] > m[2][2])) {
- double s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2; /* s = 4 * qx */
- qw = (m[2][1] - m[1][2]) / s;
- qx = 0.25 * s;
- qy = (m[0][1] + m[1][0]) / s;
- qz = (m[0][2] + m[2][0]) / s;
- } else if (m[1][1] > m[2][2]) {
- double s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2; /* s = 4 * qy */
- qw = (m[0][2] - m[2][0]) / s;
- qx = (m[0][1] + m[1][0]) / s;
- qy = 0.25 * s;
- qz = (m[1][2] + m[2][1]) / s;
- } else {
- double s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2; /* s = 4 * qz */
- qw = (m[1][0] - m[0][1]) / s;
- qx = (m[0][2] + m[2][0]) / s;
- qy = (m[1][2] + m[2][1]) / s;
- qz = 0.25 * s;
- }
+ SET_UNKNOWN_TIME(self->time);
+ self->integral = 0;
+}
- double n = sqrt(qw * qw + qx * qx + qy * qy + qz * qz);
- if (qw >= 0) {
- self[QTR_0] = qw / n;
- self[QTR_X] = -qx / n;
- self[QTR_Y] = -qy / n;
- self[QTR_Z] = -qz / n;
- } else {
- self[QTR_0] = -qw / n;
- self[QTR_X] = qx / n;
- self[QTR_Y] = qy / n;
- self[QTR_Z] = qz / n;
+void integral_initialize(Integral* self, double time, double value)
+{
+ self->time = time;
+ self->integral = value;
+}
+
+double integral_get_integral(Integral* self)
+{
+ return self->integral;
+}
+
+void integral_value(Integral* self, double time, double value)
+{
+ if (IS_KNOWN_TIME(time)) {
+ if (IS_KNOWN_TIME(self->time)) {
+ const double dt = time - self->time;
+ if (dt > 0) {
+ self->integral += dt * value;
+ self->time = time;
+ }
+ } else {
+ self->time = time;
+ }
}
}
+#if (FL_KALMAN_ANGULAR_VELOCITY)
+
void rotation_filter_init(RotationFilter* self, double rot_var)
{
self->def_rot_var = rot_var;
{
self->rot_speed = 0;
self->rot_speed_variance = self->def_rot_var;
- self->rot_speed_time = FL_UNDEFINED_TIME;
+ SET_UNKNOWN_TIME(self->rot_speed_time);
self->last_azimuth = 0;
- self->last_azimuth_time = FL_UNDEFINED_TIME;
+ SET_UNKNOWN_TIME(self->last_azimuth_time);
}
void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot_speed, double rot_speed_variance)
{
- if (KNOWN_TIME(self->rot_speed_time)) {
+ if (IS_KNOWN_TIME(self->rot_speed_time)) {
const double dt = time - self->rot_speed_time;
/* propagate/diffuse sigma^2 */
const double s2wzk = s2wzp + rot_speed_variance;
if (s2wzk > 0) {
const double inv_s2k = 1.0 / s2wzk;
- LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=" TIME_FORMAT ", w=%.4g, s2w=%.4g): __wze:%g->%g, __s2wze:%g->%g"),
+ LOG_FUSED_DEV(DBG_INFO, FUNCTION_PREFIX("(t=%.9fs, w=%.4g, s2w=%.4g): __wze:%g->%g, __s2wze:%g->%g"),
time, rot_speed, rot_speed_variance, self->rot_speed,
inv_s2k * (s2wzp * rot_speed + rot_speed_variance * self->rot_speed),
self->rot_speed_variance, inv_s2k * s2wzp * rot_speed_variance * 2);
void rotation_filter_new_azimuth(RotationFilter* self, double time, double azimuth, double vhe, double speed)
{
- if (speed > 0 && KNOWN_TIME(self->last_azimuth_time)) {
+ if (speed > 0 && IS_KNOWN_TIME(self->last_azimuth_time)) {
const double dt = time - self->last_azimuth_time;
if (speed * vhe * dt > 0) {
- /* circular difference */
- double db = azimuth - self->last_azimuth;
- if (db > M_PI) {
- db -= M_2PI;
- } else if (db < -M_PI) {
- db += M_2PI;
- }
- const double rot_speed = db / dt;
+ const double rot_speed = correct_angle(-(azimuth - self->last_azimuth)) / 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;
}
+
+#endif
#ifndef __ORIENTATION_H_
#define __ORIENTATION_H_
-#include <stdbool.h>
+#include "parameters.h"
#include "Vector.h"
-#include "Matrix.h"
#include "math-ext.h"
-#include "AccelerometerFilter.h"
+#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif
+#if (FL_KALMAN_ANGULAR_VELOCITY)
typedef struct {
double def_rot_var; /* Initial variance of z rotation speed. */
double last_azimuth_time; /* Time of last azimuth update. */
} RotationFilter;
+#endif
+
typedef struct {
- Matrix_3d rotation; /* Current rotation matrix */
- double weight2; /* Square weight (inverse covariance) vector of matrix rows */
- Vector_3d last_angular_velocity; /* Last reported angular velocity. */
- double last_angular_velocity_time; /* Time when last angular velocity was reported. */
- AccelerometerFilter acc_filter; /* Accelerometer filter. */
+ double time;
+ double integral;
+} Integral;
+
+/** Labels for quaternion components. */
+typedef enum {
+ QTR_X,
+ QTR_Y,
+ QTR_Z,
+ QTR_0,
+ QUATERNION_DIMENSION
+} _quaternion_component;
+
+typedef double Quaternion[QUATERNION_DIMENSION];
+typedef double* Quaternion_ref;
+typedef const double* const_Quaternion_ref;
+
+typedef struct {
+ Quaternion orientation; /* Current orientation quaternion. */
+ double orientation_time; /* Time of last orientation update. */
+ Vector_3d last_angular_velocity; /* Last reported angular velocity. */
+ double last_angular_velocity_time; /* Time of last angular velocity report. */
+ double acc_weight; /* Weight used by orientation update on new acceleration. */
#if (FL_KALMAN_ANGULAR_VELOCITY)
RotationFilter rotation_filter; /* Filters z-rotation speed. */
#else
- double z_rot_speed; /* Angular z rotation speed (horizontal plane). */
+ double heading_rate; /* Heading change rate (angular rotation speed in horizontal plane). */
#endif
+ double heading_rate_update_time; /* Time of last heading rate update. */
+ double heading_rate_heading; /* Heading used for heading rate update. */
+ Integral z_rot; /* Integral of Z rotation speed. */
+ double heading; /* Heading of movement. */
+ double heading_update_time; /* Time of last heading update. */
} Orientation;
-extern void orientation_init(Orientation* self,
- MotionDetectorStateChangeListener on_motion_state_changed, double var_rot);
+extern void orientation_init(Orientation* self, double var_rot);
-extern void orientation_exit(Orientation* self);
extern void orientation_reset(Orientation* self);
+/** (double) */
+#define orientation_get_heading_rate_time(self) ((self)->heading_rate_update_time)
+
+/** (double) */
+#define orientation_get_heading_time(self) ((self)->heading_update_time)
+
#if (FL_KALMAN_ANGULAR_VELOCITY)
/** (double) */
-#define orientation_get_z_rot_speed(self) rotation_filter_get_rot_speed(&(self)->rotation_filter)
+#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)
#else
/** (double) */
-#define orientation_get_z_rot_speed(self) ((self)->z_rot_speed)
+#define orientation_get_heading_rate(self) ((self)->heading_rate)
#endif
+/** (double) */
+#define orientation_get_heading(self) ((self)->heading)
+
/**
* @brief Update system rotation matrix given a measurement of the device angular velocity.
- * @param[in] t Internal time of the measurement
- * @param[in] wm 3D vector of angular velocity [rad/s] in device coordinate frame
- * @param[in] s2wm Estimated variance of the measuremnt
+ * @param[in] time Internal time of the measurement
+ * @param[in] angular_velocity 3D vector of angular velocity [rad/s] in device coordinate frame
+ * @return (bool) True if new heading rate was calculated.
*/
extern bool orientation_new_angular_velocity(Orientation* self,
- double time, const_Vector_3d_ref angular_velocity, double s2wm);
+ double time, const_Vector_3d_ref angular_velocity);
-extern void orientation_new_acceleration(Orientation* self, double time, const_Vector_3d_ref acceleration);
+/**
+ * @brief Update system rotation matrix given a measurement of the device acceleration.
+ * @param[in] time Internal time of the measurement
+ * @param[in] angular_velocity 3D vector of acceleration [m/s] in device coordinate frame
+ * @return (bool) True if new heading rate was calculated.
+ */
+extern bool orientation_new_acceleration(Orientation* self,
+ double time, const_Vector_3d_ref acceleration);
extern void orientation_get_orientation(Orientation* self, double* o);
-/** Labels for quaternion components. */
-typedef enum {
- QTR_X,
- QTR_Y,
- QTR_Z,
- QTR_0,
- QUATERNION_DIMENSION
-} _quaternion_component;
-
-typedef double Quaternion[QUATERNION_DIMENSION];
-typedef double* Quaternion_ref;
-typedef const double* const_Quaternion_ref;
+/**
+ * @brief Updates heading and heading rate.
+ * @param[in] time Time of heading measurement.
+ * @param[in] heading Current heading measurement.
+ * @return (bool) True if new heading rate was calculated.
+ */
+extern bool orientation_update_heading(Orientation* self,
+ double time, double heading);
/**
* @brief Returns quaternion norm.
*/
extern double quaternion_get_rotation_angle(const_Quaternion_ref self);
+/**
+ * @brief Creates clone of quaternion.
+ * @param[in] self Quaternion to clone.
+ * @param[out] out Clone of quaternion self.
+ */
+extern void quaternion_clone(const_Quaternion_ref self, Quaternion_ref out);
+
+/**
+ * @brief Returns conjugate of quaternion.
+ * @param[in] self Input quaternion.
+ * @param[out] out Conjugate of quaternion self.
+ */
+extern void quaternion_conjugate(const_Quaternion_ref self, Quaternion_ref out);
+
+/**
+ * @brief Returns product of two quaternions.
+ * @param[in] a First factor.
+ * @param[in] b Second factor.
+ * @param[out] out Product of a and b.
+ */
+extern void quaternion_multiply_quaternion(const_Quaternion_ref a,
+ const_Quaternion_ref b, Quaternion_ref out);
+
+/**
+ * @brief Multiplies quaternion with vector part of another one.
+ * @param[in] self First factor.
+ * @param[in] v Vector representing vector part of second quaternion.
+ * @param[out] out Product of quaternions.
+ */
+extern void quaternion_multiply_vector_3d(const_Quaternion_ref self,
+ const_Vector_3d_ref v, Quaternion_ref out);
+
+/**
+ * @brief Rotates 3D vector using quaternion.
+ * @param[in] self Quaternion representing rotation (normed).
+ * @param[in] v Vector to rotate.
+ * @param[out] out Rotated v vector using rotation represented by quaternion q.
+ */
+extern void quaternion_rotate_vector_3d(const_Quaternion_ref self,
+ const_Vector_3d_ref v, Vector_3d_ref out);
+
+/**
+ * @brief Rotates 3D vector using inverted quaternion.
+ * @param[in] self Quaternion representing inverted rotation (normed).
+ * @param[in] v Vector to rotate.
+ * @param[out] out Rotated v vector using rotation represented by inversion of quaternion q.
+ */
+extern void quaternion_invrotate_vector_3d(const_Quaternion_ref self,
+ const_Vector_3d_ref v, Vector_3d_ref out);
+
/**
* @brief Calculates quaternion representing rotation multiplied by some number.
* @param[out] self Output quaternion.
* @param[in] rot_vector Rotation vector.
* @param[in] scale Scale to multiply rotation vector.
*/
-extern void quaternion_from_rotation_vector(Quaternion_ref self, const_Vector_3d_ref rot_vector, double scale);
+extern void quaternion_from_rotation_vector(Quaternion_ref self,
+ const_Vector_3d_ref rot_vector, double scale);
/**
* @brief Returns rotation vector assuming quaternion represents rotation.
* @param[in] self Input quaternion. Must be normed.
* @param[out] rot Rotation vector representing same rotation as input quaternion.
*/
-extern void quaternion_get_rotation_vector(const_Quaternion_ref self, Vector_3d_ref rot);
+extern void quaternion_get_rotation_vector(const_Quaternion_ref self,
+ Vector_3d_ref rot);
-/**
- * @brief Maps a quaternion into system rotation matrix assuming quaternion represents rotation.
- * @param[in] self Input quaternion. Must be normed.
- * @param[out] rot_matrix Corresponding system rotation 3D matrix
- */
-extern void quaternion_to_rotation_matrix(const_Quaternion_ref self, Matrix_3d_ref rot_matrix);
+extern void integral_init(Integral* self);
-/**
- * @brief Creates quaternion representing rotation from given rotation matrix.
- * @param m 3D rotation matrix.
- * @return Normed quaternion representing rotation with nonnegative scalar part.
- */
-extern void quaternion_from_rotation_matrix(Quaternion_ref self, const_Matrix_3d_ref m);
+extern void integral_reset(Integral* self);
+
+extern void integral_initialize(Integral* self, double time, double value);
+
+extern void integral_value(Integral* self, double time, double value);
+
+extern double integral_get_integral(Integral* self);
+
+#if (FL_KALMAN_ANGULAR_VELOCITY)
/**
* @brief Initialization of the filter (constructor).
* @param[in] rot_speed Angular velocity projection on the axis.
* @param[in] rot_speed_variance Corresponding variance.
*/
-extern void rotation_filter_new_rot_speed(RotationFilter* self, double time, double rot_speed, double rot_speed_variance);
+extern void rotation_filter_new_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_azimuth(RotationFilter* self,
+ double time, double azimuth, double vhe, double speed);
/** (double) Returns current rotation speed along axis */
#define rotation_filter_get_rot_speed(self) ((self)->rot_speed)
+#endif
+
#ifdef __cplusplus
}
#endif
*/
#include "PeaceDetector.h"
-#include "Conversions.h"
+#include "Time.h"
void peace_detector_init(PeaceDetector *self, double stable_level, long stable_number)
{
void peace_detector_reset(PeaceDetector *self)
{
self->stable_counter = self->stable_number;
- self->last_timestamp = FL_UNDEFINED_TIME;
+ SET_UNKNOWN_TIME(self->last_timestamp);
}
bool peace_detector_is_stable(PeaceDetector *self)
bool peace_detector_new_data(PeaceDetector *self, double timestamp, const_Vector_3d_ref data)
{
- if ((!KNOWN_TIME(self->last_timestamp)) || self->last_timestamp > timestamp) {
+ if ((!IS_KNOWN_TIME(self->last_timestamp)) || self->last_timestamp > timestamp) {
self->stable_counter = self->stable_number;
} else {
const double length = vector_3d_distance(self->previous_measurement, data);
#define __PEACEDETECTOR_H_
#include "Vector.h"
-#include <stdbool.h>
+#include <stdbool.h>
typedef struct {
double stable_level; /* Noise level below which the data are treated as stable. */
} PeaceDetector;
extern void peace_detector_init(PeaceDetector *self, double stable_level, long stable_number);
+
extern void peace_detector_reset(PeaceDetector *self);
+
extern bool peace_detector_is_stable(PeaceDetector *self);
+
extern bool peace_detector_new_data(PeaceDetector *self, double timestamp, const_Vector_3d_ref data);
#endif /* __PEACEDETECTOR_H_ */
--- /dev/null
+/*
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "PositionFusion1D.h"
+#include "parameters.h"
+#include "math-ext.h"
+
+/* the bigger the default standard deviations, the swifter response to first measurements */
+#define INITIAL_POSITION_SIGMA2 SQUARE(1024 * FL_DEFAULT_POSITION_SIGMA)
+#define INITIAL_VELOCITY_SIGMA2 SQUARE(1024 * FL_DEFAULT_VELOCITY_SIGMA)
+
+void position_fusion_1d_init(PositionFusion1D* self)
+{
+ position_fusion_1d_reset(self);
+}
+
+void position_fusion_1d_destroy(PositionFusion1D* self)
+{
+}
+
+void position_fusion_1d_reset(PositionFusion1D* self)
+{
+ self->position = 0;
+ self->position_dev = INITIAL_POSITION_SIGMA2;
+ self->speed = 0;
+ self->speed_dev = INITIAL_VELOCITY_SIGMA2;
+}
+
+double position_fusion_1d_get_position(const PositionFusion1D* self)
+{
+ return self->position;
+}
+
+void position_fusion_1d_set_position(PositionFusion1D* self, double pos)
+{
+ self->position = pos;
+}
+
+double position_fusion_1d_get_position_deviation(const PositionFusion1D* self)
+{
+ return self->position_dev;
+}
+
+double position_fusion_1d_get_speed(const PositionFusion1D* self)
+{
+ return self->speed;
+}
+
+double position_fusion_1d_get_speed_deviation(const PositionFusion1D* self)
+{
+ return self->speed_dev;
+}
+
+void position_fusion_1d_update_position(PositionFusion1D* self, double measuredPosition, double positionDeviation)
+{
+ self->position = measuredPosition;
+ self->position_dev = positionDeviation;
+}
+
+void position_fusion_1d_update_speed(PositionFusion1D* self, double measuredSpeed, double speedDeviation)
+{
+ self->speed = measuredSpeed;
+ self->speed_dev = speedDeviation;
+}
+
+void position_fusion_1d_update_position_speed(PositionFusion1D* self,
+ double measuredPosition, double positionDeviation, double measuredSpeed, double speedDeviation)
+{
+ self->position = measuredPosition;
+ self->position_dev = positionDeviation;
+ self->speed = measuredSpeed;
+ self->speed_dev = speedDeviation;
+}
--- /dev/null
+/*
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file PositionFusion1D.h
+ * @brief Currently simply stores current position and speed.
+ */
+#pragma once
+#ifndef __POSITION_FUSION_1D_H__
+#define __POSITION_FUSION_1D_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+ double position; /* Position in [m]. */
+ double position_dev; /* Position deviation in [m]. */
+ double speed; /* Speed in [m/s]. */
+ double speed_dev; /* Speed deviation in [m/s]. */
+} PositionFusion1D;
+
+/**
+ * Initialize PositionFusion1D object.
+ */
+extern void position_fusion_1d_init(PositionFusion1D* self);
+
+/**
+ * Deinitialize/destroy PositionFusion1D object .
+ */
+extern void position_fusion_1d_destroy(PositionFusion1D* self);
+
+/**
+ * Resets filter to initial state.
+ */
+extern void position_fusion_1d_reset(PositionFusion1D* self);
+
+/**
+ * @return Position [m].
+ */
+extern double position_fusion_1d_get_position(const PositionFusion1D* self);
+
+extern void position_fusion_1d_set_position(PositionFusion1D* self, double pos);
+
+/**
+ * @return Standard deviation of position in [m].
+ */
+extern double position_fusion_1d_get_position_deviation(const PositionFusion1D* self);
+
+/**
+ * @return Speed in [m/s].
+ */
+extern double position_fusion_1d_get_speed(const PositionFusion1D* self);
+
+/**
+ * @return Standard deviation of speed in [m/s].
+ */
+extern double position_fusion_1d_get_speed_deviation(const PositionFusion1D* self);
+
+/**
+ * @brief Updates state by measured position.
+ * @param measuredPosition Measured position in [m].
+ * @param positionDeviation Standard deviation of position measurement in [m].
+ */
+extern void position_fusion_1d_update_position(PositionFusion1D* self,
+ double measuredPosition, double positionDeviation);
+
+/**
+ * @brief Updates state by measured speed.
+ * @param measuredSpeed Measured speed in [m/s].
+ * @param speedDeviation Standard deviation of speed measurement in [m/s].
+ */
+extern void position_fusion_1d_update_speed(PositionFusion1D* self,
+ double measuredSpeed, double speedDeviation);
+
+/**
+ * @brief Updates state by measured position.
+ * @param measuredPosition Measured position in [m].
+ * @param positionDeviation Standard deviation of position measurement in [m].
+ * @param measuredSpeed Measured speed in [m/s].
+ * @param speedDeviation Standard deviation of speed measurement in [m/s].
+ */
+extern void position_fusion_1d_update_position_speed(PositionFusion1D* self,
+ double measuredPosition, double positionDeviation,
+ double measuredSpeed, double speedDeviation);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __POSITION_FUSION_1D_H__ */
--- /dev/null
+/*
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "PositionFusion2D.h"
+#include "math-ext.h"
+#include "debug_util.h"
+#include <limits.h>
+
+/** Process noise standard deviation of linear acceleration in [m/s^2]. */
+static const double STD_ACC = 0.5; /* TODO: Tuning this vale can improve algorithm accuracy. */
+
+/** Process noise standard deviation of angular acceleration in [rad/s^2]. */
+static const double STD_A_ACC = 0.5; /* TODO: Tuning this vale can improve algorithm accuracy. */
+
+/** Process noise variance of linear acceleration in [(m/s^2)^2]. */
+#define VAR_ACC (STD_ACC * STD_ACC)
+
+/** Process noise variance of angular acceleration in [(rad/s^2)^2]. */
+#define VAR_A_ACC (STD_A_ACC * STD_A_ACC)
+
+/** Initial standard deviation of position in [m]. */
+#define INIT_POS_DEV 1e6 /* TODO: Tuning this vale can improve algorithm accuracy. */
+
+/** Initial standard deviation of speed in [m/s]. */
+#define INIT_SPEED_DEV 500.0 /* TODO: Tuning this vale can improve algorithm accuracy. */
+
+/** Initial standard deviation of direction in [rad]. */
+#define INIT_DIRECTION_DEV 100.0 /* TODO: Tuning this vale can improve algorithm accuracy. */
+
+/** Initial standard deviation of angular speed in [rad/s]. */
+#define INIT_ANGULAR_SPEED_DEV 100.0 /* TODO: Tuning this vale can improve algorithm accuracy. */
+
+/** Size of state vector. */
+#define STATE_VECTOR_SIZE 5
+
+/** Index of estimate of x coordinate in state vector. */
+static const int STATE_X_INDEX = 0;
+
+/** Index of estimate of y coordinate in state vector. */
+static const int STATE_Y_INDEX = 1;
+
+/** Index of estimate of linear speed in state vector. */
+static const int STATE_SPEED_INDEX = 2;
+
+/** Index of estimate of direction in state vector. */
+static const int STATE_DIRECTION_INDEX = 3;
+
+/** Index of estimate of angular speed in state vector. */
+static const int STATE_ANGULAR_SPEED_INDEX = 4;
+
+/** Initial state. */
+static double INITIAL_STATE[STATE_VECTOR_SIZE] = {0, 0, 0, 0, 0};
+
+/** Initial covariance matrix. */
+static const double INITIAL_COVARIANCE[STATE_VECTOR_SIZE][STATE_VECTOR_SIZE] = {
+ { INIT_POS_DEV * INIT_POS_DEV / 2, 0, 0, 0, 0 },
+ { 0, INIT_POS_DEV * INIT_POS_DEV / 2, 0, 0, 0 },
+ { 0, 0, INIT_SPEED_DEV * INIT_SPEED_DEV, 0, 0 },
+ { 0, 0, 0, INIT_DIRECTION_DEV * INIT_DIRECTION_DEV, 0 },
+ { 0, 0, 0, 0, INIT_ANGULAR_SPEED_DEV * INIT_ANGULAR_SPEED_DEV }
+ };
+
+/** Position measurement matrix. */
+static const double POSITION_MEASUREMENT_MATRIX[2][5] = {
+ { 1, 0, 0, 0, 0 },
+ { 0, 1, 0, 0, 0 }
+ };
+
+/** Speed measurement matrix. */
+static const double SPEED_MEASUREMENT_MATRIX[1][5] = {
+ { 0, 0, 1, 0, 0 }
+ };
+
+/** Position speed measurement matrix. */
+static const double POSITION_SPEED_MEASUREMENT_MATRIX[3][5] = {
+ { 1, 0, 0, 0, 0 },
+ { 0, 1, 0, 0, 0 },
+ { 0, 0, 1, 0, 0 }
+ };
+
+/** Speed and direction measurement matrix. */
+static const double SPEED_DIRECTION_MEASUREMENT_MATRIX[2][5] = {
+ { 0, 0, 1, 0, 0 },
+ { 0, 0, 0, 1, 0 }
+ };
+
+/** Speed and angular speed measurement matrix. */
+static const double ANGULAR_SPEED_MEASUREMENT_MATRIX[2][5] = {
+ { 0, 0, 0, 0, 1 }
+ };
+
+/** Speed and angular speed measurement matrix. */
+static const double SPEED_ANGULAR_SPEED_MEASUREMENT_MATRIX[2][5] = {
+ { 0, 0, 1, 0, 0 },
+ { 0, 0, 0, 0, 1 }
+ };
+
+/** Speed, direction and angular speed measurement matrix. */
+static const double PDR_MEASUREMENT_MATRIX[3][5] = {
+ { 0, 0, 1, 0, 0 },
+ { 0, 0, 0, 1, 0 },
+ { 0, 0, 0, 0, 1 }
+ };
+
+/**
+ * Initialize PositionFusion2D object.
+ */
+void 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->positionSpeedMeasurementMatrix = matrix_create_from_values(3, 5, POSITION_SPEED_MEASUREMENT_MATRIX);
+ self->speedDirectionMeasurementMatrix = matrix_create_from_values(2, 5, SPEED_DIRECTION_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);
+}
+
+void position_fusion_2d_destroy(PositionFusion2D* self)
+{
+ kalman_filter_destroy(&self->mKalmanFilter);
+ matrix_destroy(self->positionMeasurementMatrix);
+ matrix_destroy(self->speedMeasurementMatrix);
+ matrix_destroy(self->positionSpeedMeasurementMatrix);
+ matrix_destroy(self->speedDirectionMeasurementMatrix);
+ matrix_destroy(self->angularSpeedMeasurementMatrix);
+ matrix_destroy(self->speedAngularSpeedMeasurementMatrix);
+ matrix_destroy(self->pdrMeasurementMatrix);
+}
+
+void position_fusion_2d_reset(PositionFusion2D* self)
+{
+ self->mLastTimestamp = UNKNOWN_TIMESTAMP;
+ kalman_filter_set_state(&self->mKalmanFilter, INITIAL_STATE, INITIAL_COVARIANCE);
+}
+
+static double position_fusion_2d_get_variable_value(const PositionFusion2D* self, const int index)
+{
+ return kalman_filter_get_variable_value(&self->mKalmanFilter, index);
+}
+
+static double position_fusion_2d_get_variable_variance(const PositionFusion2D* self, const int index)
+{
+ return kalman_filter_get_variable_variance(&self->mKalmanFilter, index);
+}
+
+/**
+ * @return Time of current prediction in [ns].
+ */
+Timestamp position_fusion_2d_get_current_time(const PositionFusion2D* self)
+{
+ return self->mLastTimestamp;
+}
+
+double position_fusion_2d_get_x(const PositionFusion2D* self)
+{
+ return position_fusion_2d_get_variable_value(self, STATE_X_INDEX);
+}
+
+double position_fusion_2d_get_y(const PositionFusion2D* self)
+{
+ LOG_FUSED_DEV(DBG_LOW, INDENT("()"));
+ return position_fusion_2d_get_variable_value(self, STATE_Y_INDEX);
+}
+
+void position_fusion_2d_set_position(PositionFusion2D* self, double x, double y)
+{
+ kalman_filter_set_variable_value(&self->mKalmanFilter, STATE_X_INDEX, x);
+ kalman_filter_set_variable_value(&self->mKalmanFilter, STATE_Y_INDEX, y);
+}
+
+double position_fusion_2d_get_position_deviation(const PositionFusion2D* self)
+{
+ const double cx = position_fusion_2d_get_variable_variance(self, STATE_X_INDEX);
+ const double cy = position_fusion_2d_get_variable_variance(self, STATE_Y_INDEX);
+ if (cx < 0 || cy < 0) {
+ LOG_FUSED_DEV(DBG_WARN, UNINDENT("() cx=%f, cy=%f"), cx, cy);
+ return 0;
+ }
+ return sqrt(cx + cy);
+}
+
+double position_fusion_2d_get_speed(const PositionFusion2D* self)
+{
+ return position_fusion_2d_get_variable_value(self, STATE_SPEED_INDEX);
+}
+
+double position_fusion_2d_get_speed_x(const PositionFusion2D* self)
+{
+ return position_fusion_2d_get_speed(self) * cos(position_fusion_2d_get_direction(self));
+}
+
+double position_fusion_2d_get_speed_y(const PositionFusion2D* self)
+{
+ return position_fusion_2d_get_speed(self) * sin(position_fusion_2d_get_direction(self));
+}
+
+double position_fusion_2d_get_speed_deviation(const PositionFusion2D* self)
+{
+ const double var = position_fusion_2d_get_variable_variance(self, STATE_SPEED_INDEX);
+ if (var < 0) {
+ LOG_FUSED_DEV(DBG_WARN, UNINDENT("() Pv=%f"), var);
+ return 0;
+ }
+ return sqrt(var);
+}
+
+double position_fusion_2d_get_direction(const PositionFusion2D* self)
+{
+ return position_fusion_2d_get_variable_value(self, STATE_DIRECTION_INDEX);
+}
+
+double position_fusion_2d_get_direction_deviation(const PositionFusion2D* self)
+{
+ const double var = position_fusion_2d_get_variable_variance(self, STATE_DIRECTION_INDEX);
+ if (var < 0) {
+ LOG_FUSED_DEV(DBG_WARN, UNINDENT("() Ph=%f"), var);
+ return 0;
+ }
+ return sqrt(var);
+}
+
+double position_fusion_2d_get_angular_speed(const PositionFusion2D* self)
+{
+ return position_fusion_2d_get_variable_value(self, STATE_ANGULAR_SPEED_INDEX);
+}
+
+double position_fusion_2d_get_angular_speed_deviation(const PositionFusion2D* self)
+{
+ const double var = position_fusion_2d_get_variable_variance(self, STATE_ANGULAR_SPEED_INDEX);
+ if (var < 0) {
+ LOG_FUSED_DEV(DBG_WARN, UNINDENT("() Pa=%f"), var);
+ return 0;
+ }
+ return sqrt(var);
+}
+
+void position_fusion_2d_predict(PositionFusion2D* self, Timestamp predictTimestamp)
+{
+ if (predictTimestamp != UNKNOWN_TIMESTAMP && self->mLastTimestamp != UNKNOWN_TIMESTAMP) {
+ const Timestamp time = predictTimestamp - self->mLastTimestamp;
+
+ if (time > 0) {
+ const double dt = time / 1e9;
+ const double teta = position_fusion_2d_get_direction(self);
+ const double v = position_fusion_2d_get_speed(self);
+ const double dTsin = dt * sin(teta);
+ const double dTcos = dt * cos(teta);
+ void state_transition_function(const double* x, double* out)
+ {
+ out[STATE_X_INDEX] = x[STATE_X_INDEX] + dt * x[STATE_SPEED_INDEX] * cos(x[STATE_DIRECTION_INDEX]);
+ out[STATE_Y_INDEX] = x[STATE_Y_INDEX] + dt * x[STATE_SPEED_INDEX] * sin(x[STATE_DIRECTION_INDEX]);
+ out[STATE_SPEED_INDEX] = x[STATE_SPEED_INDEX];
+ out[STATE_DIRECTION_INDEX] = x[STATE_DIRECTION_INDEX] + dt * x[STATE_ANGULAR_SPEED_INDEX];
+ out[STATE_ANGULAR_SPEED_INDEX] = x[STATE_ANGULAR_SPEED_INDEX];
+ };
+ const double stateTransition[5][5] = {
+ { 1, 0, dTcos, -v * dTsin, 0 },
+ { 0, 1, dTsin, v * dTcos, 0 },
+ { 0, 0, 1, 0, 0 },
+ { 0, 0, 0, 1, dt },
+ { 0, 0, 0, 0, 1 }
+ };
+ Matrix *stateTransitionMatrix = matrix_create_from_values(5, 5, stateTransition);
+ const double dt2 = dt * dt;
+ const double dT2sin2 = dt * dTsin / 2;
+ const double dT2cos2 = dt * dTcos / 2;
+ const double wvdt2cos2 = VAR_ACC * dT2cos2;
+ const double wvdt2sin2 = VAR_ACC * dT2sin2;
+ const double c00 = wvdt2cos2 * dT2cos2;
+ const double c01 = wvdt2cos2 * dT2sin2;
+ const double c02 = wvdt2cos2 * dt;
+ const double c11 = wvdt2sin2 * dT2sin2;
+ const double c12 = wvdt2sin2 * dt;
+ const double c22 = VAR_ACC * dt2;
+ const double c44 = VAR_A_ACC * dt2;
+ const double c34 = c44 * dt / 2;
+ const double c33 = c34 * dt / 2;
+ const double processNoise[5][5] = {
+ { c00, c01, c02, 0, 0 },
+ { c01, c11, c12, 0, 0 },
+ { c02, c12, c22, 0, 0 },
+ { 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);
+ matrix_destroy(stateTransitionMatrix);
+ matrix_destroy(processNoiseMatrix);
+ }
+ }
+ self->mLastTimestamp = predictTimestamp;
+}
+
+void position_fusion_2d_update_position(PositionFusion2D* self,
+ Timestamp timestamp, double x, double y, double positionDeviation)
+{
+ position_fusion_2d_predict(self, timestamp);
+
+ const double innovation[2] = {
+ x - position_fusion_2d_get_x(self),
+ y - position_fusion_2d_get_y(self)
+ };
+ const double rx = positionDeviation * positionDeviation / 2;
+ const double noiseVector[2] = {rx, rx};
+
+ kalman_filter_correct(&self->mKalmanFilter, self->positionMeasurementMatrix, innovation, noiseVector);
+}
+
+extern void position_fusion_2d_update_speed(PositionFusion2D* self,
+ Timestamp timestamp, double measuredSpeed, double speedDeviation)
+{
+ LOG_FUSED_DEV(DBG_LOW, INDENT("speed=%f, acc=%f"), measuredSpeed, speedDeviation);
+
+ position_fusion_2d_predict(self, timestamp);
+
+ const double innovation[1] = {measuredSpeed - position_fusion_2d_get_speed(self)};
+ const double noiseVector[1] = {speedDeviation * speedDeviation};
+
+ kalman_filter_correct(&self->mKalmanFilter, self->speedMeasurementMatrix, innovation, noiseVector);
+
+ LOG_FUSED_DEV(DBG_LOW, INDENT("x=%f, y=%f, acc=%f, speed=%f, acc=%f"),
+ position_fusion_2d_get_x(self),
+ position_fusion_2d_get_y(self),
+ position_fusion_2d_get_position_deviation(self),
+ position_fusion_2d_get_speed(self),
+ position_fusion_2d_get_speed_deviation(self));
+}
+
+void position_fusion_2d_update_position_speed(PositionFusion2D* self,
+ Timestamp timestamp, double x, double y, double positionDeviation, double measuredSpeed, double speedDeviation)
+{
+ LOG_FUSED_DEV(DBG_LOW, INDENT("x=%f, y=%f, acc=%f, speed=%f, acc=%f"), x, y, positionDeviation, measuredSpeed, speedDeviation);
+
+ position_fusion_2d_predict(self, timestamp);
+
+ const double innovation[3] = {
+ x - position_fusion_2d_get_x(self),
+ y - position_fusion_2d_get_y(self),
+ measuredSpeed - position_fusion_2d_get_speed(self)
+ };
+ const double rx = positionDeviation * positionDeviation / 2;
+ const double noiseVector[3] = {rx, rx, speedDeviation * speedDeviation};
+
+ kalman_filter_correct(&self->mKalmanFilter, self->positionSpeedMeasurementMatrix, innovation, noiseVector);
+
+ LOG_FUSED_DEV(DBG_LOW, INDENT("x=%f, y=%f, acc=%f, speed=%f, acc=%f"),
+ position_fusion_2d_get_x(self),
+ position_fusion_2d_get_y(self),
+ position_fusion_2d_get_position_deviation(self),
+ position_fusion_2d_get_speed(self),
+ position_fusion_2d_get_speed_deviation(self));
+}
+
+void position_fusion_2d_update_speed_direction(PositionFusion2D* self, Timestamp timestamp,
+ double measuredSpeed, double speedDeviation, double measuredDirection, double directionDeviation)
+{
+ position_fusion_2d_predict(self, timestamp);
+
+ const double innovation[2] = {
+ measuredSpeed - position_fusion_2d_get_speed(self),
+ correct_angle(measuredDirection - position_fusion_2d_get_direction(self))
+ };
+ const double noiseVector[2] = {speedDeviation * speedDeviation, directionDeviation * directionDeviation};
+
+ kalman_filter_correct(&self->mKalmanFilter, self->speedDirectionMeasurementMatrix, innovation, noiseVector);
+}
+
+void position_fusion_2d_update_angular_speed(PositionFusion2D* self, Timestamp timestamp, double measuredAngularSpeed, double angularSpeedDeviation)
+{
+ position_fusion_2d_predict(self, timestamp);
+
+ const double angularSpeed = position_fusion_2d_get_angular_speed(self);
+
+ const double innovation[1] = {measuredAngularSpeed - angularSpeed};
+ const double noiseVector[1] = {angularSpeedDeviation * angularSpeedDeviation};
+
+ kalman_filter_correct(&self->mKalmanFilter, self->angularSpeedMeasurementMatrix, innovation, noiseVector);
+}
+
+void position_fusion_2d_update_speed_angular_speed(PositionFusion2D* self, Timestamp timestamp,
+ double measuredSpeed, double speedDeviation, double measuredAngularSpeed, double angularSpeedDeviation)
+{
+ position_fusion_2d_predict(self, timestamp);
+
+ const double innovation[2] = {
+ measuredSpeed - position_fusion_2d_get_speed(self),
+ measuredAngularSpeed - position_fusion_2d_get_angular_speed(self)
+ };
+ const double noiseVector[2] = {
+ speedDeviation * speedDeviation,
+ angularSpeedDeviation * angularSpeedDeviation
+ };
+
+ kalman_filter_correct(&self->mKalmanFilter, self->speedAngularSpeedMeasurementMatrix, innovation, noiseVector);
+}
+
+void position_fusion_2d_update_speed_direction_angular_speed(PositionFusion2D* self, Timestamp timestamp,
+ double measuredSpeed, double speedDeviation,
+ double measuredDirection, double directionDeviation,
+ double measuredAngularSpeed, double angularSpeedDeviation)
+{
+ position_fusion_2d_predict(self, timestamp);
+
+ const double innovation[3] = {
+ measuredSpeed - position_fusion_2d_get_speed(self),
+ correct_angle(measuredDirection - position_fusion_2d_get_direction(self)),
+ measuredAngularSpeed - position_fusion_2d_get_angular_speed(self)
+ };
+ const double noiseVector[3] = {
+ speedDeviation * speedDeviation,
+ directionDeviation * directionDeviation,
+ angularSpeedDeviation * angularSpeedDeviation
+ };
+
+ kalman_filter_correct(&self->mKalmanFilter, self->pdrMeasurementMatrix, innovation, noiseVector);
+}
--- /dev/null
+/*
+ * Copyright (c) 2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file PositionFusion2D.h
+ * @brief Implementation of Kalman filter for fusion of 2D position, speed and heading rate measurements.
+ */
+#pragma once
+#ifndef __POSITION_FUSION_2D_H__
+#define __POSITION_FUSION_2D_H__
+
+#include "KalmanFilter.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef long long Timestamp; /** Time-stamps in [ns] */
+
+/** Indicates unknown timestamp */
+#define UNKNOWN_TIMESTAMP 0x8000000000000000LL
+
+/**
+ * Keeps current state of positioning engine (currently position, speed, direction and angular speed).
+ * Updates properly state if new position or speed is measured.
+ * This implementation maintain state on 2-dimensional plane.
+ * z coordinate is always 0.
+ */
+typedef struct {
+ KalmanFilter mKalmanFilter;
+ Timestamp mLastTimestamp; /** Timestamp of previous state prediction in [ns]. */
+ Matrix *positionMeasurementMatrix;
+ Matrix *speedMeasurementMatrix;
+ Matrix *positionSpeedMeasurementMatrix;
+ Matrix *speedDirectionMeasurementMatrix;
+ Matrix *angularSpeedMeasurementMatrix;
+ Matrix *speedAngularSpeedMeasurementMatrix;
+ Matrix *pdrMeasurementMatrix;
+} PositionFusion2D;
+
+/** Initialize PositionFusion2D object. */
+extern void position_fusion_2d_init(PositionFusion2D* self);
+
+/** Deinitialize/destroy PositionFusion2D object . */
+extern void position_fusion_2d_destroy(PositionFusion2D* self);
+
+/** Resets filter to initial state. */
+extern void position_fusion_2d_reset(PositionFusion2D* self);
+
+/** @return Time of current prediction in [ns]. */
+extern Timestamp position_fusion_2d_get_current_time(const PositionFusion2D* self);
+
+/** @return x coordinate in [m]. */
+extern double position_fusion_2d_get_x(const PositionFusion2D* self);
+
+/** @return y coordinate in [m]. */
+extern double position_fusion_2d_get_y(const PositionFusion2D* self);
+
+/** Changes position without changing any other state components. */
+extern void position_fusion_2d_set_position(PositionFusion2D* self, double x, double y);
+
+/** @return Standard deviation of position in [m]. */
+extern double position_fusion_2d_get_position_deviation(const PositionFusion2D* self);
+
+/** @return Current speed in [m/s]. */
+extern double position_fusion_2d_get_speed(const PositionFusion2D* self);
+
+/** @return Speed in x direction in [m/s]. */
+extern double position_fusion_2d_get_speed_x(const PositionFusion2D* self);
+
+/** @return Speed in y direction in [m/s]. */
+extern double position_fusion_2d_get_speed_y(const PositionFusion2D* self);
+
+/** @return Speed standard deviation in [m/s]. */
+extern double position_fusion_2d_get_speed_deviation(const PositionFusion2D* self);
+
+/** @return Direction (angle between speed vector and x axis counterclockwise) in [rad]. */
+extern double position_fusion_2d_get_direction(const PositionFusion2D* self);
+
+/** @return Direction deviation in [rad]. */
+extern double position_fusion_2d_get_direction_deviation(const PositionFusion2D* self);
+
+/** @return Speed of rotation in [rad/s] counterclockwise. */
+extern double position_fusion_2d_get_angular_speed(const PositionFusion2D* self);
+
+/** @return Rotation speed deviation in [rad/s]. */
+extern double position_fusion_2d_get_angular_speed_deviation(const PositionFusion2D* self);
+
+/**
+ * @brief Make state prediction for given time.
+ * @param predictTimestamp Time in [ns] for which prediction is calculated.
+ */
+extern void position_fusion_2d_predict(PositionFusion2D* self, Timestamp predictTimestamp);
+
+/**
+ * @brief Updates state by measured position.
+ * @param timestamp Timestamp of measure in [ns].
+ * @param x Measured x coordinate of position in [m].
+ * @param y Measured y coordinate of position in [m].
+ * @param positionDeviation Standard deviation of position measurement in [m].
+ */
+extern void position_fusion_2d_update_position(PositionFusion2D* self, Timestamp timestamp,
+ double x, double y, double positionDeviation);
+
+/**
+ * @brief Updates state by measured speed.
+ * @param timestamp Timestamp of measure in [ns].
+ * @param measuredSpeed Measured speed in [m/s].
+ * @param speedDeviation Standard deviation of speed measurement in [m/s].
+ */
+extern void position_fusion_2d_update_speed(PositionFusion2D* self, Timestamp timestamp,
+ double measuredSpeed, double speedDeviation);
+
+/**
+ * @brief Updates state by measured position.
+ * @param timestamp Timestamp of measure in [ns].
+ * @param x Measured x coordinate of position in [m].
+ * @param y Measured y coordinate of position in [m].
+ * @param positionDeviation Standard deviation of position measurement in [m].
+ * @param measuredSpeed Measured speed in [m/s].
+ * @param speedDeviation Standard deviation of speed measurement in [m/s].
+ */
+extern void position_fusion_2d_update_position_speed(PositionFusion2D* self, Timestamp timestamp,
+ double x, double y, double positionDeviation, double measuredSpeed, double speedDeviation);
+
+/**
+ * @brief Updates state by measured speed and direction.
+ * @param timestamp Timestamp of measure in [ns].
+ * @param measuredSpeed Measured speed in [m/s].
+ * @param speedDeviation Standard deviation of speed measurement in [m/s].
+ * @param measuredDirection Measured direction (angle between speed vector and x axis counterclockwise) in [rad].
+ * @param directionDeviation Standard deviation of direction measurement in [rad].
+ */
+extern void position_fusion_2d_update_speed_direction(PositionFusion2D* self, Timestamp timestamp,
+ double measuredSpeed, double speedDeviation, double measuredDirection, double directionDeviation);
+
+/**
+ * @brief Updates state by measured speed.
+ * @param timestamp Timestamp of measure in [ns].
+ * @param measuredAngularSpeed Measured angular speed (rate of change of speed vector direction counterclockwise) in [rad/s].
+ * @param angularSpeedDeviation Standard deviation of angular speed in [rad/s].
+ */
+extern void position_fusion_2d_update_angular_speed(PositionFusion2D* self, Timestamp timestamp,
+ double measuredAngularSpeed, double angularSpeedDeviation);
+
+/**
+ * @brief Updates state by measured speed.
+ * @param timestamp Timestamp of measure in [ns].
+ * @param measuredSpeed Measured speed in [m/s].
+ * @param speedDeviation Standard deviation of speed measurement in [m/s].
+ * @param measuredAngularSpeed Measured angular speed (rate of change of speed vector direction counterclockwise) in [rad/s].
+ * @param angularSpeedDeviation Standard deviation of angular speed in [rad/s].
+ */
+extern void position_fusion_2d_update_speed_angular_speed(PositionFusion2D* self, Timestamp timestamp,
+ double measuredSpeed, double speedDeviation, double measuredAngularSpeed, double angularSpeedDeviation);
+
+/**
+ * @brief Updates state by measured speed and direction.
+ * @param timestamp Timestamp of measure in [ns].
+ * @param measuredSpeed Measured speed in [m/s].
+ * @param speedDeviation Standard deviation of speed measurement in [m/s].
+ * @param measuredDirection Measured direction (angle between speed vector and x axis counterclockwise) in [rad].
+ * @param directionDeviation Standard deviation of direction measurement in [rad].
+ * @param measuredAngularSpeed Measured angular speed (rate of change of speed vector direction counterclockwise) in [rad/s].
+ * @param angularSpeedDeviation Standard deviation of angular speed in [rad/s].
+ */
+extern void position_fusion_2d_update_speed_direction_angular_speed(PositionFusion2D* self, Timestamp timestamp,
+ double measuredSpeed, double speedDeviation, double measuredDirection, double directionDeviation,
+ double measuredAngularSpeed, double angularSpeedDeviation);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __POSITION_FUSION_2D_H__ */
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "TangentFrame.h"
-#include "Conversions.h"
-
-#include <memory.h>
-
-#define globalToTangent(tf, g, t) matrix_3d_multiply_vector_3d((tf)->fsr, g, t)
-#define tangentToGlobal(tf, t, g) vector_3d_multiply_matrix_3d(t, (tf)->fsr, g)
-
-void earth_global_to_spherical(const_Vector_3d_ref p, double* latitude, double* longitude, double* altitude)
-{
- const double yz2 = fl_square(p[Y]) + fl_square(p[Z]);
- const double ry = atan2(p[Y], p[Z]);
- *longitude = conversions_radians_to_longitude(ry);
-
- const double xyz2 = yz2 + fl_square(p[X]);
- const double rx = atan2(p[X], sqrt(yz2));
- *latitude = conversions_radians_to_latitude(rx);
-
- *altitude = sqrt(xyz2) - EARTH_RADIUS;
-}
-
-void earth_spherical_to_global(double latitude, double longitude, double radius, Vector_3d_ref p)
-{
- const double rx = conversions_degrees_to_radians(latitude);
- const double ry = conversions_degrees_to_radians(longitude);
- const double sin_rx = radius * sin(rx);
- const double cos_rx = radius * cos(rx);
- vector_3d_set(p, sin_rx, cos_rx * sin(ry), cos_rx * cos(ry));
-}
-
-void tangent_frame_init(TangentFrame* self)
-{
- memset(self, 0, sizeof(*self));
- self->fsr[0][0] = 1;
- self->fsr[1][1] = 1;
- self->fsr[2][2] = 1;
- self->cos_lat = 1;
-}
-
-void tangent_frame_create(TangentFrame* self, Vector_3d_ref pos, Vector_3d_ref vel)
-{
- Vector_3d p;
- tangentToGlobal(self, pos, p);
- Vector_3d v;
- tangentToGlobal(self, vel, v);
-
- matrix_3d_from_vector_3d(p, self->fsr, &self->lat, &self->lon, &self->sin_lat, &self->cos_lat);
-
- globalToTangent(self, p, pos);
- globalToTangent(self, v, vel);
-}
-
-/**
- * @brief Returns 2D vector in tangent coordinate system pointing from given 2D point to the North direction.
- * @param[in] self Tangent coordinate system.
- * @param[in] p 2D point coordinates in tangent coordinate system.
- * @param[out] u 2D vector pointing to the North in tangent coordinate system.
- */
-static void tangent_frame_to_north(const TangentFrame* self, const_Vector_2d_ref p, Vector_2d_ref u)
-{
- u[PLANAR_X] = EARTH_RADIUS * self->cos_lat - p[PLANAR_X] * self->sin_lat;
- u[PLANAR_Y] = - p[PLANAR_Y] * self->sin_lat;
-}
-
-/**
- * @brief Returns azimuth in tangent coordinate system from given 2D point.
- * @param[in] self Tangent coordinate system.
- * @param[in] p 2D point coordinates in tangent coordinate system.
- * @return Azimuth from given 2D point, i.e angle in radians from North to the given point.
- */
-static double tangent_frame_azimuth(const TangentFrame* self, const_Vector_2d_ref p)
-{
- Vector_2d u;
- tangent_frame_to_north(self, p, u);
- return atan2(-u[PLANAR_Y], u[PLANAR_X]);
-}
-
-void tangent_frame_tangent_to_spherical(const TangentFrame* self, const_Vector_3d_ref pos,
- double* latitude, double* longitude, double* altitude)
-{
- Vector_3d p;
- tangentToGlobal(self, pos, p);
- earth_global_to_spherical(p, latitude, longitude, altitude);
-}
-
-void tangent_frame_spherical_to_tangent(const TangentFrame* self,
- double latitude, double longitude, double radius, Vector_3d_ref pos)
-{
- Vector_3d pg;
- earth_spherical_to_global(latitude, longitude, radius, pg);
- globalToTangent(self, pg, pos);
-}
-
-void tangent_frame_spherical_to_tangent_speed(const TangentFrame* self,
- const_Vector_3d_ref pos, double speed, double azimuth, double climb, Vector_3d_ref v)
-{
- double rb = conversions_degrees_to_radians(azimuth) - tangent_frame_azimuth(self, pos);
- vector_3d_set(v, speed * cos(rb), speed * sin(rb), climb);
-}
-
-void tangent_frame_tangent_to_spherical_speed(const TangentFrame* self,
- const_Vector_3d_ref pos, const_Vector_3d_ref vel, double* speed, double* direction, double* climb)
-{
- Vector_2d to_north;
- tangent_frame_to_north(self, pos, to_north);
- *direction = conversions_radians_to_positive_degrees(-vector_2d_angle(vel, to_north));
- *speed = vector_2d_length(vel);
- *climb = vel[Z];
-}
+++ /dev/null
-/*
- * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/**
- * @file TangentFrame.h
- * @brief Tangent frame.
- */
-
-#pragma once
-#ifndef __TANGENTFRAME_H_
-#define __TANGENTFRAME_H_
-
-#include "Matrix.h"
-#include "Vector.h"
-#include "math-ext.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/**
- * @brief Convert spherical coordinate in radians to arc-length along the Earth's great circle in meters.
- * @param[in] radians Angle spanned by the great arc.
- * @return (double) Corresponding value in meters along Earth's surface.
- */
-#define earth_radians_to_meters(radians) ((radians) * EARTH_RADIUS)
-
-/**
- * @brief Convert arc-length in meters along the Earth's great circle to arc angle in radians.
- * @param[in] meters Arc length in meters.
- * @return (double) Corresponding angle in radians.
- */
-#define earth_meters_to_radians(meters) ((meters) * (1.0 / EARTH_RADIUS));
-
-/**
- * @brief Convert point in global coordinates to spherical coordinates.
- * @param[in] p Represents coordinates of a point in global (Earth) coordinate system.
- * @param[out] latitude Latitude of provided point.
- * @param[out] longitude Longitude of provided point.
- * @param[out] altitude Altitude of provided point.
- */
-extern void earth_global_to_spherical(const_Vector_3d_ref p, double* latitude, double* longitude, double* altitude);
-
-/**
- * @brief Convert point in spherical coordinates to global coordinates.
- * @param[in] latitude Latitude.
- * @param[in] longitude Longitude.
- * @param[in] altitude Altitude.
- * @param[out] p Represents coordinates of a point in global (Earth) coordinate system.
- */
-extern void earth_spherical_to_global(double latitude, double longitude, double altitude, Vector_3d_ref p);
-
-
-
-/**
- * Global frame coordinate system:
- * - Center in the middle of the Earth.
- * - OX directed to the North Pole (latitude = 90).
- * - OY directed to the latitude = 0 and longitude = 90;
- * - OZ directed to the latitude = 0 and longitude = 0;
- */
-
-/**
- * Tangent frame coordinate system:
- * - OX directed to the North.
- * - OY directed to the East;
- * - OZ directed to the up;
- */
-
-/** Tangent space T(p)E to the embedding manifold E=SxR at point p */
-typedef struct {
- double lat; /* latitude */
- double lon; /* longitude */
- double sin_lat; /* sin(latitude) */
- double cos_lat; /* cos(latitude) */
- Matrix_3d fsr; /* push-forward (tangent to global) coordinate system rotation matrix */
-} TangentFrame;
-
-extern void tangent_frame_init(TangentFrame* self);
-
-/**
- * @brief Create coordinate map from spherical M = S^2 x R to the tangent space TM.
- * Notice that only two angles are being used, what means the course on the tangent plane
- * is not straight North (x-axis), but still described by the bearing.
- * @param[in/out] self Tangent coordinate system.
- * @param[in/out] pos Position in old tangent frame
- * @param[in/out] vel Velocity in old tangent frame
- */
-extern void tangent_frame_create(TangentFrame* self, Vector_2d_ref pos, Vector_2d_ref vel);
-
-/**
- * @brief Converts coordinates in tangent coordinate system to spherical coordinate system.
- * @param[in] self Tangent coordinate system.
- * @param[in] pos Coordinates of given point in tangent coordinate system.
- * @param[out] latitude Latitude in degrees.
- * @param[out] longitude Longitude in degrees.
- * @param[out] altitude Altitude in meters.
- */
-extern void tangent_frame_tangent_to_spherical(const TangentFrame* self,
- const_Vector_3d_ref pos, double* latitude, double* longitude, double* altitude);
-
-/**
- * @brief Converts coordinates in spherical coordinate system to tangent coordinate system.
- * @param[in] self Tangent coordinate system.
- * @param[in] latitude Latitude in degrees.
- * @param[in] longitude Longitude in degrees.
- * @param[in] radius Distance from the center of the Earth (altitude + Earth radius) in meters.
- * @param[out] pos Coordinates of given point in tangent coordinate system.
- */
-extern void tangent_frame_spherical_to_tangent(const TangentFrame* self,
- double latitude, double longitude, double radius, Vector_3d_ref pos);
-
-/**
- * @brief Converts speed in spherical coordinate system to tangent coordinate system.
- * @param[in] self Tangent coordinate system.
- * @param[in] pos Coordinates of location in tangent coordinate system.
- * @param[in] speed Horizontal speed in m/s.
- * @param[in] azimuth Bearing (angle between true North and speed direction clockwise) in radians.
- * @param[in] climb Vertical speed in m/s (positive up).
- * @param[out] v Speed in given location in tangent coordinate system.
- */
-extern void tangent_frame_spherical_to_tangent_speed(const TangentFrame* self,
- const_Vector_3d_ref pos, double speed, double azimuth, double climb, Vector_3d_ref v);
-
-/**
- * @brief Converts speed in tangent coordinate system to spherical coordinate system.
- * @param[in] self Tangent coordinate system.
- * @param[in] pos Coordinates of location in tangent coordinate system.
- * @param[in] vel Speed in given location in tangent coordinate system.
- * @param[out] speed Horizontal speed in m/s.
- * @param[out] azimuth Bearing (angle between true North and speed direction clockwise) in radians.
- * @param[out] climb Vertical speed in m/s (positive up).
- */
-extern void tangent_frame_tangent_to_spherical_speed(const TangentFrame* self,
- const_Vector_3d_ref pos, const_Vector_3d_ref vel, double* speed, double* azimuth, double* climb);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __TANGENTFRAME_H_ */
--- /dev/null
+/*
+ * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file Time.h
+ * @brief Handles undefined time
+ */
+#pragma once
+#ifndef __TIME_H__
+#define __TIME_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define FL_UNDEFINED_TIME -1e+48
+#define SET_UNKNOWN_TIME(t) ((t) = FL_UNDEFINED_TIME)
+#define IS_KNOWN_TIME(t) ((t) > FL_UNDEFINED_TIME)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __TIME_H__ */
*/
#include "TimeOffset.h"
-#include "Conversions.h"
+#include "Time.h"
#include "parameters.h"
-void time_offset_init(TimeOffset* self)
+void time_offset_init(TimeOffset* self, double initial_offset)
{
- moving_average_1d_filter_init(&self->filter, 1 << FL_TIMO_AEMA_PLATEAU_BITS);
+ moving_average_1d_filter_init(&self->filter, 1 << FL_TIMO_AEMA_PLATEAU_BITS, initial_offset);
time_offset_reset(self);
}
void time_offset_reset(TimeOffset* self)
{
moving_average_1d_filter_reset(&self->filter);
- self->first_reference_time = FL_UNDEFINED_TIME;
- self->last_reference_time = FL_UNDEFINED_TIME;
+ SET_UNKNOWN_TIME(self->first_reference_time);
+ SET_UNKNOWN_TIME(self->last_reference_time);
self->reference_count = 0;
- self->correct_time = FL_UNDEFINED_TIME;
-}
-
-void time_offset_exit(TimeOffset* self)
-{
- moving_average_1d_filter_exit(&self->filter);
+ SET_UNKNOWN_TIME(self->correct_time);
}
static void time_offset_new(TimeOffset* self, double dt)
self->last_reference_time = time;
self->reference_count++;
- if (KNOWN_TIME(self->correct_time)) {
+ if (IS_KNOWN_TIME(self->correct_time)) {
const double dt = self->last_reference_time - (self->correct_time - time_offset_get(self));
if (dt < 0) {
time_offset_new(self, self->correct_time - self->last_reference_time);
double time_offset_correct_time(TimeOffset* self, double time)
{
self->correct_time = time;
- if (KNOWN_TIME(self->last_reference_time)) {
- double dt = self->correct_time - time_offset_get(self)
- - self->last_reference_time;
+ if (IS_KNOWN_TIME(self->last_reference_time)) {
+ double dt = self->correct_time - time_offset_get(self) - self->last_reference_time;
+
if (dt < 0) {
time_offset_new(self, self->correct_time - self->last_reference_time);
dt = 0;
return self->last_reference_time + dt;
}
- return self->correct_time;
+ return self->correct_time - time_offset_get(self);
}
double time_offset_get(const TimeOffset* self)
extern "C" {
#endif
-
typedef struct {
MovingAverage1dFilter filter;
double first_reference_time; /* Arrival time of the first sensor event. */
double correct_time; /* Arrival time of the last position measurement event. */
} TimeOffset;
-extern void time_offset_init(TimeOffset* self);
+extern void time_offset_init(TimeOffset* self, double initial_offset);
+
extern void time_offset_reset(TimeOffset* self);
-extern void time_offset_exit(TimeOffset* self);
/**
* @brief Supplies reference time
typedef double* Vector_3d_ref;
typedef const double* const_Vector_3d_ref;
-
#define vector_3d_clear(self) memset(self, 0, sizeof(Vector_3d))
+
#define vector_3d_clear_array(self) memset(self, 0, sizeof(self))
/**
(out)[Z] = (u)[Z] - (v)[Z];\
} while (0)
+/**
+ * @brief Inlined sum of two 3D vectors.
+ * @param[in] u 3D vector
+ * @param[in] v 3D vector
+ * @param[out] out Sum of a @u and a @v
+ */
+#define vector_3d_add_vector_3d(u, v, out)\
+ do {\
+ (out)[X] = (u)[X] + (v)[X];\
+ (out)[Y] = (u)[Y] + (v)[Y];\
+ (out)[Z] = (u)[Z] + (v)[Z];\
+ } while (0)
+
/**
* @brief Inlined cross-product of two 3D vectors.
* @param[in] u 3D vector
}
const double update_rate = weight * self->weight_norm;
const double decay_rate = 1.0 - update_rate;
- LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("fw2=%g, e=%g, s2=%g, w=%g"),
- fw2, self->evidence, self->scale2, weight);
+
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("fw2=%g, e=%g, s2=%g, w=%g"), fw2, self->evidence, self->scale2, weight);
vector_3d_linear_combination(decay_rate, self->value, update_rate, wm, self->value);
vector_3d_sub_vector_3d(wm, self->value, self->wf);
return self->wf;
} Waema3dEstimator;
extern void waema3d_estimator_reset(Waema3dEstimator* self, double alpha, double aevidence, double ascale2);
+
extern void waema3d_estimator_set(Waema3dEstimator* self, double scale2, double evidence, double weight_norm, const_Vector_3d_ref w0);
+
extern const_Vector_3d_ref waema3d_estimator_filter(Waema3dEstimator* self, const_Vector_3d_ref wm);
#ifdef __cplusplus
--- /dev/null
+/*
+ * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "WgsConversion.h"
+#include "Time.h"
+#include "debug_util.h"
+#include "math-ext.h"
+
+#include <memory.h>
+
+void wgs_conversion_init(WgsConversion* self)
+{
+ memset(self, 0, sizeof(*self));
+}
+
+bool wgs_conversion_is_valid(WgsConversion* self)
+{
+ return self->mercator_scale != 0;
+}
+
+void wgs_conversion_create(WgsConversion* self, double latitude, double longitude)
+{
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("latitude=%.17e, longitude=%.17e"), latitude, longitude);
+ self->latitude = latitude;
+ self->longitude = longitude;
+ self->mercator_scale = EARTH_RADIUS * cos(to_radians(latitude));
+ self->log_lat = log(tan(M_PI / 4 + to_radians(latitude) / 2));
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("scale=%.17e, log_lat=%.17e"), self->mercator_scale, self->log_lat);
+}
+
+void wgs_conversion_local_to_wgs(const WgsConversion* self, double x, double y, double* latitude, double* longitude)
+{
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("x=%.17e, y=%.17e"), x, y);
+ *longitude = to_degrees(x / self->mercator_scale) + self->longitude;
+ *latitude = to_degrees(atan(sinh((y / self->mercator_scale + self->log_lat))));
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("lat=%.17e, lon=%.17e"), *latitude, *longitude);
+}
+
+void wgs_conversion_wgs_to_local(const WgsConversion* self, double latitude, double longitude, double* x, double* y)
+{
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("lat=%.17e, lon=%.17e"), latitude, longitude);
+ *x = to_radians(longitude - self->longitude) * self->mercator_scale;
+ *y = (log(tan(M_PI / 4 + to_radians(latitude) / 2)) - self->log_lat) * self->mercator_scale;
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("x=%.17e, y=%.17e"), *x, *y);
+}
+
+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;
+ 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;
+ LOG_FUSED_DEV(DBG_LOW, FUNCTION_PREFIX("bearing=%.17e"), *bearing);
+}
--- /dev/null
+/*
+ * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file WgsConversion.h
+ */
+
+#pragma once
+#ifndef __WGSCONVERSION_H_
+#define __WGSCONVERSION_H_
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * Distance from the center of Earth to the equator in m (semi-major axis in WGS84 model).
+ */
+#define EARTH_RADIUS 6378137.0
+
+/**
+ * @brief Mean Earth gravity value in [m/s^2].
+ * Notice that, strictly speaking, the mean Earth gravity acceleration at surface
+ * level is negative; the positive value we measure with an accelerometer is the
+ * ground reaction which counters the gravity and prevents us from falling into the interior.
+ */
+#define EARTH_GRAVITY 9.80665
+
+/**
+ * Local frame coordinate system:
+ * - OX directed to the East;
+ * - OY directed to the North.
+ * - OZ directed to the up;
+ */
+
+typedef struct {
+ double latitude; /* Latitude of center of the local frame. */
+ double longitude; /* Longitude of center of the local frame. */
+ double mercator_scale;
+ double log_lat;
+} WgsConversion;
+
+extern void wgs_conversion_init(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
+ */
+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.
+ * @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.
+ * @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.
+ */
+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).
+ */
+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.
+ */
+extern void wgs_conversion_wgs_azimuth_to_local_bearing(const WgsConversion* self, double latitude, double longitude, double azimuth, double *bearing);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __WGSCONVERSION_H_ */
--- /dev/null
+/*
+ * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "math-ext.h"
+
+#define TWO_PI (2.0 * M_PI)
+
+double correct_angle(const double angle)
+{
+ /*
+ * Warning!
+ * remainder() and remquo() functions returns invalid result.
+ * Workaround below should work in any case.
+ */
+ double v = angle - ((int)(angle / TWO_PI + 0.5)) * TWO_PI;
+ if (v > M_PI) {
+ v -= TWO_PI;
+ }
+ if (v <= -M_PI) {
+ v += TWO_PI;
+ }
+ return v;
+/* return remainder(angle, 2.0 * M_PI); */
+}
#define fl_square(x) ({double __y__ = (x); __y__ * __y__;})
/**
- * @brief Mean Earth gravity value in [m/s^2].
- * Notice that, strictly speaking, the mean Earth gravity acceleration at surface
- * level is negative; the positive value we measure with an accelerometer is the
- * ground reaction which counters the gravity and prevents us from falling into the interior.
- * Also notice that given the inaccuracy (i.e. bias & miscalibration) of mobile device
- * acceleromenters at ~1 [m/s^2] the number of significant digits in the reference value below is way too many.
+ * @brief Returns angle corrected (mod 2 * PI) to fit in the range [-pi..pi].
+ * @param angle An angle to correct in [rad].
+ * @return angle in the range [-pi..pi].
*/
-#define EARTH_GRAVITY 9.80665
+extern double correct_angle(const double angle);
-#define EARTH_RADIUS 6371000.0
+#define to_radians(x) ((x) * (M_PI / 180.0))
+
+#define to_degrees(x) ((x) / (M_PI / 180.0))
#ifdef __cplusplus
}
/* Features */
-/** 0 = off, 1 = detach location source (WPS/GPS) when sleeping (immobile for at least 30s) */
-#define FL_MOTION_DETECTOR 0
-/** 0 = no WPS pausing in BALANCED mode, 1 = WPS pausing + regular predictions between measurements */
-#define FL_ACCEL_IN_BALANCED (0 & FL_MOTION_DETECTOR)
/** 0 = off, 1 = Kalman-merging of angular velocities from GPS and gyroscope. */
-#define FL_KALMAN_ANGULAR_VELOCITY 1
+#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.0
+#define FL_MIN_NOTIFICATION_INTERVAL 1
/** Maximum time in seconds from last input location for sending output position to clients */
#define FL_MAX_TIME_FROM_LAST_POS 300.0
/** Max time in seconds from last gps when pedometer skip may occur (filtration pedometer false positive in driving case) */
#define FL_DEFAULT_VELOCITY_SIGMA 1.0
/** [rad/s] Default standard deviation of measured angular velocity */
#define FL_DEFAULT_SPIN_SIGMA 0.2
-/** [m/s^2] Accelerometer noise level */
-#define FL_ACCEL_NOISE_LEVEL 0.25
-/** [Hz] Low-pass acceleration frequency threshold for estimation of gravity direction */
-#define FL_LP3D_CUTOFF_FREQUENCY 10
-/** [m/s^2] Upper level of IMMOBILE state */
-#define FL_MOTI_NOISE_LEVEL FL_ACCEL_NOISE_LEVEL
-/** [m/s^2] Lower level of MOVING state */
-#define FL_MOTI_MOTION_LEVEL (3*FL_ACCEL_NOISE_LEVEL)
-/** [s] Continuous immobility interval before transition to SLEEP state */
-#define FL_MOTI_IMMOBILITY_INTERVAL 90
-/** Log-2 of the AEMA averaging constant used for motion detection. */
-#define FL_MOTI_AEMA_PLATEAU_BITS 4
/** Log-2 of the AEMA averaging constant used for gyro drift suppression. */
#define FL_GYRO_AEMA_PLATEAU_BITS 14
/** [rad/s] Initial width of the Gaussian window for gyro bias determination */
#define FL_GYRO_SPIN_SIGMA_I FL_DEFAULT_SPIN_SIGMA
/** [rad/s] Final width of the Gaussian window for gyro bias determination */
#define FL_GYRO_SPIN_SIGMA_F 0.0004
-/** Log-2 of the AEMA averaging constant used for gravity value (accelerometer calibration). */
-#define FL_GRES_AEMA_PLATEAU_BITS 16
/** Log-2 of the AEMA averaging constant used for clock synchronization. */
#define FL_TIMO_AEMA_PLATEAU_BITS 10
Name: lbs-server
Summary: LBS Server for Tizen
-Version: 1.3.0
+Version: 1.3.1
Release: 1
Group: Location/Service
License: Apache-2.0
projects / platform / core / location / lbs-server.git / commitdiff