Merge remote-tracking branch 'origin/master'

APP/NavEstimator.c

@@ -0,0 +1,575 @@
+#ifndef NAV_ESTIMATOR_H
+#define NAV_ESTIMATOR_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <math.h>
+#include <string.h>
+
+#define useSatInfo
+
+#define NAV_DRIFT_HISTORY 20
+#define NAV_STD_IMU_WINDOW 20
+#define NAV_EARTH_RADIUS_M 6371000.0
+#define NAV_DEG_TO_RAD (M_PI / 180.0)  // Константа для преобразования градусов в радианы
+
+#define STATIC_IMU_THRESHOLD 0.01
+#define LOOP_THRESHOLD 100.0
+#define HDOP_THRESHOLD 5
+#define PDOP_THRESHOLD 6
+
+#ifdef useSatInfo
+
+#define SAT_MAX_NUMBER (20)
+typedef struct {
+    int id;
+    int in_use;
+    int elv;
+    int azimuth;
+    int sig;
+} tSatellite;
+
+typedef struct {
+    int inuse;
+    int inview;
+    tSatellite sat[SAT_MAX_NUMBER];
+} tSatellitesInfo;
+#endif
+
+typedef enum {
+    Quality_Invalid = 0,
+    Quality_Fix,
+    Quality_Differential,
+    Quality_Sensitive
+} eGpsQuality;
+
+typedef enum {
+    FixNotAvailable = 0,
+    Fix_Differential,
+    Fix_2D,
+    Fix_3D
+} eFixType;
+
+typedef struct {
+    double latitude;
+    double longitude;
+}tNavPosition;
+
+typedef struct {
+    unsigned long timestamp;
+
+    eGpsQuality quality;
+    eFixType fix;
+
+    double PDOP;
+    double HDOP;
+    double VDOP;
+
+    double latitude;
+    double longitude;
+    double elv;
+    double speed;
+    double direction;
+    double declination;
+#ifdef useSatInfo
+    tSatellitesInfo satinfo;
+#endif
+} tNavigationData;
+
+typedef struct {
+    double x;
+    double y;
+    double z;
+} t3DVectorData;
+
+typedef struct {
+    t3DVectorData accelerometer;
+    t3DVectorData gyroscope;
+    t3DVectorData magnetometer;
+} tIMU;
+
+typedef struct {
+    tNavigationData history[NAV_DRIFT_HISTORY];
+    int head;
+    int count;
+} tDriftBuffer;
+
+typedef struct {
+    t3DVectorData accelHistory[NAV_STD_IMU_WINDOW];
+    int accelHead;
+    int accelCount;
+} tIMUVarianceBuffer;
+
+typedef struct {
+    double x;
+    double vx;
+    double ax;
+    double y;
+    double vy;
+    double ay;
+    double bias1;
+    double bias2;
+} tKalmanState2D;
+
+typedef struct {
+    double P[6][6];
+    double Q[6]; // Process noise
+    double R[2]; // Measurement noise (lat/lon)
+} tKalmanFilter2D;
+
+typedef struct {
+    tNavigationData lastNav;
+    bool isNavDataUpdate;
+    tIMU lastIMU;
+    bool isIMUUpdate;
+
+    tDriftBuffer drift;
+    tIMUVarianceBuffer imuStats;
+
+    tKalmanState2D kalmanState;
+    tKalmanFilter2D kalmanFilter;
+    unsigned long lastKalmanUpdate;
+
+    double confidence;
+} tNavEstimator;
+
+
+//public
+//  init
+void NavEstimator_Init(tNavEstimator* ctx);
+
+//  loop
+void NavEstimator_Filter(tNavEstimator* ctx);
+
+//  setters
+void NavEstimator_SetNav(tNavEstimator* ctx, const tNavigationData* nav);
+void NavEstimator_SetIMU(tNavEstimator* ctx, const tIMU* imu);
+
+//  getters
+double NavEstimator_GetConfidence(const tNavEstimator* ctx);
+void NavEstimator_GetPosition(const tNavEstimator* ctx, double* lat,double* lon);
+
+//private
+bool NavEstimator_IsLoopDetected(const tDriftBuffer* buf);
+bool NavEstimator_IsStaticIMU(const tIMUVarianceBuffer* buf);
+double NavEstimator_CalcIMUVariance(const tIMUVarianceBuffer* buf);
+double NavEstimator_Haversine(double lat1, double lon1, double lat2, double lon2);
+void NavEstimator_KalmanPredict(tNavEstimator* ctx, double dt);
+void NavEstimator_KalmanUpdate(tNavEstimator* ctx, double measuredLat, double measuredLon);
+double NavEstimator_KalmanInnovationNorm(const tNavEstimator* ctx, double measuredLat, double measuredLon);
+void NavEstimator_KalmanUpdateMatrix(tKalmanFilter2D* kf, tKalmanState2D* state, double measuredLat, double measuredLon);
+
+#endif // NAV_ESTIMATOR_H
+
+extern unsigned long SystemGetMs();
+
+void NavEstimator_Init(tNavEstimator* ctx)
+{
+
+    if (!ctx) return;
+
+    // Обнуляем все поля структуры
+    memset(ctx, 0, sizeof(tNavEstimator));
+
+    // Инициализируем кольцевой буфер дрейфа
+    ctx->drift.head = 0;
+    ctx->drift.count = 0;
+
+    // Инициализируем буфер статистики IMU
+    ctx->imuStats.accelHead = 0;
+    ctx->imuStats.accelCount = 0;
+
+    // Инициализируем состояние Калмана
+    tKalmanState2D* state = &ctx->kalmanState;
+    state->x = 0;
+    state->vx = 0;
+    state->ax = 0;
+    state->y = 0;
+    state->vy = 0;
+    state->ay = 0;
+
+    // Инициализация матрицы ковариаций ошибок
+    tKalmanFilter2D* kf = &ctx->kalmanFilter;
+    memset(kf->P, 0, sizeof(kf->P));
+    for (int i = 0; i < 6; ++i) {
+        kf->P[i][i] = 1.0; // начальная неопределенность
+    }
+
+    // Настройка шумов процесса (можно подбирать)
+    for (int i = 0; i < 6; ++i) {
+        kf->Q[i] = 1e-3;
+    }
+
+    // Настройка шумов измерений (широта/долгота)
+    kf->R[0] = 1e-4; // лат
+    kf->R[1] = 1e-4; // лон
+
+    ctx->confidence = 0.0;
+}
+
+void NavEstimator_SetNav(tNavEstimator* ctx, const tNavigationData* nav)
+{
+    if (!ctx || !nav) return;
+
+    // Если буфер полон, сдвигаем его вперед
+    if (ctx->drift.count == NAV_DRIFT_HISTORY) {
+        ctx->drift.head = (ctx->drift.head + 1) % NAV_DRIFT_HISTORY;
+    } else {
+        ctx->drift.count++;
+    }
+
+    // Записываем новые данные в буфер
+    ctx->drift.history[(ctx->drift.head + ctx->drift.count - 1) % NAV_DRIFT_HISTORY] = *nav;
+
+    // Обновляем последние полученные данные
+    ctx->lastNav = *nav;
+
+    // Выставляем флаг обновления навигации
+    ctx->isNavDataUpdate = true;
+}
+
+void NavEstimator_SetIMU(tNavEstimator* ctx, const tIMU* imu)
+{
+    if (!ctx || !imu) return;
+
+    // Если буфер полон, сдвигаем его вперед
+    if (ctx->imuStats.accelCount == NAV_STD_IMU_WINDOW) {
+        ctx->imuStats.accelHead = (ctx->imuStats.accelHead + 1) % NAV_STD_IMU_WINDOW;
+    } else {
+        ctx->imuStats.accelCount++;
+    }
+
+    // Записываем новые данные в буфер
+    ctx->imuStats.accelHistory[(ctx->imuStats.accelHead + ctx->imuStats.accelCount - 1) % NAV_STD_IMU_WINDOW] = imu->accelerometer;
+
+    // Обновление последних данных IMU
+    ctx->lastIMU = *imu;
+
+    // Пример дальнейших действий:
+    // Вычисление статистики для выявления аномального дрейфа
+    double variance = NavEstimator_CalcIMUVariance(&ctx->imuStats);
+
+    // Например, можно обновить состояние фильтра Калмана или принять решение
+    // о стабильности IMU (например, если variance слишком высок, это может
+    // указывать на нестабильность)
+    ctx->isIMUUpdate = true;
+}
+
+double NavEstimator_GetConfidence(const tNavEstimator* ctx)
+{
+    if (!ctx) return 0.0;  // Возвращаем 0, если контекст не задан
+
+    double confidence = 0.0;
+
+    // Смотрим в буфере статус каждого измерения, если координаты были фиксированы,
+    // увеличиваем доверие, иначе уменьшаем
+    if (ctx->drift.count == NAV_DRIFT_HISTORY) {
+        for(int i = NAV_DRIFT_HISTORY - 1; i >= 0; i--) {
+            confidence += ctx->drift.history->quality == Quality_Fix ? 0.005 : -0.005;
+            confidence += ctx->drift.history->HDOP > HDOP_THRESHOLD ? 0.005 : -0.005;
+            confidence += ctx->drift.history->PDOP > PDOP_THRESHOLD ? 0.005 : -0.005;
+        }
+    }
+
+    // Если обнаружили зацикленность маршрута, доверие снижается
+    if(NavEstimator_IsLoopDetected(&ctx->drift)){
+        confidence -= 0.3;
+    }
+    // Дополнительно учитываем данные с IMU
+    if(NavEstimator_IsStaticIMU(&ctx->imuStats)){
+        confidence += 0.1;
+    }
+
+    if (ctx->imuStats.accelCount > 0) {
+        double imuVariance = NavEstimator_CalcIMUVariance(&ctx->imuStats);
+
+        // Если IMU стабильное (дисперсия мала), увеличиваем доверие
+        if (imuVariance < 0.1) {
+            confidence += 0.05;  // Добавляем 0.2, если IMU стабильный
+        }
+            // Если IMU нестабильное (дисперсия велика), уменьшаем доверие
+        else if (imuVariance > 1.0) {
+            confidence -= 0.05;  // Уменьшаем доверие
+        }
+    }
+
+    // Усредняем с имеющимся в контексте значением доверия
+    confidence += ctx->confidence;
+    confidence /= 2;
+
+    // Ограничиваем доверие от 0 до 1
+    if (confidence > 1.0) {
+        confidence = 1.0;
+    } else if (confidence < 0.0) {
+        confidence = 0.0;
+    }
+
+    return confidence;
+}
+
+bool NavEstimator_IsLoopDetected(const tDriftBuffer* buf)
+{
+    if (!buf || buf->count < 2) {
+        // Если буфер пуст или в нем меньше 2-х записей, петля не может быть обнаружена
+        return false;
+    }
+
+    // Определяем максимально допустимый радиус для "петли" (например, 100 метров)
+    const double loopThreshold = LOOP_THRESHOLD;  // метров
+
+    // Получаем последние две позиции из буфера
+    const tNavigationData* lastNav = &buf->history[(buf->head - 1 + NAV_DRIFT_HISTORY) % NAV_DRIFT_HISTORY];
+    const tNavigationData* secondLastNav = &buf->history[(buf->head - 2 + NAV_DRIFT_HISTORY) % NAV_DRIFT_HISTORY];
+
+    // Вычисляем расстояние между последними двумя позициями с помощью Haversine формулы
+    double distance = NavEstimator_Haversine(
+            secondLastNav->latitude, secondLastNav->longitude,
+            lastNav->latitude, lastNav->longitude
+    );
+
+    // Если расстояние между последними двумя точками меньше порогового значения (100 метров), это может быть петля
+    return distance < loopThreshold;
+}
+
+bool NavEstimator_IsStaticIMU(const tIMUVarianceBuffer* buf)
+{
+    if (!buf || buf->accelCount < NAV_STD_IMU_WINDOW) {
+        // Если буфер пуст или в нем недостаточно данных для статистики
+        return false;
+    }
+
+    // Рассчитываем дисперсию для каждого из векторов акселерометра
+    double accelVarianceX = 0.0;
+    double accelVarianceY = 0.0;
+    double accelVarianceZ = 0.0;
+
+    // Вычисляем среднее значение для каждого из каналов акселерометра
+    double meanX = 0.0, meanY = 0.0, meanZ = 0.0;
+    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
+        meanX += buf->accelHistory[i].x;
+        meanY += buf->accelHistory[i].y;
+        meanZ += buf->accelHistory[i].z;
+    }
+    meanX /= NAV_STD_IMU_WINDOW;
+    meanY /= NAV_STD_IMU_WINDOW;
+    meanZ /= NAV_STD_IMU_WINDOW;
+
+    // Вычисляем дисперсию для каждого канала
+    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
+        accelVarianceX += pow(buf->accelHistory[i].x - meanX, 2);
+        accelVarianceY += pow(buf->accelHistory[i].y - meanY, 2);
+        accelVarianceZ += pow(buf->accelHistory[i].z - meanZ, 2);
+    }
+    accelVarianceX /= NAV_STD_IMU_WINDOW;
+    accelVarianceY /= NAV_STD_IMU_WINDOW;
+    accelVarianceZ /= NAV_STD_IMU_WINDOW;
+
+    // Рассчитываем общую дисперсию для акселерометра
+    double totalVariance = (accelVarianceX + accelVarianceY + accelVarianceZ) / 3.0;
+
+    // Порог для статичного состояния (например, если дисперсия меньше 0.01)
+    const double staticThreshold = STATIC_IMU_THRESHOLD;
+
+    // Если общая дисперсия ниже порога, значит, устройство, вероятно, стоит на месте
+    return totalVariance < staticThreshold;
+}
+
+double NavEstimator_CalcIMUVariance(const tIMUVarianceBuffer* buf)
+{
+    if (!buf || buf->accelCount < NAV_STD_IMU_WINDOW) {
+        // Если буфер пуст или в нем недостаточно данных для статистики
+        return -1.0; // Вернем -1 для обозначения ошибки
+    }
+
+    // Рассчитываем среднее значение для каждого из каналов акселерометра (X, Y, Z)
+    double meanX = 0.0, meanY = 0.0, meanZ = 0.0;
+
+    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
+        meanX += buf->accelHistory[i].x;
+        meanY += buf->accelHistory[i].y;
+        meanZ += buf->accelHistory[i].z;
+    }
+
+    meanX /= NAV_STD_IMU_WINDOW;
+    meanY /= NAV_STD_IMU_WINDOW;
+    meanZ /= NAV_STD_IMU_WINDOW;
+
+    // Рассчитываем дисперсию для каждого канала (X, Y, Z)
+    double varianceX = 0.0, varianceY = 0.0, varianceZ = 0.0;
+
+    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
+        varianceX += pow(buf->accelHistory[i].x - meanX, 2);
+        varianceY += pow(buf->accelHistory[i].y - meanY, 2);
+        varianceZ += pow(buf->accelHistory[i].z - meanZ, 2);
+    }
+
+    // Получаем среднюю дисперсию для всех трех каналов
+    varianceX /= NAV_STD_IMU_WINDOW;
+    varianceY /= NAV_STD_IMU_WINDOW;
+    varianceZ /= NAV_STD_IMU_WINDOW;
+
+    // Возвращаем среднюю дисперсию по всем каналам
+    return (varianceX + varianceY + varianceZ) / 3.0;
+}
+
+double NavEstimator_Haversine(double lat1, double lon1, double lat2, double lon2)
+{
+    // Переводим координаты из градусов в радианы
+    lat1 *= NAV_DEG_TO_RAD;
+    lon1 *= NAV_DEG_TO_RAD;
+    lat2 *= NAV_DEG_TO_RAD;
+    lon2 *= NAV_DEG_TO_RAD;
+
+    // Разница между широтами и долготами
+    double dlat = lat2 - lat1;
+    double dlon = lon2 - lon1;
+
+    // Применяем формулу Хаверсина
+    double a = sin(dlat / 2) * sin(dlat / 2) +
+               cos(lat1) * cos(lat2) * sin(dlon / 2) * sin(dlon / 2);
+    double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+    // Радиус Земли в метрах
+    double R = NAV_EARTH_RADIUS_M;
+
+    // Возвращаем расстояние между точками в метрах
+    return R * c;
+}
+
+
+void NavEstimator_Filter(tNavEstimator* ctx)
+{
+    if(!ctx) return;
+    if((ctx->isNavDataUpdate == false) &&(ctx->isIMUUpdate == false)) return;
+
+    ctx->isNavDataUpdate = false;
+    ctx->isIMUUpdate = false;
+
+    unsigned long dt = 0;
+    if(ctx->lastKalmanUpdate != 0) dt = SystemGetMs() - ctx->lastKalmanUpdate;
+
+    NavEstimator_KalmanPredict(ctx,dt);
+    NavEstimator_KalmanUpdate(ctx, ctx->lastNav.latitude, ctx->lastNav.latitude);
+}
+
+
+void NavEstimator_KalmanPredict(tNavEstimator* ctx, double dt)
+{
+    if (!ctx) return;
+
+    // Прогнозирование позиции и скорости по фильтру Калмана
+    tKalmanState2D* state = &ctx->kalmanState;
+
+    // Обновляем положение с учетом скорости и ускорения
+    state->x += state->vx * dt + 0.5 * state->ax * dt * dt;  // X = X + Vx * dt + 0.5 * Ax * dt^2
+    state->y += state->vy * dt + 0.5 * state->ay * dt * dt;  // Y = Y + Vy * dt + 0.5 * Ay * dt^2
+    state->vx += state->ax * dt;  // Vx = Vx + Ax * dt
+    state->vy += state->ay * dt;  // Vy = Vy + Ay * dt
+
+    // Прогнозируем погрешности (обновляем матрицу P)
+    // Допустим, что процессное шумовое распределение (Q) и матрица ковариаций обновляются по времени
+    for (int i = 0; i < 6; ++i) {
+        ctx->kalmanFilter.P[i][i] += ctx->kalmanFilter.Q[i] * dt;
+    }
+}
+
+void NavEstimator_KalmanUpdate(tNavEstimator* ctx, double measuredLat, double measuredLon)
+{
+    if (!ctx) return;
+
+    // Измерения
+    double measured[2] = { measuredLat, measuredLon };
+
+    // Калькуляция инновативной нормы (разница между предсказанными и измеренными значениями)
+    double innovationNorm = NavEstimator_KalmanInnovationNorm(ctx, measured[0], measured[1]);
+
+    // Обновление состояния с помощью фильтра Калмана
+    NavEstimator_KalmanUpdateMatrix(&ctx->kalmanFilter, &ctx->kalmanState, measured[0], measured[1]);
+
+    // Обновляем уверенность в данных на основе инновативной нормы
+    if (innovationNorm < 1.0) {
+        ctx->confidence = 1.0;
+    } else {
+        ctx->confidence = 0.5;
+    }
+}
+
+void NavEstimator_KalmanUpdateMatrix(tKalmanFilter2D* kf, tKalmanState2D* state, double measuredLat, double measuredLon)
+{
+    // Ошибка измерения (инновация)
+    double y[2] = {
+            measuredLat - state->x,
+            measuredLon - state->y
+    };
+
+    // S = H * P * H^T + R
+    double S[2][2] = {
+            {kf->P[0][0] + kf->R[0], kf->P[0][1]},
+            {kf->P[1][0], kf->P[1][1] + kf->R[1]}
+    };
+
+    // Вычисление обратной матрицы S (2x2)
+    double detS = S[0][0] * S[1][1] - S[0][1] * S[1][0];
+    if (fabs(detS) < 1e-8) return;  // сингулярная матрица
+
+    double invS[2][2] = {
+            { S[1][1] / detS, -S[0][1] / detS },
+            { -S[1][0] / detS, S[0][0] / detS }
+    };
+
+    // K = P * H^T * inv(S)
+    double K[6][2];
+    for (int i = 0; i < 6; ++i) {
+        K[i][0] = kf->P[i][0] * invS[0][0] + kf->P[i][1] * invS[1][0];
+        K[i][1] = kf->P[i][0] * invS[0][1] + kf->P[i][1] * invS[1][1];
+    }
+
+    // Обновление состояния: x = x + K * y
+    state->x += K[0][0] * y[0] + K[0][1] * y[1];
+    state->y += K[1][0] * y[0] + K[1][1] * y[1];
+    state->vx += K[2][0] * y[0] + K[2][1] * y[1];
+    state->vy += K[3][0] * y[0] + K[3][1] * y[1];
+    state->bias1 += K[4][0] * y[0] + K[4][1] * y[1];
+    state->bias2 += K[5][0] * y[0] + K[5][1] * y[1];
+
+    // Обновление ковариационной матрицы: P = (I - K * H) * P
+    // H — 2x6: [1 0 0 0 0 0; 0 1 0 0 0 0]
+    double KH[6][6] = {0};
+    for (int i = 0; i < 6; ++i) {
+        KH[i][0] = K[i][0];  // K * H: только два столбца
+        KH[i][1] = K[i][1];
+    }
+
+    double I_KH[6][6];
+    for (int i = 0; i < 6; ++i)
+        for (int j = 0; j < 6; ++j)
+            I_KH[i][j] = (i == j ? 1.0 : 0.0) - KH[i][j];
+
+    // P = (I - K * H) * P
+    double newP[6][6] = {0};
+    for (int i = 0; i < 6; ++i)
+        for (int j = 0; j < 6; ++j)
+            for (int k = 0; k < 6; ++k)
+                newP[i][j] += I_KH[i][k] * kf->P[k][j];
+
+    // Копируем обратно
+    for (int i = 0; i < 6; ++i)
+        for (int j = 0; j < 6; ++j)
+            kf->P[i][j] = newP[i][j];
+}
+
+double NavEstimator_KalmanInnovationNorm(const tNavEstimator* ctx, double measuredLat, double measuredLon)
+{
+    if (!ctx) return -1.0;
+
+    double predictLat = ctx->kalmanState.x;
+    double predictLon =  ctx->kalmanState.y;
+
+    return NavEstimator_Haversine(predictLat,predictLon,measuredLat,measuredLon);
+}
+
+void NavEstimator_GetPosition(const tNavEstimator* ctx, double* lat,double* lon)
+{
+    *lat = ctx->kalmanState.x;
+    *lon = ctx->kalmanState.y;
+}