Compare commits
No commits in common. "9711f2e8200e168d935812709fba2af7c7c91aba" and "5a54a0864b2b4bf4273185e05ca47eabebf15be5" have entirely different histories.
9711f2e820
...
5a54a0864b
|
|
@ -1,575 +0,0 @@
|
|||
#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;
|
||||
}
|
||||
Loading…
Reference in New Issue