Fusion and extraction

C/.gitignore

@@ -0,0 +1,3 @@
+*.pro.user
+*.iml
+.DS_Store

C/include/Commons.h

@@ -0,0 +1,54 @@
+#ifndef COMMONS_H
+#define COMMONS_H
+
+#include <math.h>
+#include <stdlib.h>
+#include <assert.h>
+
+#ifdef __cplusplus
+#define restrict __restrict__
+#endif
+
+#define UNUSED_ARG(x) ((void)x)
+
+inline double Degree2Rad(double degree) {
+  static const double coef = M_PI/180.0;
+  return degree*coef;
+}
+//////////////////////////////////////////////////////////////////////////
+
+inline double Rad2Degree(double rad) {
+  static const double coef = 180.0/M_PI;
+  return rad*coef;
+}
+//////////////////////////////////////////////////////////////////////////
+
+inline void SwapPtrs(void **a, void **b) {
+  void *tmp = *a;
+  *a = *b;
+  *b = tmp;
+}
+//////////////////////////////////////////////////////////////////////////
+
+inline void SwapDoubles(double *a, double *b) {
+  double tmp = *a;
+  *a = *b;
+  *b = tmp;
+}
+//////////////////////////////////////////////////////////////////////////
+
+inline double MilesPerHour2MeterPerSecond(double mph) {
+  return 2.23694 * mph;
+}
+//////////////////////////////////////////////////////////////////////////
+
+inline int RandomBetween2Vals(int low, int hi) {
+  assert(low <= hi);
+  return (rand() % (hi - low)) + low;
+}
+
+inline double LowPassFilter(double prev, double measured, double alpha) {
+  return prev + alpha * (measured - prev);
+}
+
+#endif // COMMONS_H

C/include/Coordinates.h

@@ -0,0 +1,34 @@
+#ifndef COORDINATES_H
+#define COORDINATES_H
+
+#include <vector>
+class QString;
+
+#define COORDINATES_HIGH_ACCURACY 0
+
+struct geopoint_t {
+  double Latitude, Longitude;
+  geopoint_t() : Latitude(0.0), Longitude(0.0) {
+  }
+  geopoint_t(double lat, double lon) : Latitude(lat), Longitude(lon) {
+  }
+};
+
+std::vector<geopoint_t> CoordGetFromFile(const QString& filePath, int interested);
+std::vector<geopoint_t> CoordFilterByGeoHash(const std::vector<geopoint_t> &lstSrc,
+                                             int precision,
+                                             int minPointCount);
+
+#define EARTH_RADIUS (6371.0 * 1000.0) // meters
+#define ACTUAL_GRAVITY 9.80665
+
+/*todo write tests*/
+double CoordDistanceBetweenPointsMeters(double lat1, double lon1, double lat2, double lon2);
+double CoordLongitudeToMeters(double lon);
+double CoordLatitudeToMeters(double lat);
+geopoint_t CoordMetersToGeopoint(double lonMeters,
+                            double latMeters);
+
+double CoordCaclulateDistance(const std::vector<geopoint_t> &lst);
+
+#endif // COORDINATES_H

C/include/GPSAccKalman.h

@@ -0,0 +1,31 @@
+#ifndef GPSACCKALMAN_H
+#define GPSACCKALMAN_H
+
+#include <stdint.h>
+#include "Kalman.h"
+#include "Matrix.h"
+
+typedef struct GPSAccKalmanFilter {
+  double predictTime;
+  double updateTime;
+  double accDev;
+  uint32_t predictCount;
+  KalmanFilter_t *kf;
+} GPSAccKalmanFilter_t;
+
+GPSAccKalmanFilter_t* GPSAccKalmanAlloc(double x, double y,
+    double xVel, double yVel,
+    double accDev, double posDev,
+    double timeStamp);
+
+void GPSAccKalmanFree(GPSAccKalmanFilter_t *k);
+
+void GPSAccKalmanPredict(GPSAccKalmanFilter_t *k, double timeNow, double xAcc, double yAcc);
+void GPSAccKalmanUpdate(GPSAccKalmanFilter_t *k, double timeStamp, double x, double y, double xVel, double yVel, double posDev);
+
+double GPSAccKalmanGetX(const GPSAccKalmanFilter_t *k);
+double GPSAccKalmanGetY(const GPSAccKalmanFilter_t *k);
+double GPSAccKalmanGetXVel(const GPSAccKalmanFilter_t *k);
+double GPSAccKalmanGetYVel(const GPSAccKalmanFilter_t *k);
+
+#endif // GPSACCKALMAN_H

