576 lines
19 KiB
C
576 lines
19 KiB
C
#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;
|
||
}
|