From 6500e97ee9a9872ce5e425a257010225f75476fe Mon Sep 17 00:00:00 2001 From: Villuton Date: Wed, 14 May 2025 02:18:05 +0300 Subject: [PATCH] fix --- APP/NavEstimator.c | 575 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 575 insertions(+) create mode 100644 APP/NavEstimator.c diff --git a/APP/NavEstimator.c b/APP/NavEstimator.c new file mode 100644 index 0000000..36a3f05 --- /dev/null +++ b/APP/NavEstimator.c @@ -0,0 +1,575 @@ +#ifndef NAV_ESTIMATOR_H +#define NAV_ESTIMATOR_H + +#include +#include +#include +#include + +#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; +}