C/include/Geohash.h

@@ -0,0 +1,10 @@
+#ifndef GEOHASH_H
+#define GEOHASH_H
+
+#include <stdint.h>
+static const int GEOHASH_MAX_PRECISION = 12;
+uint64_t GeohashEncodeU64(double lat, double lon, int prec);
+int GeohashComparePointsU64(double lon1, double lat1, double lon2, double lat2, int precision);
+
+
+#endif // GEOHASH_H

C/include/Kalman.h

@@ -0,0 +1,44 @@
+#ifndef KALMAN_H
+#define KALMAN_H
+
+typedef struct matrix matrix_t;
+
+typedef struct KalmanFilter {
+  /*these matrices should be provided by user*/
+  matrix_t *F; //state transition model
+  matrix_t *H; //observation model
+  matrix_t *B; //control matrix
+  matrix_t *Q; //process noise covariance
+  matrix_t *R; //observation noise covariance
+
+  /*these matrices will be updated by user*/
+  matrix_t *Uk; //control vector
+  matrix_t *Zk; //actual values (measured)
+  matrix_t *Xk_km1; //predicted state estimate
+  matrix_t *Pk_km1; //predicted estimate covariance
+  matrix_t *Yk; //measurement innovation
+
+  matrix_t *Sk; //innovation covariance
+  matrix_t *SkInv; //innovation covariance inverse
+
+  matrix_t *K; //Kalman gain (optimal)
+  matrix_t *Xk_k; //updated (current) state
+  matrix_t *Pk_k; //updated estimate covariance
+  matrix_t *Yk_k; //post fit residual
+
+  /*auxiliary matrices*/
+  matrix_t *auxBxU;
+  matrix_t *auxSDxSD;
+  matrix_t *auxSDxMD;
+
+} KalmanFilter_t;
+
+KalmanFilter_t *KalmanFilterCreate(int stateDimension,
+                                   int measureDimension,
+                                   int controlDimension);
+void KalmanFilterFree(KalmanFilter_t *k);
+
+void KalmanFilterPredict(KalmanFilter_t *k);
+void KalmanFilterUpdate(KalmanFilter_t *k);
+
+#endif // KALMAN_H

C/include/MadgwickAHRS.h

@@ -0,0 +1,49 @@
+//=====================================================================================================
+// MadgwickAHRS.h
+//=====================================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+//=====================================================================================================
+#ifndef MadgwickAHRS_h
+#define MadgwickAHRS_h
+
+//----------------------------------------------------------------------------------------------------
+// Variable declaration
+
+typedef struct quaternion {
+  float w, x, y, z;
+} quaternion_t;
+
+typedef struct MadgwickFilterAlloc {
+  float beta;             //algorithm gain
+  float sampleFreq;
+  float q0, q1, q2, q3;   //quaternion of sensor frame relative to auxiliary frame
+} MadgwickFilter_t;
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+MadgwickFilter_t* MadgwickFilterAlloc(float beta, float sampleFreqHZ);
+void MadgwickFilterFree(MadgwickFilter_t *f);
+void MadgwickAHRSupdate(MadgwickFilter_t *f,
+                        float gx, float gy, float gz,
+                        float ax, float ay, float az,
+                        float mx, float my, float mz);
+
+void MadgwickAHRSupdateIMU( MadgwickFilter_t *f,
+                            float gx, float gy, float gz,
+                            float ax, float ay, float az);
+
+void MadgwickRotationMatrix (MadgwickFilter_t *mf,
+                              float *mtx);
+
+#endif
+//=====================================================================================================
+// End of file
+//=====================================================================================================

C/include/Matrix.h

