fix(gps): adapt Doppler bandwidth from board validation

This commit is contained in:
2026-07-26 03:15:52 +03:00
parent 23650be635
commit 156cfc8b77
24 changed files with 592 additions and 49 deletions
+5 -1
View File
@@ -82,6 +82,8 @@ BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c
GPS_NAV_SOLVE_TIMER_TICKS ?= 10
GPS_PPS_STATUS_LOG_INTERVAL ?= 10
GPS_PPS_DOPPLER_PREDICTOR_ENABLE ?= 1
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES ?= 0
include ../common/app.mk
@@ -94,7 +96,9 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) \
-DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL)
-DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL) \
-DGPS_PPS_DOPPLER_PREDICTOR_ENABLE=$(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) \
-DGPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=$(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES)
LDLIBS += -lm
ifeq ($(GPS_DEBUG),1)
+82 -18
View File
@@ -176,6 +176,24 @@ Iki komut ayni MCS/BIN yoluna yazdigi icin iki imaj birlikte saklanacaksa
build'ler arasinda profil adiyla kopyalanmalidir. Bu secenekler software
derleme parametresidir; mevcut bitstream ile `fpga-mcs` yeterlidir.
PPS Doppler datum-drift predictor production'da varsayilan olarak aciktir.
Kontrollu A/B icin build-time secenekleri:
```sh
# Production: predictor sürekli acik, otomatik A/B yok.
make -C soc/software/gps_system \
GPS_PPS_DOPPLER_PREDICTOR_ENABLE=1 \
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=0
# Tani: her 200 navigation update'inda (~40 s) kapali/acik degistir.
make -C soc/software/gps_system \
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=200
```
`GPS_PPS_DOPPLER_PREDICTOR_ENABLE=0` yalniz kontrollu fallback testi icindir.
`GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES` sifirdan buyukse A/B secimi enable
degerini bilerek ezer; production image'da mutlaka `0` kalmalidir.
Otomatik WHO_AM_I algilama varsayilandir:
```sh
@@ -812,15 +830,20 @@ range_rate_mps = -doppler_hz * c / 1575420000
Ham 20 ms carrier snapshot'inda kartta gorulen birkac hertz jitter, L1'de
metre/saniye seviyesinde range-rate gurultusune ve zayif geometride daha buyuk
receiver velocity hatasina donusur. Software ISR her PRN icin ham degeri
degistirmeden korur ve yalniz PPS predictor yolu icin ayri bir state'te:
degistirmeden korur ve iki ayri state gunceller:
```text
doppler_filtered = 0.05 * doppler_raw + 0.95 * doppler_previous
PPS: doppler_slow = 0.05 * raw + 0.95 * previous
ESKF: doppler_fast = alpha * raw + (1-alpha) * previous
alpha = 0.05 stationary, 0.20 moving, 0.40 high acceleration
```
hesaplar. Ham ve filtreli range-rate ayri alanlarda tutulur. 50 Hz measurement
cadence'inde bu yaklasik `0.39 s` zaman sabitidir. Reacquisition/PRN
degisiminde eski kanal state'i kullanilmaz.
PPS yolu 50 Hz cadence'de yaklasik `0.39 s` zaman sabitine sahiptir. ESKF yolu
fusion motion state ve gravity-compensated linear acceleration ile bant
genisligini acar. Alpha buyudugunde EWMA noise kazanci kadar WLS range-rate
sigma da buyutulur; bu, FDE ve ESKF covariance'in filtre bant genisliginden
habersiz kalmasini engeller. Reacquisition/PRN degisiminde eski kanal state'i
kullanilmaz.
IIR donanimda da uygulanabilirdi; bunun icin kanal basina state,
coefficient-multiply, rounding/saturation ve reacquisition reset kurali
@@ -847,12 +870,13 @@ weight = 1 / sigma_rate^2
Her 200 ms navigation epoch'unda iki velocity tuketicisi vardir:
```text
raw range-rate -> robust velocity WLS -> fusion adapter -> ESKF
filtered range-rate -> robust velocity WLS -> PPS bias-drift predictor
adaptive range-rate -> robust velocity WLS -> fusion adapter -> ESKF
slow range-rate -> robust velocity WLS -> PPS bias-drift predictor
```
Ham ESKF yolu temporal filtreleme isini ESKF'ye birakir ve `alpha=0.05`
IIR'in yaklasik 390 ms dinamik gecikmesini velocity measurement'a eklemez.
Tam ham 20 ms snapshot yolu kart A/B'sinde fazla gurultulu bulundugu icin
ESKF'ye yayinlanmaz. Adaptif yol sakin durumda PPS kadar dusuk gurultulu,
harekette ise sabit `alpha=0.05` yolundan daha dusuk gecikmelidir.
PPS'in ana olcumu position WLS `clock_bias_m` sonucudur. Filtreli velocity
yalniz bu referans-datum bias'inin 200 ms epoch'lar arasindaki turevini
ongormek ve PPS kalite profilini siniflandirmak icin kullanilir. Doppler
@@ -905,9 +929,8 @@ velocity/covariance, `speed_mps`, `velocity_residual_rms_mps`,
Fusion adapter covariance'i `R_ecef_to_enu * P * R^T` ile ENU'ya dondurur.
2026-07-25 sabit kart verisinde zayif geometriyle gorulen
`p95=6.020 m/s` speed hatasi nedeniyle default alt sinirlar yatay eksenlerde
`2.0 m/s`, dikey eksende `3.0 m/s`'dir. ESKF artik ham Doppler WLS aldigi icin
bu konservatif tabanlar temporal korelasyondan cok zayif geometri ve kartta
henuz yeniden kalibre edilmemis raw-Doppler hata kuyruklarini sinirlar; formal
`2.0 m/s`, dikey eksende `3.0 m/s`'dir. Bu konservatif tabanlar zayif geometri
ve kartta kalan Doppler hata kuyruklarini sinirlar; formal
covariance daha buyukse aynen korunur. Velocity innovation 3-DOF `%99.9`
`NIS=16.266` kapisindan gecer. Kabul/red sayilari ve son velocity NIS'i
`[FUSION]` UART status satirinda `velocity=accepted/rejected` ve `vel_nis`
@@ -964,9 +987,9 @@ GPS_LOG(level, category, ...)
| `GPS_DEBUG_LEVEL` | Eklenen cikti |
| ---: | --- |
| `1` | Temel telemetri: ham GPS/WLS `[LOCATION] source=gps` satirinda DOP ile birlikte `velocity_valid`, `speed` ve `velocity_rms`; gecerliyse ESKF `[LOCATION] source=imu_corrected`; sifir tabanli donanim kanal numaralarini tasiyan `[TRK] tracked satellites=... prn_indices=[...]` (`0` = GPS PRN `G01`). Baslangicta fusion/IMU konfigurasyonu da bir kez yazilir. |
| `1` | Temel telemetri: ham GPS/WLS `[LOCATION] source=gps` satirinda DOP ile birlikte `velocity_valid`, `speed` ve `velocity_rms`; gecerliyse ESKF `[LOCATION] source=imu_corrected`; sifir tabanli donanim kanal numaralarini tasiyan `[TRK] tracked satellites=... prn_indices=[...]` (`0` = GPS PRN `G01`). Baslangicta fusion/IMU konfigurasyonu ve periyodik `[FUSION]` NIS/kabul-red durumu da gorulur. |
| `2` | Gelecekteki ek status ayrintilari icin ayrilmistir. Su an level 2'ye ozel mesaj yoktur; pratikte level 1 ile ayni ciktiyi verir. |
| `3` | Level 1'e ek olarak acquisition/tracking/navigation durum gecisleri, warning/error mesajlari, integrity bilgileri ve periyodik ayrintili `[FUSION] state=...` durumu. |
| `3` | Level 1'e ek olarak acquisition/tracking/navigation durum gecisleri, warning/error mesajlari ve integrity bilgileri. |
| `4` | Level 3'e ek olarak performans, scheduler, nav-bit/parser, uydu/olcum ve dusuk seviye TRACE tanilari. UART yuku en yuksek seviyedir. |
`Makefile` varsayilanlari `GPS_DEBUG=1` ve `GPS_DEBUG_LEVEL=4`'tur. Bu nedenle
@@ -1234,10 +1257,51 @@ azaltiyor ve C/N0 weighting ornek epoch'ta sonucu iyilestiriyor. Buna ragmen
position WLS'deki pseudorange/code-phase hata ve zayif geometriyi tek basina
duzeltmiyor. Doppler velocity artik ESKF state update'ine baglidir; formal
covariance, kart verisine dayali floor ve ayri NIS gate ile asiri guven
sinirlanir. Bu yeni software'in gercek kartta `vel_nis` ve
`velocity=accepted/rejected` sayaclariyla saha A/B sonucu henuz olculmedi;
yukaridaki board istatistikleri ESKF entegrasyonundan onceki ham velocity
dogrulamasidir.
sinirlanir. Yukaridaki board istatistikleri ESKF entegrasyonundan onceki
velocity dogrulamasidir; entegrasyon sonrasi `vel_nis` ve
`velocity=accepted/rejected` saha sonucu bir sonraki bolumdedir.
### 2026-07-26 Genesys2 PPS ve Doppler/ESKF A/B
Kart sabitken once tam ham 20 ms snapshot -> robust WLS -> ESKF yolu olculdu.
Son 120 PPS penceresinde ham WLS speed RMS `9.17 m/s`, p95 `16.71 m/s` ve
raw-FDE `86/120` oldu. Yavas `alpha=0.05` PPS yolu ayni pencerede `0.92 m/s`
RMS, `1.63 m/s` p95 ve `0/120` FDE verdi. ESKF+ZUPT fused hizi dusuk tutsa da
bu sonuc tam ham yolun hareketli platform icin guvenli oldugunu dogrulamadi.
Bu nedenle ESKF icin yukarida tarif edilen ayri adaptif yol uygulandi. Ayni
kartta 240 epoch'luk yerlesmis pencerede `alpha=0.05`, speed RMS `0.48 m/s`,
p95 `0.83 m/s`, FDE `0/240` olculdu. Son 180 saniyede ESKF velocity kabul/red
farki `892/0`, velocity NIS median/p95/max `0.028/0.111/0.180`, fused speed
RMS `0.0052 m/s` oldu. IMU overflow sayaci bu pencerede `15` olarak sabit
kaldi; olcum telemetrisi yeni kayip uretmedi.
PPS predictor, yeniden boot etmeden 40 saniyelik kapali/acik segmentlerle A/B
yapildi (`GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=200`, yalniz tanisal build).
Her segmentin ilk bes saniyesi atildiginda predictor acik segmentlerde software
clock-bias innovation RMS `12.52 m`, kapali segmentlerde `46.48 m` oldu.
Hardware phase p95 acikken `51.02 ns`, kapaliyken `58.21 ns` idi. Tek acik
segmentte gorulen bir `-216.48 ns` one-epoch phase transient'i lock kaybi
olusturmadi; bu nedenle physical-pin jitter sonucu olarak yorumlanmaz.
Production default predictor acik, A/B toggling kapali (`0`) kalir.
Son olarak `adaptive-pps-final-20260726` production image'i
`PREDICTOR_ENABLE=1`, `PREDICTOR_AB_UPDATES=0` ile RAM'den karta yuklendi.
Acquisition/coarse baslangici dislandiktan sonraki 120 lock status orneginde:
- predictor tum orneklerde `1`, ESKF alpha sabit kartta tum orneklerde `0.05`,
- adaptive velocity-WLS speed RMS/p95 `0.52/0.92 m/s`, FDE `0/120`,
- ESKF velocity kabul/red farki `448/0`, velocity NIS p95/max
`0.139/0.341`, fused speed RMS `0.0061 m/s`,
- PPS software clock innovation RMS `12.36 m`,
- internal hardware phase RMS ve p95-absolute `28.50/58.36 ns`, lock kaybi
`0/120`,
- IMU overflow sayaci `13..13`, tracking measurement missed sayaci `0..0`
kaldi.
Bu `hw_phase`, APB status'taki son commit residual'idir; fiziksel PPS pin
jitter'i degildir. Dolayisiyla `15-20 ns` pin hedefinin saglandigi bu logdan
iddia edilmez; scope veya time-interval counter ile ayri olculmelidir.
### 2026-07-08 ACQ Debug Notu
+23 -3
View File
@@ -29,6 +29,22 @@
#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
#define IMU_SAMPLES_AFTER_GPS_SERVICE 3u
static void update_eskf_doppler_bandwidth(
const navigation_fusion_output_t* output)
{
if((output == NULL) || !output->local_valid)
{
return;
}
double alpha = 0.05;
if(output->motion_state == FUSION_MOTION_MOVING)
{
alpha = (output->linear_acceleration_norm_mps2 >= 2.5) ?
0.40 : 0.20;
}
gps_receiver_set_eskf_doppler_alpha(alpha);
}
static void initialize_sensor_fusion(gps_system_state_t* system)
{
system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
@@ -69,9 +85,13 @@ static unsigned int feed_pending_imu_to_eskf(
imu_sensor_take_measurement(&system->imu_sample))
{
bool accepted = navigation_fusion_push_imu(&system->imu_sample);
if(accepted &&
navigation_fusion_get_output(&system->fused_solution) &&
system->fused_solution.global_valid &&
bool output_available = accepted &&
navigation_fusion_get_output(&system->fused_solution);
if(output_available)
{
update_eskf_doppler_bandwidth(&system->fused_solution);
}
if(output_available && system->fused_solution.global_valid &&
(system->fused_solution.timestamp_ticks !=
system->last_fused_location_tick))
{
@@ -19,6 +19,7 @@ typedef struct {
double height_m;
double position_std_m[3];
double speed_mps;
double linear_acceleration_norm_mps2;
double last_position_nis;
double last_velocity_nis;
fusion_motion_state_t motion_state;
@@ -55,6 +55,8 @@
void gps_background_task(void);
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
void gps_tracking_set_eskf_doppler_alpha(double alpha);
double gps_tracking_get_eskf_doppler_alpha(void);
void trck_intr_handler(volatile gps_trck_t* const trck_ip, volatile gps_frame_t *gps_frame_parsed, volatile int unsigned gps_subframe[10], unsigned char trck_ip_no);
@@ -38,4 +38,9 @@ bool gps_receiver_solve_ready_epoch(
uint64_t gps_receiver_status_ticks(void);
uint64_t gps_receiver_status_ticks_per_second(void);
/* Select the low-latency Doppler EWMA used only by velocity WLS/ESKF.
* PPS keeps its independent fixed low-noise path. */
void gps_receiver_set_eskf_doppler_alpha(double alpha);
double gps_receiver_get_eskf_doppler_alpha(void);
#endif
@@ -6,6 +6,8 @@
#include "gps/gps_ip_defines.h"
#define GPS_TRACK_PPS_DOPPLER_ALPHA 0.05
typedef struct {
uint32_t prn;
uint32_t sequence;
@@ -17,9 +19,12 @@ typedef struct {
int32_t cn0_sum_q;
uint64_t cn0_wbp;
unsigned int cn0_sample_count;
/* Raw coherent 20 ms carrier snapshot used by velocity WLS/ESKF. */
/* Raw coherent 20 ms carrier snapshot retained for diagnostics. */
double doppler_hz;
double pseudorange_rate_mps;
/* Motion-adaptive low-latency path published to velocity WLS/ESKF. */
double eskf_doppler_hz;
double eskf_pseudorange_rate_mps;
/* Slow software IIR path retained only for PPS bias-drift prediction. */
double filtered_doppler_hz;
double filtered_pseudorange_rate_mps;
@@ -115,7 +115,7 @@ void gps_system_uart_publish_periodic_fusion_status(
if(output->global_valid)
{
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
"state=%s lat=%.10lf lon=%.10lf height=%.3lf "
"speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf "
"gps=%lu/%lu velocity=%lu/%lu recoveries=%lu "
@@ -142,7 +142,7 @@ void gps_system_uart_publish_periodic_fusion_status(
return;
}
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
"state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
"imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
navigation_fusion_state_name(output->engine_state),
@@ -141,6 +141,8 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
}
output->speed_mps = eskf_speed_mps(&runtime->filter);
output->linear_acceleration_norm_mps2 =
runtime->motion.last_linear_accel_norm_mps2;
output->last_position_nis = runtime->filter.last_position_nis;
output->last_velocity_nis = runtime->filter.last_velocity_nis;
output->motion_state = runtime->motion.state;
@@ -9,6 +9,7 @@
#include <bsp/irq.h>
#include <bsp/mmio.h>
#include <bsp/plic.h>
#include <math.h>
#include <string.h>
typedef enum {
@@ -91,9 +92,13 @@ static gps_external_irq_handler_t gps_external_irq_handler = NULL;
(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u)
#define GPS_NAV_HOLDOFF_LOCAL_TICKS \
(((uint64_t)BSP_CLINT_TIMEBASE_HZ * GPS_NAV_EPOCH_HOLDOFF_MS) / 1000u)
#define GPS_TRACK_DOPPLER_SMOOTH_ALPHA 0.05
#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN 0.05
#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX 0.40
#define GPS_NAV_TOW_SNAP_TOLERANCE_SEC 0.005
static volatile double gps_tracking_eskf_doppler_alpha =
GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN;
_Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS <
(GPS_NAV_SOLVE_PERIOD_MS / 2u),
"NAV capture window must be below half the solve period");
@@ -112,6 +117,22 @@ static unsigned int gps_mask_count(uint32_t mask)
return count;
}
void gps_tracking_set_eskf_doppler_alpha(double alpha)
{
if(!isfinite(alpha))
{
return;
}
gps_tracking_eskf_doppler_alpha = fmax(
GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN,
fmin(GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX, alpha));
}
double gps_tracking_get_eskf_doppler_alpha(void)
{
return gps_tracking_eskf_doppler_alpha;
}
static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phase)
{
return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
@@ -157,12 +178,19 @@ static void gps_update_tracking_measurement_isr(
* 0.4 s time constant while retaining real receiver dynamics.
*/
measurement.filtered_doppler_hz =
GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz +
(1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) *
GPS_TRACK_PPS_DOPPLER_ALPHA * measurement.doppler_hz +
(1.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) *
previous->filtered_doppler_hz;
measurement.filtered_pseudorange_rate_mps =
gps_tracking_doppler_to_range_rate_mps(
measurement.filtered_doppler_hz);
double eskf_alpha = gps_tracking_eskf_doppler_alpha;
measurement.eskf_doppler_hz =
eskf_alpha * measurement.doppler_hz +
(1.0 - eskf_alpha) * previous->eskf_doppler_hz;
measurement.eskf_pseudorange_rate_mps =
gps_tracking_doppler_to_range_rate_mps(
measurement.eskf_doppler_hz);
}
measurement.local_timestamp_ticks = local_timestamp_ticks;
gps_latest_tracking_measurement[tracking_index] = measurement;
+63 -5
View File
@@ -69,11 +69,25 @@ static bool gps_pps_reference_svid_valid;
static unsigned int gps_pps_reference_svid;
static bool gps_pps_filter_reference_svid_valid;
static unsigned int gps_pps_filter_reference_svid;
#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0
static uint64_t gps_pps_predictor_ab_update_counter;
#endif
#if !defined(GPS_PPS_STATUS_LOG_INTERVAL) || (GPS_PPS_STATUS_LOG_INTERVAL < 1)
#error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one"
#endif
#if !defined(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) || \
((GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) && \
(GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 1))
#error "GPS_PPS_DOPPLER_PREDICTOR_ENABLE must be zero or one"
#endif
#if !defined(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) || \
(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES < 0)
#error "GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES must be zero or positive"
#endif
volatile uint64_t gps_systick_ticks = 0;
static void gps_init_acq_tables(void)
@@ -854,6 +868,9 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
bool pps_clock_bias_drift_valid = false;
double pps_clock_bias_drift_mps = 0.0;
bool pps_velocity_valid = false;
double eskf_doppler_alpha =
gps_tracking_get_eskf_doppler_alpha();
double pps_velocity_speed_mps = NAN;
double pps_velocity_residual_rms_mps = 0.0;
uint32_t pps_velocity_excluded_row_mask = 0u;
gps_nav_solution_t solution;
@@ -964,6 +981,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
}
if(ok && solution.solution_valid)
{
double reference_noise_ratio =
eskf_doppler_alpha *
(2.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) /
(GPS_TRACK_PPS_DOPPLER_ALPHA *
(2.0 - eskf_doppler_alpha));
double eskf_cn0_penalty_db =
10.0 * log10(fmax(1.0, reference_noise_ratio));
double satellite_positions_array[GPS_TRCK_IP_COUNT][3];
double satellite_velocities_array[GPS_TRCK_IP_COUNT][3];
gps_tracking_measurement_t
@@ -978,6 +1002,12 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
velocities->matrix[row][component];
}
velocity_measurements[row] = measurements[row];
velocity_measurements[row].doppler_hz =
measurements[row].eskf_doppler_hz;
velocity_measurements[row].pseudorange_rate_mps =
measurements[row].eskf_pseudorange_rate_mps;
velocity_measurements[row].cn0_dbhz -=
eskf_cn0_penalty_db;
if((solution.used_row_mask & (1u << row)) == 0u)
{
velocity_measurements[row].valid = false;
@@ -1011,10 +1041,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
/*
* PPS keeps the existing slow Doppler path as a low-noise derivative
* predictor for the position-WLS reference datum. This result is not
* published to fusion; fusion receives the raw robust solution above.
* published to fusion; fusion receives the adaptive low-latency
* solution above.
*/
for(unsigned int row = 0u; row < satellite_count; row++)
{
velocity_measurements[row].cn0_dbhz =
measurements[row].cn0_dbhz;
velocity_measurements[row].doppler_hz =
measurements[row].filtered_doppler_hz;
velocity_measurements[row].pseudorange_rate_mps =
@@ -1037,6 +1070,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
&pps_velocity_excluded_row_mask);
if(pps_velocity_valid)
{
pps_velocity_speed_mps =
sqrt(pps_velocity_ecef_mps[0] * pps_velocity_ecef_mps[0] +
pps_velocity_ecef_mps[1] * pps_velocity_ecef_mps[1] +
pps_velocity_ecef_mps[2] * pps_velocity_ecef_mps[2]);
pps_clock_bias_drift_valid =
gps_pps_reference_bias_drift(
solution.ecef_m,
@@ -1050,8 +1087,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
(pps_velocity_excluded_row_mask != 0u))
{
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
"velocity FDE raw_mask=0x%02lx pps_mask=0x%02lx "
"raw_rms=%.3lf pps_rms=%.3lf\r\n",
"velocity FDE adaptive_mask=0x%02lx pps_mask=0x%02lx "
"adaptive_rms=%.3lf pps_rms=%.3lf\r\n",
(unsigned long)solution.velocity_excluded_row_mask,
(unsigned long)pps_velocity_excluded_row_mask,
solution.velocity_residual_rms_mps,
@@ -1178,11 +1215,21 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
}
double filtered_clock_bias_m = 0.0;
double clock_innovation_m = 0.0;
bool pps_predictor_enabled =
(GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) &&
pps_clock_bias_drift_valid;
#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0
pps_predictor_enabled =
(((gps_pps_predictor_ab_update_counter /
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) & 1u) != 0u) &&
pps_clock_bias_drift_valid;
gps_pps_predictor_ab_update_counter++;
#endif
gps_pps_time_filter_result_t filter_result =
gps_pps_filter_clock_bias(
pps_reference_counter,
solution.clock_bias_m,
pps_clock_bias_drift_valid,
pps_predictor_enabled,
pps_clock_bias_drift_mps,
solution.tdop,
&filtered_clock_bias_m,
@@ -1244,7 +1291,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
"PPS profile=%s mode=%s locked=%u tdop=%.2lf gdop=%.2lf "
"clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm "
"raw_bias=%.3lfm filtered_bias=%.3lfm "
"clock_drift=%.3lfmps bias_drift=%.3lfmps ref_svid=%u "
"clock_drift=%.3lfmps bias_drift=%.3lfmps "
"predictor=%u eskf_alpha=%.2lf "
"eskf_v=%.3lf/%.3lf/0x%02lx "
"pps_v=%.3lf/%.3lf/0x%02lx ref_svid=%u "
"capture_corr=%.2lfns "
"hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm "
"pulse=%u seq=%u status=0x%03x\r\n",
@@ -1261,6 +1311,14 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
filtered_clock_bias_m,
solution.clock_drift_mps,
pps_clock_bias_drift_mps,
pps_predictor_enabled ? 1u : 0u,
eskf_doppler_alpha,
solution.speed_mps,
solution.velocity_residual_rms_mps,
(unsigned long)solution.velocity_excluded_row_mask,
pps_velocity_speed_mps,
pps_velocity_residual_rms_mps,
(unsigned long)pps_velocity_excluded_row_mask,
ephemeris[reference_index].SVID,
pps_capture_correction_sec * 1.0e9,
hardware_phase_ns, rate_command_ppm, integrator_ppm,
@@ -42,3 +42,13 @@ uint64_t gps_receiver_status_ticks_per_second(void)
{
return GPS_SYSTICK_HZ;
}
void gps_receiver_set_eskf_doppler_alpha(double alpha)
{
gps_tracking_set_eskf_doppler_alpha(alpha);
}
double gps_receiver_get_eskf_doppler_alpha(void)
{
return gps_tracking_get_eskf_doppler_alpha();
}
@@ -141,6 +141,10 @@ bool gps_tracking_measurement_read(
measurement_out->doppler_hz;
measurement_out->filtered_pseudorange_rate_mps =
measurement_out->pseudorange_rate_mps;
measurement_out->eskf_doppler_hz =
measurement_out->doppler_hz;
measurement_out->eskf_pseudorange_rate_mps =
measurement_out->pseudorange_rate_mps;
unsigned int version =
meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT;
@@ -156,6 +160,8 @@ bool gps_tracking_measurement_read(
cn0_valid &&
isfinite(measurement_out->doppler_hz) &&
isfinite(measurement_out->pseudorange_rate_mps) &&
isfinite(measurement_out->eskf_doppler_hz) &&
isfinite(measurement_out->eskf_pseudorange_rate_mps) &&
isfinite(measurement_out->filtered_doppler_hz) &&
isfinite(measurement_out->filtered_pseudorange_rate_mps);
return measurement_out->valid;
@@ -138,6 +138,99 @@ static void test_bias_ramp_without_doppler_uses_bounded_fallback(void)
assert(sqrt(squared_error_sum / (double)tracked) < 45.0);
}
static void test_board_calibrated_doppler_predictor_beats_fallback(void)
{
/* Values measured on Genesys2 on 2026-07-26: clock-bias sigma was
* 15.87 m and reference-datum drift was about 291.75 m/s. Feed the
* exact same deterministic WLS noise to two filters so this is a true
* predictor on/off A/B, not two unrelated Monte-Carlo runs. */
static const double bias_noise_m[] = {
8.0, -18.0, 12.0, -7.0, 21.0, -14.0, 5.0, -25.0,
16.0, -3.0, 10.0, -20.0, 6.0, 24.0, -11.0, 2.0
};
static const double drift_noise_mps[] = {
1.8, -3.2, 4.1, -2.4, 0.7, 3.5, -4.3, 2.6,
-1.1, 4.6, -2.8, 1.5, -3.7, 2.2, -0.4, 3.0
};
gps_pps_time_filter_t aided;
gps_pps_time_filter_t fallback;
gps_pps_time_filter_init(&aided);
gps_pps_time_filter_init(&fallback);
double aided_squared_error_sum = 0.0;
double fallback_squared_error_sum = 0.0;
unsigned int tracked = 0u;
for(unsigned int update = 0u; update < 900u; update++)
{
double time_sec = 0.2 * (double)update;
double long_omega = 2.0 * 3.14159265358979323846 / 75.0;
double short_omega = 2.0 * 3.14159265358979323846 / 19.0;
double varying_bias_m =
5.0 * (1.0 - cos(long_omega * time_sec)) / long_omega +
2.0 * (1.0 - cos(short_omega * time_sec)) / short_omega;
double true_drift_mps =
291.75 + 5.0 * sin(long_omega * time_sec) +
2.0 * sin(short_omega * time_sec);
double true_bias_m =
159624.654 + 291.75 * time_sec + varying_bias_m;
double measured_bias_m = true_bias_m +
bias_noise_m[update %
(sizeof(bias_noise_m) / sizeof(bias_noise_m[0]))];
double aided_bias_m = 0.0;
double fallback_bias_m = 0.0;
double innovation_m = 0.0;
gps_pps_time_filter_result_t aided_result =
gps_pps_time_filter_update(
&aided,
(uint64_t)llround(time_sec * COUNTER_HZ),
COUNTER_HZ,
measured_bias_m,
true,
true_drift_mps + drift_noise_mps[update %
(sizeof(drift_noise_mps) /
sizeof(drift_noise_mps[0]))],
1.5,
1.0 / 32.0,
12u,
&aided_bias_m,
&innovation_m);
gps_pps_time_filter_result_t fallback_result =
gps_pps_time_filter_update(
&fallback,
(uint64_t)llround(time_sec * COUNTER_HZ),
COUNTER_HZ,
measured_bias_m,
false,
0.0,
1.5,
1.0 / 32.0,
12u,
&fallback_bias_m,
&innovation_m);
assert(aided_result != GPS_PPS_TIME_FILTER_REJECTED);
assert(fallback_result != GPS_PPS_TIME_FILTER_REJECTED);
if((update >= 300u) &&
(aided_result == GPS_PPS_TIME_FILTER_TRACKING) &&
(fallback_result == GPS_PPS_TIME_FILTER_TRACKING))
{
double aided_error_m = aided_bias_m - true_bias_m;
double fallback_error_m = fallback_bias_m - true_bias_m;
aided_squared_error_sum += aided_error_m * aided_error_m;
fallback_squared_error_sum +=
fallback_error_m * fallback_error_m;
tracked++;
}
}
assert(tracked > 550u);
double aided_rms_m = sqrt(aided_squared_error_sum / (double)tracked);
double fallback_rms_m =
sqrt(fallback_squared_error_sum / (double)tracked);
printf("board PPS bias RMS: Doppler=%.3f m fallback=%.3f m\n",
aided_rms_m, fallback_rms_m);
assert(aided_rms_m < 8.0);
assert(aided_rms_m < 0.35 * fallback_rms_m);
}
static void test_reference_geometry_defines_bias_drift(void)
{
const double receiver_position_m[3] = {0.0, 0.0, 0.0};
@@ -208,6 +301,7 @@ int main(void)
test_drift_prediction_rejects_bias_noise();
test_quality_gap_and_outlier_handling();
test_bias_ramp_without_doppler_uses_bounded_fallback();
test_board_calibrated_doppler_predictor_beats_fallback();
test_reference_geometry_defines_bias_drift();
test_reference_selection_is_stable();
puts("gps_pps_time_filter tests passed");
@@ -86,6 +86,14 @@ static void test_cn0_and_mailbox_decode(void)
measurement.filtered_pseudorange_rate_mps,
measurement.pseudorange_rate_mps,
1.0e-12));
assert(close_to(
measurement.eskf_doppler_hz,
measurement.doppler_hz,
1.0e-12));
assert(close_to(
measurement.eskf_pseudorange_rate_mps,
measurement.pseudorange_rate_mps,
1.0e-12));
registers.MEASUREMENT_META &=
~(1u << GPS_TRCK_MEAS_META_LOCK_POS);