fix(gps): adapt Doppler bandwidth from board validation
This commit is contained in:
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user