Fundamentos, modelos matemáticos e “mock” de GPS
Para implementar algoritmos de trigonometria esférica em sistemas embarcados, começamos pelo alicerce: sistema de coordenadas, conversões angulares, constantes do modelo terrestre e uma função que simula (mock) a leitura de um GPS. A partir disso, construiremos, nas próximas seções, funções para distância, rumo (bearing), ponto de destino, e, quando necessário, passaremos do modelo esférico ao elipsoidal (WGS‑84) e às conversões ECEF/ENU.
Sistemas de coordenadas e convenções
- Coordenadas geodésicas (WGS‑84): latitude φ (N/S), longitude λ (E/W) e altitude h.
- Unidade angular: internamente usaremos radianos [mais estável numericamente).
Conversões: \(\text{rad} = \text{deg}\cdot\frac{\pi}{180},\qquad \text{deg} = \text{rad}\cdot\frac{180}{\pi}\] - Modelo esférico (para navegação geral): raio médio \(R \approx 6\,371\,008.8\ \text{m}.\)
- Modelo elipsoidal WGS‑84 (para maior precisão): semieixo maior \(a=6\,378\,137.0\ \text{m},\) achatamento f=1/298.257223563, semieixo menor b=a(1-f). (Usaremos este mais adiante quando necessário.)
Distância angular e distância de arco (visão conceitual)
Entre dois pontos \(P_1(\varphi_1,\lambda_1)\) e \(P_2(\varphi_2,\lambda_2)\), a distância angular θ\theta na esfera é dada pela lei dos cossenos esférica: \[\cos\theta=\sin\varphi_1\sin\varphi_2+\cos\varphi_1\cos\varphi_2\cos(\Delta\lambda),\quad \Delta\lambda=\lambda_2-\lambda_1\]
A distância de arco (superfície): \[d=R\cdot\theta\]
Mais adiante implementaremos também a haversine, que é numericamente estável para curtas distâncias: \[\operatorname{hav}(\theta)=\operatorname{hav}(\varphi_2-\varphi_1)+\cos\varphi_1\cos\varphi_2\ \operatorname{hav}(\lambda_2-\lambda_1)\]
onde \(\operatorname{hav}(x)=\sin^2(x/2)\).
Tipos, utilitários matemáticos e “mock” de GPS
Abaixo, criamos uma base para o projeto: tipos fortes para coordenadas, utilitários (deg↔rad, “clamp”, normalização de ângulos) e uma função que simula leituras do GPS com jitter controlado (útil para testes em bancada).
/* =========================================================================
* gps_spherical_basics.h
* Fundamentos para algoritmos de trigonometria esférica com dados de GPS
* Autor: (você)
* ========================================================================= */
#ifndef GPS_SPHERICAL_BASICS_H
#define GPS_SPHERICAL_BASICS_H
#include <stdint.h>
/** Raio médio da Terra para modelo esférico (m) */
#define EARTH_RADIUS_MEAN_M 6371008.8
/** Constantes WGS-84 para uso posterior (modelo elipsoidal) */
#define WGS84_A 6378137.0 /* semieixo maior (m) */
#define WGS84_F (1.0/298.257223563) /* achatamento */
#define WGS84_B (WGS84_A*(1.0 - WGS84_F)) /* semieixo menor */
/** Estrutura de coordenadas geodésicas (graus, metros) */
typedef struct {
double lat_deg; /* latitude em graus (+N, -S) */
double lon_deg; /* longitude em graus (+E, -W) */
double alt_m; /* altitude em metros (WGS-84) */
} GeoDeg;
/** Mesmo ponto em radianos (mais seguro para trigonometria) */
typedef struct {
double lat_rad; /* latitude em radianos */
double lon_rad; /* longitude em radianos */
double alt_m; /* altitude em metros */
} GeoRad;
/** Medição GPS simplificada (adapte conforme seu módulo) */
typedef struct {
GeoDeg pos_deg;
double hdop; /* Diluição de precisão horizontal (qualidade do fix) */
uint32_t unix_s; /* timestamp (segundos desde época Unix) */
uint8_t fix_ok; /* 1 = fix válido, 0 = inválido */
} GPSReading;
/* ------------------ Utilitários matemáticos ------------------ */
/** Converte graus -> radianos */
double deg2rad(double deg);
/** Converte radianos -> graus */
double rad2deg(double rad);
/** Normaliza ângulo em radianos para intervalo [-PI, PI) */
double normalize_rad_pm_pi(double ang);
/** Clampa valor entre [min, max] (útil para proteger arccos/arcsin) */
double clamp(double x, double minv, double maxv);
/** Converte GeoDeg -> GeoRad */
GeoRad geodeg_to_georad(const GeoDeg g);
/* ------------------ Mock de GPS para testes ------------------ */
/**
* Simula uma leitura de GPS gerando uma trajetória simples e ruído.
* - Trajetória: latitude fixa; longitude aumenta ~velocidade angular.
* - Ruído: gaussiano leve nos ângulos e altitude.
* - hdop/ fix_ok configuráveis para testes.
*
* @param t_s tempo em segundos (ex.: tick do seu sistema)
* @param base_lat latitude de referência (graus)
* @param base_lon longitude de referência (graus)
* @param speed_mps velocidade aproximada (m/s) para gerar variação na lon
* @return leitura GPS simulada
*/
GPSReading gps_mock_read(uint32_t t_s, double base_lat, double base_lon, double speed_mps);
#endif /* GPS_SPHERICAL_BASICS_H */
Explicando as escolhas:
EARTH_RADIUS_MEAN_M: distancia de arco d=Rθd=R\theta usa um raio médio; suficiente para a maioria das aplicações em firmware (tracking, telemetria). Para topografia ou precisão sub‑métrica usaremos WGS‑84 (Seções futuras).GeoDegeGeoRad: manter as duas representações evita conversões repetidas e facilita interfaces (NMEA, UBX) que vêm em graus.gps_mock_read: permite testar o pipeline sem um receptor físico (ou em CI/CD).
Agora, implementamos os utilitários e o mock numa unidade .c:
/* =========================================================================
* gps_spherical_basics.c
* ========================================================================= */
#include "gps_spherical_basics.h"
#include <math.h>
/* --- Helpers de aleatoriedade reprodutível para ruído simples --- */
static uint32_t lcg_state = 0x12345678u;
static uint32_t lcg_next(void) { /* Linear Congruential Generator */
lcg_state = 1664525u * lcg_state + 1013904223u;
return lcg_state;
}
static double u01(void) { /* [0,1) */
return (lcg_next() / (double)UINT32_MAX);
}
static double randn(void) { /* Box-Muller gaussiano ~N(0,1) */
double u1 = fmax(u01(), 1e-12);
double u2 = u01();
return sqrt(-2.0*log(u1))*cos(2.0*M_PI*u2);
}
/* ------------------ Implementações dos utilitários ------------------ */
double deg2rad(double deg){ return deg * (M_PI / 180.0); }
double rad2deg(double rad){ return rad * (180.0 / M_PI); }
double normalize_rad_pm_pi(double ang){
/* normaliza em [-pi, pi) evitando drift numérico */
while(ang >= M_PI) ang -= 2.0*M_PI;
while(ang < -M_PI) ang += 2.0*M_PI;
return ang;
}
double clamp(double x, double minv, double maxv){
return (x < minv) ? minv : (x > maxv ? maxv : x);
}
GeoRad geodeg_to_georad(const GeoDeg g){
GeoRad r;
r.lat_rad = deg2rad(g.lat_deg);
r.lon_rad = deg2rad(g.lon_deg);
r.alt_m = g.alt_m;
return r;
}
/* ------------------ Mock de GPS ------------------ */
GPSReading gps_mock_read(uint32_t t_s, double base_lat, double base_lon, double speed_mps){
/* Modelo simples: caminha leste-oeste em lat fixa, com jitter */
const double R = EARTH_RADIUS_MEAN_M;
const double omega = speed_mps / (R * cos(deg2rad(base_lat))); /* rad/s no paralelo */
double lon0_rad = deg2rad(base_lon);
double lat0_rad = deg2rad(base_lat);
/* Evolução nominal da longitude com o tempo */
double lon_nom_rad = lon0_rad + omega * (double)t_s;
/* Ruído (exagere/amenize conforme seu cenário) */
double n_lat_deg = 0.00005 * randn(); /* ~5e-5 deg ≈ 5.5 m em N-S */
double n_lon_deg = 0.00005 * randn(); /* idem em E-O (escala c/ lat) */
double n_alt_m = 0.5 * randn(); /* 0.5 m RMS */
GPSReading r = {0};
r.pos_deg.lat_deg = rad2deg(lat0_rad) + n_lat_deg;
r.pos_deg.lon_deg = rad2deg(lon_nom_rad) + n_lon_deg;
r.pos_deg.alt_m = 35.0 + n_alt_m; /* altura “urbana” de exemplo */
r.hdop = 0.9 + 0.2 * fabs(randn()); /* 0.9..~1.5 */
r.unix_s = t_s;
r.fix_ok = 1;
return r;
}
Notas importantes do mock:
- Velocidade angular ω=vRcosφ\omega = \dfrac{v}{R\cos\varphi}: aproxima o deslocamento sobre o paralelo (latitude constante). É didático para validar a matemática.
- O ruído é gaussiano simples (Box‑Muller) para simular jitter de fix.
hdopsimulado ajuda a decidir se descartamos medidas ruins em filtros (tema futuro).
Validação rápida (exemplo de uso)
Para fechar esta seção, um main rápido mostrando como obter leituras do mock e convertê-las para radianos (pronto para as fórmulas da próxima seção):
/* =========================================================================
* demo_basics.c
* ========================================================================= */
#include <stdio.h>
#include "gps_spherical_basics.h"
int main(void){
/* Posição base: Fortaleza/CE (aprox.) */
double base_lat = -3.7327; /* graus */
double base_lon = -38.5267; /* graus */
double speed = 15.0; /* m/s ~ 54 km/h */
for(uint32_t t=0; t<=10; ++t){
GPSReading r = gps_mock_read(t, base_lat, base_lon, speed);
GeoRad rr = geodeg_to_georad(r.pos_deg);
printf("[%u s] lat=%.6f°, lon=%.6f°, alt=%.2f m (rad: φ=%.6f, λ=%.6f), HDOP=%.2f\n",
r.unix_s, r.pos_deg.lat_deg, r.pos_deg.lon_deg, r.pos_deg.alt_m,
rr.lat_rad, rr.lon_rad, r.hdop);
}
return 0;
}
O que observar na saída: longitude variando suavemente (caminhando para leste), latitude quase constante com pequeno ruído, e valores em radianos prontos para uso nas fórmulas esféricas.
Distância e Rumo na Esfera (Lei dos Cossenos, Haversine e Bearing)
Na primeira seção criamos a base: representação de coordenadas, utilitários matemáticos e um mock de GPS. Agora aplicaremos as fórmulas de trigonometria esférica para calcular distâncias e direções entre dois pontos obtidos via GPS.
Fórmulas envolvidas
- Lei dos Cossenos Esférica:
\[\cos(\theta) = \sin(\varphi_1)\sin(\varphi_2) + \cos(\varphi_1)\cos(\varphi_2)\cos(\Delta \lambda)\]
onde θ\theta é a distância angular. d=R⋅θd = R \cdot \theta
- Fórmula de Haversine (mais estável para distâncias curtas):
\[\operatorname{hav}(\theta) = \sin^2\left(\frac{\Delta \varphi}{2}\right) + \cos(\varphi_1)\cos(\varphi_2)\sin^2\left(\frac{\Delta \lambda}{2}\right) d=2R⋅arcsin(hav(θ))d = 2R \cdot \arcsin(\sqrt{\operatorname{hav}(\theta)})\]
- Rumo Inicial (Bearing):
\[\alpha = \operatorname{atan2}\left(\sin(\Delta \lambda)\cos(\varphi_2), \cos(\varphi_1)\sin(\varphi_2) – \sin(\varphi_1)\cos(\varphi_2)\cos(\Delta \lambda)\right)\]
Código em C — Distância e Rumo
/* =========================================================================
* gps_spherical_calc.c
* Funções de cálculo: distância e rumo usando trigonometria esférica
* ========================================================================= */
#include "gps_spherical_basics.h"
#include <math.h>
/* Distância pela Lei dos Cossenos */
double spherical_distance_m(GeoRad p1, GeoRad p2) {
double dlon = p2.lon_rad - p1.lon_rad;
double cos_theta = sin(p1.lat_rad)*sin(p2.lat_rad) +
cos(p1.lat_rad)*cos(p2.lat_rad)*cos(dlon);
cos_theta = clamp(cos_theta, -1.0, 1.0); /* proteger contra erro numérico */
double theta = acos(cos_theta);
return EARTH_RADIUS_MEAN_M * theta;
}
/* Distância pela Haversine (mais estável em curtas distâncias) */
double haversine_distance_m(GeoRad p1, GeoRad p2) {
double dlat = p2.lat_rad - p1.lat_rad;
double dlon = p2.lon_rad - p1.lon_rad;
double a = sin(dlat/2.0)*sin(dlat/2.0) +
cos(p1.lat_rad)*cos(p2.lat_rad)*sin(dlon/2.0)*sin(dlon/2.0);
double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
return EARTH_RADIUS_MEAN_M * c;
}
/* Rumo inicial (azimute) em radianos */
double initial_bearing_rad(GeoRad p1, GeoRad p2) {
double dlon = p2.lon_rad - p1.lon_rad;
double y = sin(dlon) * cos(p2.lat_rad);
double x = cos(p1.lat_rad)*sin(p2.lat_rad) -
sin(p1.lat_rad)*cos(p2.lat_rad)*cos(dlon);
double brng = atan2(y, x);
return normalize_rad_pm_pi(brng);
}
Exemplo prático — Distância e rumo entre dois pontos do mock GPS
/* =========================================================================
* demo_distance.c
* ========================================================================= */
#include <stdio.h>
#include "gps_spherical_basics.h"
/* Declaração das funções de cálculo */
double spherical_distance_m(GeoRad p1, GeoRad p2);
double haversine_distance_m(GeoRad p1, GeoRad p2);
double initial_bearing_rad(GeoRad p1, GeoRad p2);
int main(void) {
double base_lat = -3.7327; /* Fortaleza, CE */
double base_lon = -38.5267;
double speed = 250.0; /* avião ~900 km/h */
GPSReading r1 = gps_mock_read(0, base_lat, base_lon, speed);
GPSReading r2 = gps_mock_read(600, base_lat, base_lon, speed); // após 10 min
GeoRad p1 = geodeg_to_georad(r1.pos_deg);
GeoRad p2 = geodeg_to_georad(r2.pos_deg);
double d1 = spherical_distance_m(p1, p2);
double d2 = haversine_distance_m(p1, p2);
double brng = initial_bearing_rad(p1, p2);
printf("Ponto 1: lat=%.6f°, lon=%.6f°\n", r1.pos_deg.lat_deg, r1.pos_deg.lon_deg);
printf("Ponto 2: lat=%.6f°, lon=%.6f°\n", r2.pos_deg.lat_deg, r2.pos_deg.lon_deg);
printf("Distância (Lei dos Cossenos): %.2f m\n", d1);
printf("Distância (Haversine): %.2f m\n", d2);
printf("Rumo inicial: %.2f°\n", rad2deg(brng));
return 0;
}
O que acontece aqui:
- Simulamos 10 minutos de voo a ~900 km/h.
- Calculamos a distância por duas fórmulas (cossenos e haversine).
- Obtivemos o rumo inicial em graus, que representa o azimute a ser seguido.
Ponto de Destino e Conversões Cartesianas (ECEF e ENU)
Depois de calcular distâncias e rumos entre pontos GPS, o próximo passo é projetar trajetórias e converter coordenadas geodésicas para sistemas cartesianos tridimensionais, usados em algoritmos de navegação, sensores inerciais e integração com sistemas de guerra eletrônica.
Ponto de Destino na Esfera
Dado um ponto inicial \((\varphi_1, \lambda_1)\), um rumo inicial α e uma distância percorrida d, podemos calcular o ponto de destino sobre a esfera de raio R pela fórmula: \(\varphi_2 = \arcsin(\sin\varphi_1 \cos\frac{d}{R} + \cos\varphi_1 \sin\frac{d}{R}\cos\alpha) λ2=λ1+arctan2(sinαsindRcosφ1,cosdR−sinφ1sinφ2)\lambda_2 = \lambda_1 + \arctan2\Big(\sin\alpha \sin\frac{d}{R}\cos\varphi_1, \cos\frac{d}{R} – \sin\varphi_1 \sin\varphi_2\Big)\)
Essas equações são usadas em planejamento de rotas, onde conhecemos a posição inicial e o vetor de movimento.
3.2. Conversão para Coordenadas Cartesianas (ECEF)
O sistema ECEF (Earth-Centered, Earth-Fixed) coloca a origem no centro da Terra. As coordenadas (x,y,z) são dadas por: \[x = (N(\varphi) + h)\cos\varphi\cos\lambda y=(N(φ)+h)cosφsinλy = (N(\varphi) + h)\cos\varphi\sin\lambda z=[(1−e2)N(φ)+h]sinφz = \left[(1-e^2)N(\varphi) + h\right]\sin\varphi\]
onde:
- \(N(\varphi) = \frac{a}{\sqrt{1-e^2 \sin^2\varphi}}\) é o raio de curvatura no meridiano.
- \(a = 6378137.0\) m (semieixo maior do WGS-84).
- \(f – f^2\) é a excentricidade da Terra (\(f = 1/298.257223563\)).
- h é a altitude.
Este sistema é fundamental para fusão com sensores inerciais e radares.
3.3. Conversão para Coordenadas Locais (ENU)
O sistema ENU (East, North, Up) é um referencial local tangente à superfície terrestre, útil para representar deslocamentos próximos a uma estação base.
Transformação ECEF → ENU exige a posição de referência \((\varphi_0, \lambda_0)\). A matriz de rotação é: \[\begin{bmatrix} e \\ n \\ u \end{bmatrix} = \begin{bmatrix} -\sin\lambda_0 & \cos\lambda_0 & 0 \\ -\sin\varphi_0 \cos\lambda_0 & -\sin\varphi_0 \sin\lambda_0 & \cos\varphi_0 \\ \cos\varphi_0 \cos\lambda_0 & \cos\varphi_0 \sin\lambda_0 & \sin\varphi_0 \end{bmatrix} \cdot \begin{bmatrix} x – x_0 \\ y – y_0 \\ z – z_0 \end{bmatrix}\]
Assim obtemos deslocamentos locais em metros.
3.4. Código em C — Cálculo do Destino e Conversões
/* =========================================================================
* gps_projection.c
* Ponto de destino, ECEF e ENU
* ========================================================================= */
#include "gps_spherical_basics.h"
#include <math.h>
/* Ponto de destino após caminhar distância d (m) com rumo alpha (rad) */
GeoRad destination_point(GeoRad start, double bearing_rad, double dist_m) {
double R = EARTH_RADIUS_MEAN_M;
double delta = dist_m / R; /* distância angular */
double lat2 = asin(sin(start.lat_rad)*cos(delta) +
cos(start.lat_rad)*sin(delta)*cos(bearing_rad));
double lon2 = start.lon_rad + atan2(sin(bearing_rad)*sin(delta)*cos(start.lat_rad),
cos(delta)-sin(start.lat_rad)*sin(lat2));
GeoRad result = {lat2, lon2, start.alt_m};
return result;
}
/* Estrutura para coordenadas cartesianas */
typedef struct {
double x, y, z;
} Vec3;
/* Conversão Geodésica -> ECEF (WGS-84) */
Vec3 geodetic_to_ecef(GeoRad g) {
double a = WGS84_A;
double f = WGS84_F;
double e2 = 2*f - f*f;
double N = a / sqrt(1.0 - e2*sin(g.lat_rad)*sin(g.lat_rad));
double x = (N + g.alt_m) * cos(g.lat_rad) * cos(g.lon_rad);
double y = (N + g.alt_m) * cos(g.lat_rad) * sin(g.lon_rad);
double z = ((1-e2)*N + g.alt_m) * sin(g.lat_rad);
Vec3 res = {x,y,z};
return res;
}
/* Conversão ECEF -> ENU em relação a ponto de referência */
Vec3 ecef_to_enu(Vec3 p, Vec3 ref, GeoRad g0) {
double dx = p.x - ref.x;
double dy = p.y - ref.y;
double dz = p.z - ref.z;
double sl = sin(g0.lon_rad), cl = cos(g0.lon_rad);
double sp = sin(g0.lat_rad), cp = cos(g0.lat_rad);
Vec3 enu;
enu.x = -sl*dx + cl*dy; /* East */
enu.y = -sp*cl*dx - sp*sl*dy + cp*dz; /* North */
enu.z = cp*cl*dx + cp*sl*dy + sp*dz; /* Up */
return enu;
}
3.5. Exemplo — Projeção de rota
#include <stdio.h>
#include "gps_spherical_basics.h"
GeoRad destination_point(GeoRad start, double bearing_rad, double dist_m);
Vec3 geodetic_to_ecef(GeoRad g);
Vec3 ecef_to_enu(Vec3 p, Vec3 ref, GeoRad g0);
int main(void) {
GeoDeg dstart = {-3.7327, -38.5267, 50.0}; /* Fortaleza */
GeoRad start = geodeg_to_georad(dstart);
/* Ponto após 100 km rumo leste */
GeoRad dest = destination_point(start, deg2rad(90.0), 100000.0);
Vec3 ecef_start = geodetic_to_ecef(start);
Vec3 ecef_dest = geodetic_to_ecef(dest);
Vec3 enu = ecef_to_enu(ecef_dest, ecef_start, start);
printf("Destino: lat=%.6f°, lon=%.6f°\n", rad2deg(dest.lat_rad), rad2deg(dest.lon_rad));
printf("ENU relativo ao ponto inicial: E=%.2f m, N=%.2f m, U=%.2f m\n", enu.x, enu.y, enu.z);
return 0;
}
Resultado esperado:
- A latitude muda pouco (rota leste-oeste).
- O deslocamento em ENU deve mostrar algo próximo de 100 km em “E” (East) e valores pequenos em N e U.
Integração em Sistemas Embarcados (GPS + Sensores Inerciais + Filtros)
Até aqui implementamos as funções principais de distância, rumo, projeção de rota e conversão para ECEF/ENU. Em sistemas embarcados reais — como drones, veículos autônomos e plataformas militares — raramente o GPS é usado de forma isolada. Ele é integrado com sensores inerciais (IMUs contendo acelerômetro, giroscópio e magnetômetro) e processado por filtros de fusão, como Kalman estendido (EKF) ou Madgwick.
Problema da Integração
- O GPS fornece posição absoluta (latitude, longitude, altitude), mas com baixa taxa (1–10 Hz) e ruído dependente das condições de visibilidade dos satélites.
- A IMU fornece medições rápidas (100–1000 Hz) de aceleração e rotação, mas sofre com deriva.
- Para obter navegação estável, usamos filtros que combinam os dois mundos: GPS corrige a deriva da IMU, e a IMU interpola a posição entre as amostras do GPS.
Uso do Sistema ENU
O sistema ENU (East, North, Up) é particularmente útil porque:
- Transforma coordenadas globais (lat, lon, alt) em deslocamentos locais (em metros).
- Permite somar incrementos vindos da IMU (Δx, Δy, Δz) diretamente.
- Facilita a implementação de filtros: o estado do EKF pode ser definido em termos de posição ENU, velocidade e orientação.
Estrutura de Dados para Fusão
Podemos definir um estado simplificado para navegação em um sistema embarcado:
typedef struct {
Vec3 pos_enu; /* posição local (E, N, U) em metros */
Vec3 vel_enu; /* velocidade local (m/s) */
double yaw; /* orientação em relação ao norte (rad) */
double pitch; /* inclinação (rad) */
double roll; /* rolamento (rad) */
} NavState;
Esse NavState pode ser atualizado continuamente pela IMU e corrigido quando novas leituras de GPS chegam.
Exemplo de Atualização de Estado (Pseudo-Fusão)
No exemplo abaixo, simulamos a fusão básica:
- Atualização rápida com a IMU (integração direta).
- Correção periódica com o GPS usando nossa conversão para ENU.
/* =========================================================================
* nav_fusion.c
* Exemplo simplificado de integração GPS + IMU
* ========================================================================= */
#include <stdio.h>
#include "gps_spherical_basics.h"
typedef struct {
Vec3 pos_enu;
Vec3 vel_enu;
double yaw, pitch, roll;
} NavState;
/* Função simplificada de atualização via IMU (integra aceleração) */
void imu_update(NavState *st, Vec3 acc_body, double dt) {
// Aqui assumimos que acc_body já está transformado para ENU (após aplicar matriz de rotação do AHRS)
st->vel_enu.x += acc_body.x * dt;
st->vel_enu.y += acc_body.y * dt;
st->vel_enu.z += acc_body.z * dt;
st->pos_enu.x += st->vel_enu.x * dt;
st->pos_enu.y += st->vel_enu.y * dt;
st->pos_enu.z += st->vel_enu.z * dt;
}
/* Correção de posição com GPS */
void gps_correction(NavState *st, GeoRad gps, GeoRad ref, Vec3 refECEF) {
Vec3 gpsECEF = geodetic_to_ecef(gps);
Vec3 gpsENU = ecef_to_enu(gpsECEF, refECEF, ref);
// Correção simples: aplicar diretamente posição GPS (poderia ser filtrado via EKF)
st->pos_enu = gpsENU;
}
int main(void) {
/* Referência: Fortaleza */
GeoDeg dref = {-3.7327, -38.5267, 50.0};
GeoRad ref = geodeg_to_georad(dref);
Vec3 refECEF = geodetic_to_ecef(ref);
NavState st = {0};
/* Simulação: 1 s de atualização com IMU (aceleração constante leste) */
Vec3 accENU = {0.5, 0.0, 0.0}; // aceleração 0.5 m/s² para leste
for(int i=0; i<10; i++) {
imu_update(&st, accENU, 0.1); // dt=0.1s
}
printf("Após integração IMU: E=%.2f m, N=%.2f m\n", st.pos_enu.x, st.pos_enu.y);
/* Correção com leitura GPS (mock) */
GPSReading g = gps_mock_read(60, dref.lat_deg, dref.lon_deg, 5.0); // após 1 min
GeoRad gpos = geodeg_to_georad(g.pos_deg);
gps_correction(&st, gpos, ref, refECEF);
printf("Após correção GPS: E=%.2f m, N=%.2f m\n", st.pos_enu.x, st.pos_enu.y);
return 0;
}
4.5. Explicação
- A função
imu_updateintegra acelerações (já transformadas para ENU pelo filtro Madgwick ou outro AHRS). - O GPS corrige a posição absoluta convertendo a leitura para ENU em relação a um ponto de referência.
- Esse exemplo usa fusão ingênua (aplica a posição GPS diretamente). Em aplicações reais, um Filtro de Kalman Estendido (EKF) pondera a confiança em cada sensor.
Mini‑sistema de Navegação em C (Simulação de rota com GPS‑mock + IMU + Fusão)
Nesta seção, vamos unir tudo em um exemplo completo e didático. Construiremos um loop de simulação que:
- lê o GPS‑mock (posição global),
- integra uma IMU sintética (aceleração em ENU),
- calcula distâncias, rumo (bearing), velocidade e curso com trigonometria esférica,
- faz uma fusão simples (com ponderação por qualidade HDOP) para corrigir a posição ENU,
- registra a trajetória prevista (dead‑reckoning) versus a posição corrigida pelo GPS.
Observação: este exemplo mantém o foco na clareza. Em produção, use um EKF ou um AHR/Kalman adequado ao seu hardware.
Arquivo de cabeçalho — Interface do exemplo integrado
/* =========================================================================
* nav_demo.h
* Interface do mini-sistema de navegação
* ========================================================================= */
#ifndef NAV_DEMO_H
#define NAV_DEMO_H
#include "gps_spherical_basics.h"
/* --- Cálculos já implementados nas seções anteriores --- */
double spherical_distance_m(GeoRad p1, GeoRad p2);
double haversine_distance_m(GeoRad p1, GeoRad p2);
double initial_bearing_rad(GeoRad p1, GeoRad p2);
typedef struct { double x, y, z; } Vec3;
GeoRad destination_point(GeoRad start, double bearing_rad, double dist_m);
Vec3 geodetic_to_ecef(GeoRad g);
Vec3 ecef_to_enu(Vec3 p, Vec3 ref, GeoRad g0);
/* --- Estado de navegação do nosso demo --- */
typedef struct {
/* Referência global (lat, lon, alt) e ECEF de referência */
GeoRad ref_geo;
Vec3 ref_ecef;
/* Estimativas em ENU */
Vec3 pos_enu_dr; /* posição por dead-reckoning (IMU apenas) */
Vec3 vel_enu_dr; /* velocidade por dead-reckoning (m/s) */
Vec3 pos_enu_fused; /* posição fundida (IMU + GPS) */
Vec3 vel_enu_fused; /* velocidade fundida */
/* Orientações simplificadas (apenas yaw usado para esta demo) */
double yaw_rad; /* rumo estimado (em rad) */
/* Configuração de fusão */
double alpha_hdop_good; /* peso para boas leituras GPS (baixa HDOP) */
double alpha_hdop_bad; /* peso para leituras ruins (alta HDOP) */
double hdop_threshold; /* limiar para aceitar GPS como "bom" */
} NavDemoState;
/* Inicializa o estado do demo a partir de um ponto de referência */
void nav_demo_init(NavDemoState *st, GeoDeg ref_deg);
/* Atualiza o estado via IMU (integração simples) */
void nav_demo_imu_update(NavDemoState *st, Vec3 acc_enu, double dt);
/* Aplica correção com GPS: converte leitura para ENU e faz blend com DR */
void nav_demo_gps_fuse(NavDemoState *st, GPSReading g);
#endif /* NAV_DEMO_H */
Comentário:
- Mantemos dois rastros: DR (dead‑reckoning) e fused (fundido). Assim você enxerga o ganho real da correção do GPS.
- A fusão aqui é exponencial simples (um “complementary filter”): um peso α\alpha decide o quanto confiar no GPS versus DR. Ajustamos α\alpha com base no HDOP.
Implementação — Inicialização, atualização por IMU e fusão com GPS
/* =========================================================================
* nav_demo.c
* ========================================================================= */
#include "nav_demo.h"
#include <math.h>
#include <stdio.h>
/* ----------- Helpers internos ----------- */
static Vec3 vec_add(Vec3 a, Vec3 b){ return (Vec3){a.x+b.x, a.y+b.y, a.z+b.z}; }
static Vec3 vec_scale(Vec3 v, double s){ return (Vec3){v.x*s, v.y*s, v.z*s}; }
/* Fusão exponencial simples: y <- (1-a)*y + a*x */
static Vec3 blend(Vec3 y, Vec3 x, double a){
return vec_add(vec_scale(y, 1.0 - a), vec_scale(x, a));
}
/* ----------- API do demo ----------- */
void nav_demo_init(NavDemoState *st, GeoDeg ref_deg){
st->ref_geo = geodeg_to_georad(ref_deg);
st->ref_ecef = geodetic_to_ecef(st->ref_geo);
st->pos_enu_dr = (Vec3){0,0,0};
st->vel_enu_dr = (Vec3){0,0,0};
st->pos_enu_fused= (Vec3){0,0,0};
st->vel_enu_fused= (Vec3){0,0,0};
st->yaw_rad = 0.0; /* rumo inicial arbitrário */
/* Configuração de fusão (ajuste conforme seu caso) */
st->alpha_hdop_good = 0.7; /* confiar bem no GPS bom */
st->alpha_hdop_bad = 0.2; /* confiar pouco no GPS ruim */
st->hdop_threshold = 1.5; /* <= 1.5 consideramos "bom" */
}
void nav_demo_imu_update(NavDemoState *st, Vec3 acc_enu, double dt){
/* Integra aceleração (ENU) -> velocidade -> posição (dead-reckoning) */
st->vel_enu_dr = vec_add(st->vel_enu_dr, vec_scale(acc_enu, dt));
st->pos_enu_dr = vec_add(st->pos_enu_dr, vec_scale(st->vel_enu_dr, dt));
/* Para a trilha fundida, propagamos também por integração básica */
st->vel_enu_fused = vec_add(st->vel_enu_fused, vec_scale(acc_enu, dt));
st->pos_enu_fused = vec_add(st->pos_enu_fused, vec_scale(st->vel_enu_fused, dt));
}
void nav_demo_gps_fuse(NavDemoState *st, GPSReading g){
if(!g.fix_ok) return; /* sem correção se fix inválido */
GeoRad gps_rad = geodeg_to_georad(g.pos_deg);
Vec3 gps_ecef = geodetic_to_ecef(gps_rad);
Vec3 gps_enu = ecef_to_enu(gps_ecef, st->ref_ecef, st->ref_geo);
/* Peso adaptativo baseado em HDOP */
double alpha = (g.hdop <= st->hdop_threshold) ? st->alpha_hdop_good
: st->alpha_hdop_bad;
/* Corrige posição e (opcionalmente) velocidade via blending */
st->pos_enu_fused = blend(st->pos_enu_fused, gps_enu, alpha);
/* Estimativa de velocidade por GPS (diferença finita) é possível aqui
se guardarmos a última amostra e dt; para simplificar, mantemos IMU.
Em produção, faça: v_gps ~= (enu_k - enu_{k-1}) / dt e faça o blend. */
}
Por que fusão exponencial?
- É leve (O(1)), ideal para MCUs sem FPU robusta.
- Você pode evoluir para EKF depois, mas já terá a espinha dorsal do sistema e dados coerentes para validar modelos.
Loop de simulação — GPS‑mock + IMU + Relatórios
Aqui, criamos um main que executa 5 minutos de “voo” com aceleração moderada para leste e correções periódicas do GPS‑mock. Mostramos:
- distância percorrida (Haversine),
- rumo (bearing) entre amostras GPS,
- posição ENU DR vs Fused,
- curso (course‑over‑ground) em graus.
/* =========================================================================
* main_nav_demo.c
* ========================================================================= */
#include <stdio.h>
#include <math.h>
#include "nav_demo.h"
#include "gps_spherical_basics.h"
/* Protótipos dos cálculos que já temos */
double haversine_distance_m(GeoRad p1, GeoRad p2);
double initial_bearing_rad(GeoRad p1, GeoRad p2);
/* Utilitário para ângulo em [0,360) graus */
static double wrap_deg_360(double deg){
while(deg < 0.0) deg += 360.0;
while(deg >= 360.0) deg -= 360.0;
return deg;
}
int main(void){
/* Referência global: Fortaleza/CE (aprox.) */
GeoDeg ref = {-3.7327, -38.5267, 50.0};
/* Inicializa estado */
NavDemoState st;
nav_demo_init(&st, ref);
/* Configura “perfil de movimento” (ENU):
- aceleração constante para Leste (x)
- z e y ~ 0 para simplificar (plano horizontal) */
Vec3 acc_enu = {0.15, 0.0, 0.0}; /* 0.15 m/s² -> ~54 km/h em ~1000 s */
/* Parâmetros da simulação */
const double dt = 0.1; /* 10 Hz de IMU */
const int N = (int)(5*60 / dt); /* 5 minutos */
const double vgps = 15.0; /* velocidade para o mock GPS (m/s) */
const int gps_period_ms = 1000; /* GPS 1 Hz */
int gps_accum_ms = 0;
/* Armazena última amostra GPS para distância/curso */
int have_prev = 0;
GPSReading prev_g = {0};
for(int k=0; k<N; ++k){
double t = k*dt;
/* 1) Atualização via IMU (10 Hz) */
nav_demo_imu_update(&st, acc_enu, dt);
/* 2) Simula GPS a 1 Hz */
gps_accum_ms += (int)(dt*1000);
if(gps_accum_ms >= gps_period_ms){
gps_accum_ms = 0;
GPSReading g = gps_mock_read((uint32_t)floor(t), ref.lat_deg, ref.lon_deg, vgps);
nav_demo_gps_fuse(&st, g); /* correção por blending */
/* Relatórios quando GPS chega */
GeoRad gcur = geodeg_to_georad(g.pos_deg);
if(have_prev){
GeoRad gprev = geodeg_to_georad(prev_g.pos_deg);
double d_m = haversine_distance_m(gprev, gcur);
double bearing = initial_bearing_rad(gprev, gcur);
double cog_deg = wrap_deg_360(rad2deg(bearing));
/* Mostra um resumo compacto a cada segundo */
printf("[t=%5.1fs] GPS lat=%.6f lon=%.6f hdop=%.2f "
"Δd=%.1f m COG=%.1f° | ENU_DR (E=%.1f N=%.1f) ENU_FUSED (E=%.1f N=%.1f)\n",
t, g.pos_deg.lat_deg, g.pos_deg.lon_deg, g.hdop,
d_m, cog_deg,
st.pos_enu_dr.x, st.pos_enu_dr.y,
st.pos_enu_fused.x, st.pos_enu_fused.y);
} else {
printf("[t=%5.1fs] GPS lat=%.6f lon=%.6f hdop=%.2f "
"Δd=---- COG=---- | ENU_DR (E=%.1f N=%.1f) ENU_FUSED (E=%.1f N=%.1f)\n",
t, g.pos_deg.lat_deg, g.pos_deg.lon_deg, g.hdop,
st.pos_enu_dr.x, st.pos_enu_dr.y,
st.pos_enu_fused.x, st.pos_enu_fused.y);
have_prev = 1;
}
prev_g = g;
}
}
/* Resumo final após 5 min */
double e_err = st.pos_enu_fused.x - st.pos_enu_dr.x;
double n_err = st.pos_enu_fused.y - st.pos_enu_dr.y;
double err_m = sqrt(e_err*e_err + n_err*n_err);
printf("\nResumo após 5 min:\n");
printf(" DR : E=%.1f m, N=%.1f m\n", st.pos_enu_dr.x, st.pos_enu_dr.y);
printf(" FUSED: E=%.1f m, N=%.1f m\n", st.pos_enu_fused.x, st.pos_enu_fused.y);
printf(" Diferença FUSED - DR: ΔE=%.1f m, ΔN=%.1f m (||Δ||=%.1f m)\n", e_err, n_err, err_m);
return 0;
}
Leitura do output (o que observar):
- A cada 1 segundo (GPS), são impressos: latitude/longitude simuladas, HDOP, deslocamento entre a amostra atual e a anterior (Δd, via Haversine) e o curso (COG) em graus, além das posições ENU por DR e ENU fundida.
- Ao final, um resumo da diferença entre a trilha DR e a Fused — útil para avaliar o impacto da correção de GPS.
Onde entram as fórmulas matemáticas aqui?
- Haversine e bearing (atan2): calculam Δd e COG entre amostras GPS sucessivas.
- ECEF/ENU: convertem a leitura GPS global para metros locais, permitindo somar incrementos vindos da IMU.
- Dead‑reckoning: integra a⃗\vec{a} para v⃗\vec{v} e depois para p⃗\vec{p} (cinemática básica).
- Fusão exponencial: aproxima o comportamento de um filtro complementar (alta frequência da IMU, baixa frequência do GPS).
Se quiser evoluir para EKF, substitua
nav_demo_gps_fusepor uma atualização de estado com jacobianas e covariâncias Q,RQ, R — a interface ENU facilita muito essa etapa.
perfeito — vamos direto à Seção 6: Boas práticas, testes de unidade e extensões. Mantive tudo pragmático e “plugável” ao que você já tem.
Boas práticas, testes de unidade e extensões
Boas práticas de implementação
- Carimbos de tempo primeiro: normalize tudo para um relógio monotônico (ex.:
int64 ns) e faça resampling para uma malha comum (ex.: 100 Hz). Interpolar linearmente medições mais lentas (GPS/baro). - Convenções fixas: escolha e documente um frame (NED ou ENU) e a ordem de Euler (se usar) — recomendo quaternions + frame NED. Centralize isso num único header.
- Unidades: SI estrito (m, s, rad, Pa). Converta ao entrar/sair das APIs.
- Calibração: aplique bias e scale dos sensores na borda do sistema. Guarde a data/temperatura de calibração.
- Numérico: normalize quaternions após cada atualização (ou sempre que ‖q‖ se afastar 1e‑6). Trave gains e covariâncias para limites razoáveis (ex.:
Q∈[1e-10,1],R∈[1e-4,1e2]). - Falhas previsíveis: proteja divisões por zero (ex.: em latitudes extremas), trate
NaN/inf, e degrade com elegância (modo “dead‑reckoning” curto quando GPS cai). - Telemetria: exponha resíduos, HDOP, #satélites, normas de correções e tempos de fila — são ouro para depurar.
Testes de unidade (e propriedades)
Cubra três níveis: puro-matemática, sensoriamento/observabilidade e fusão.
Matemática de base
- Normalização de quaternions
- Given:
q=[0.6,0.8,0,0]→ Expect: norma=1. - Property:
normalize(normalize(q)) == normalize(q)(tolerância 1e‑12).
- Given:
- Rotação vetorial
- Given:
q=rot_z(π/2),v=[1,0,0]→ Expect:Rv=[0,1,0] ±1e‑12.
- Given:
- ECEF↔geodésico (WGS‑84)
- Round‑trip: geodésico→ECEF→geodésico preserva
lat,lon,hem ≤1e‑6 rad / ≤1e‑3 m para casos: equador, 45°, polos e altitudes {0, 1000, 10000 m}.
- Round‑trip: geodésico→ECEF→geodésico preserva
Sensores isolados
- Barômetro:
p=101325 PaaT=15°C→h≈0 m(ISA). Injetar bias conhecido e verificar remoção. - Magnetômetro: vetor normado após hard/soft iron de teste fica com norma 1 ±1e‑3.
- IMU estática:
acc≈[0,0,g],gyro≈0→ Madgwick converge para atitude nivelada em < 5 s.
Fusão/consistência
- EKF estático: resíduos de GPS ~ N(0,R) (teste χ² por janela).
- Passo de predição: energia/covariância não negativar (matriz
Psimétrica definida‑positiva).
(Dica): complemente com testes de propriedade (property-based): “norma de quaternion permanece 1”, “altura baro diminui quando pressão sobe”, etc.
Extensão: modelo elipsoidal WGS‑84 e altitude
Parâmetros WGS‑84
- Semieixo maior
a = 6378137.0 m - Achatamento
f = 1/298.257223563 - Semieixo menor
b = a(1−f) - Excentricidade ao quadrado
e² = 1 − (b²/a²)
Geodésico ↔ ECEF
Para latitude geodésica φ, longitude λ, altura sobre o elipsóide h:
- Raio de curvatura no prime vertical:
N(φ) = a / sqrt(1 − e² sin²φ) - Ponto ECEF:
x = (N(φ)+h) cosφ cosλ y = (N(φ)+h) cosφ sinλ z = (N(φ)(1−e²)+h) sinφ - Inverso (ECEF→geodésico): use iterativo de Bowring ou algoritmo de Fukushima; convergência típica < 5 iterações com tolerância 1e‑12.
Altitude: elipsóide vs geóide
- O GPS entrega h (altura elipsoidal). Usuários costumam querer H (altura ortométrica, “nível do mar”):
H = h − N_geo, ondeN_geoé ondulação geoidal (EGM96/2008). - Estratégia simples: pré‑carregar grid grosseiro (ex.: 5′) e bilinear; ou aceitar h e só converter em UI.
Fusão de altitude (baro + GNSS)
Modelo leve para z (vertical) e bias baro:
- Estado
x = [z, v_z, b_baro]^T - Dinâmica (discretizada com Δt):
z_k+1 = z_k + v_z,k Δt + 0.5 a_z Δt² v_z,k+1 = v_z,k + a_z Δt b_k+1 = b_k + w_b (random walk) - Medições
- Barômetro:
z_baro = z + b_baro + v_b - GNSS (elipsoidal):
z_gps = z + v_g
- Barômetro:
- Ajustes práticos
Qparab_baropequeno (ex.: 1e‑6…1e‑4 m²/s) captura drift térmico.- Reduza
R_gpsquando HDOP baixo (ver §6.6).
Filtros de atitude: EKF vs Madgwick
EKF (INS leve)
- Estado mínimo:
x = [q, b_g, b_a](quaternion + bias de giroscópio e acelerômetro). - Predição:
q̇ = 0.5 q ⊗ (ω_meas − b_g), integre com RK1/RK4, normalize.ḃ_g = w_g,ḃ_a = w_a(random walk).
- Medições:
- Acelerômetro em repouso → alinhamento de gravidade:
z_acc ≈ R(q)^T g_n. - Magnetômetro → rumo:
z_mag ≈ R(q)^T m_n.
- Acelerômetro em repouso → alinhamento de gravidade:
- Notas: linearize Jacobianos cuidadosamente; rejeite
z_accquando ‖acc‖ se afasta deg(manobras).
Madgwick (gradiente descendente)
- Atualização de
qcom ganhoβminimizando erro entreacc/magmedidos e referênciasg_n/m_n. - Prático:
β: ~0.01–0.1 (mais alto = mais responsivo, menos robusto a vibração).- Rejeite magnetômetro quando soft‑iron evidente (norma fora de faixa).
- Escolha: Madgwick é ótimo para MCU sem FPU e inicia rápido; EKF dá estimativas e covariâncias (úteis p/ fusão GNSS) e lida melhor com bias.
Detecção de outliers (HDOP/RAIM)
HDOP adaptativo (fácil e eficaz)
- Defina
R_gps = σ² I, comσ = σ_base * clamp(HDOP / HDOP_ref, min=0.5, max=4.0)
Ex.:σ_base=1.5 m,HDOP_ref=0.8. - Descartar fix quando:
HDOP>6,#SV<4, fix não 3D ou salto > 5σ.
RAIM – teste de consistência (leve)
Para uma solução GNSS (linha de base):
- Calcule resíduos
r = y − H x̂e a soma ponderadaSSE = rᵀ R⁻¹ r. - Teste χ²: rejeite medição se
SSE > χ²_{α, dof}(ex.: α=0.01). - Localização de falha (opcional): remova cada SV (leave‑one‑out), recompute
SSE; rejeite o SV que mais reduzSSE.
Integração: faça o teste antes do update do EKF; marque a amostra como outlier e pule (ou aumente R drasticamente) em caso de reprovação.
Exemplos de casos de teste (prontos para automação)
- Round‑trip WGS‑84: 10k pontos aleatórios; assert de erros máximos e RMS.
- Passos verticais: simule perfil baro com bias derivando + GPS com ruído e saltos; verifique RMSE(z) < 1.5 m e convergência de
b_baro. - Manobra brusca: aceleração lateral 2 g por 3 s; Madgwick deve ignorar
acc(por gating) e manter yaw dentro de 5°. - RAIM: injete uma pseudodistância com erro de 50 m; χ² deve disparar e a solução ser rejeitada.
Pseudocódigo de cola
ECEF ← geodésico (WGS‑84)
void geodetic_to_ecef(double lat, double lon, double h,
double *x, double *y, double *z) {
const double a = 6378137.0;
const double f = 1.0/298.257223563;
const double e2 = f*(2.0 - f);
double s = sin(lat), c = cos(lat);
double N = a / sqrt(1.0 - e2*s*s);
*x = (N + h)*c*cos(lon);
*y = (N + h)*c*sin(lon);
*z = (N*(1.0 - e2) + h)*s;
}
Gating por HDOP/χ² antes do EKF
bool gps_accept(const GpsFix *g, double chi2, int dof) {
if (g->mode != FIX_3D || g->nsats < 4 || g->hdop > 6.0) return false;
return (g->sse <= chi2_threshold(0.99, dof));
}
Passo Madgwick (esqueleto)
void madgwick_step(Quat *q, Vec3 gyro, Vec3 acc, Vec3 mag, double beta, double dt){
// normalize acc/mag, compute gradient, integrate q̇ = 0.5*q⊗ω - β∇f
// ... (omisso) ...
*q = normalize(*q);
}
Checklist de integração
- Frames/Unidades consistentes (NED/ENU, rad).
- Timestamps monotônicos e latências medidas.
- Calibração IMU/baro/mag aplicada.
- Testes automatizados passados (CI).
- Telemetria de resíduos/HDOP habilitada.
- Limites de gating (HDOP/χ²) revisados em campo.