Villuton
/
WeAct_V1_Artery_AT32F403A
Template
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix

This commit is contained in:
Степанов Станислав 2025-05-14 02:18:05 +03:00
parent d11676189e
commit 6500e97ee9
1 changed files with 575 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

575
APP/NavEstimator.c Normal file
View File

@ -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;
}