@@ -0,0 +1,73 @@
+#ifndef MATRIX_H
+#define MATRIX_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "Commons.h"
+
+typedef struct matrix {
+  double **data;
+
+  uint32_t rows;
+  uint32_t cols;
+  //seems aligned, right?
+} matrix_t;
+
+matrix_t* MatrixAlloc(uint32_t rows, uint32_t cols);
+void MatrixFree(matrix_t *m);
+
+/* Set values of a matrix, row by row. */
+void MatrixSet(matrix_t *m, ...);
+
+/* Copy a matrix. */
+void MatrixCopy(matrix_t *src,
+                matrix_t *dst);
+
+/* Pretty-print a matrix. */
+void MatrixPrint(matrix_t *m);
+
+/* Add matrices a and b and put the result in c. */
+void MatrixAdd(matrix_t *ma,
+               matrix_t *mb,
+               matrix_t *mc);
+
+/* Subtract matrices a and b and put the result in c. */
+void MatrixSubtract(matrix_t *ma,
+                    matrix_t *mb,
+                    matrix_t *mc);
+
+/* Subtract from the identity matrix in place. */
+void MatrixSubtractFromIdentity(matrix_t *m);
+
+/* Multiply matrices a and b and put the result in c. */
+void MatrixMultiply( matrix_t * restrict ma,
+                     matrix_t * restrict mb,
+                     matrix_t * restrict mc);
+
+/* Multiply matrix a by b-transpose and put the result in c. */
+void MatrixMultiplyByTranspose(matrix_t *ma,
+                               matrix_t *mb,
+                               matrix_t *mc);
+/* Transpose matrix*/
+void MatrixTranspose(matrix_t *mtxin,
+                     matrix_t *mtxout);
+
+/* Whether two matrices are approximately equal. */
+bool MatrixEq(matrix_t *a,
+              matrix_t *b,
+              double tolerance);
+
+/* Multiply a matrix by a scalar. */
+void MatrixScale(matrix_t *m, double scalar);
+
+/* Invert a square matrix.
+   Returns whether the matrix is invertible.
+   input is mutated as well by this routine. */
+bool MatrixDestructiveInvert(matrix_t *input,
+                             matrix_t *output);
+
+/* Turn m into an identity matrix. */
+void MatrixSetIdentity(matrix_t *m);
+
+void MatrixSetIdentityDiag(matrix_t *m);
+#endif // MATRIX_H

C/include/MeanFilter.h

@@ -0,0 +1,78 @@
+#ifndef MEANFILTER_H
+#define MEANFILTER_H
+
+//todo rewrite using C style
+
+#include <stdint.h>
+#include <vector>
+#include <deque>
+#include <assert.h>
+#include <math.h>
+
+static const double DEFAULT_TIME_WINDOW = 0.2;
+template<typename T> class CMeanFilter {
+private:
+
+  uint32_t m_startTimeUs;
+  uint32_t m_timeStampUs;
+  uint32_t m_count;
+  double   m_timeWindow;
+  std::deque<std::vector<T> > m_deckWindow;
+
+  void mean(std::vector<T> &meanBuff) const {
+    for (auto i = m_deckWindow.cbegin(); i != m_deckWindow.cend(); ++i) {
+      auto j1 = i->cbegin();
+      auto j2 = meanBuff.begin();
+      for (; j1 != i->cend(); ++j1, ++j2) {
+        *j2 += *j1;
+      }
+    }
+
+    for (auto j2 = meanBuff.begin(); j2 != meanBuff.end(); ++j2) {
+      *j2 /= meanBuff.size();
+    }
+  }
+
+public:
+
+  CMeanFilter() :
+    m_startTimeUs(0),
+    m_timeStampUs(0),
+    m_count(0),
+    m_timeWindow(DEFAULT_TIME_WINDOW) {
+
+  }
+
+  void filter(T val,
+              std::vector<T> &meanBuff,
+              uint32_t timeStampUs) {
+    std::vector<T> tmp;
+    tmp.push_back(val);
+    filterArr(tmp, meanBuff, timeStampUs);
+  }
+
+  void filterArr(const std::vector<T> &data,
+              std::vector<T> &meanBuff,
+              uint32_t timeStampUs) {
+    assert(data.size() == meanBuff.size());
+    m_timeStampUs = timeStampUs;
+    ++m_count;
+    if (m_startTimeUs == 0UL) {
+      m_startTimeUs = timeStampUs;
+      auto i = meanBuff.begin();
+      auto j = data.cbegin();
+      for (; i != meanBuff.end(); ++i, ++j)
+        *i = *j;      
+      return;
+    }
+
+    double hz = m_count / ((m_timeStampUs - m_startTimeUs) / 1.0e3); //herz frequency
+    uint32_t fw = ceil(hz * m_timeWindow); //filter window
+    m_deckWindow.push_back(data);
+    while (m_deckWindow.size() > fw)
+      m_deckWindow.pop_front();
+    mean(meanBuff);
+  }
+};
+
+#endif // MEANFILTER_H