mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-08-07 22:43:52 +00:00
Code Issues Releases Wiki Activity

Merge branch 'vtl' into ft/extra-data

Browse Source
This commit is contained in:
Victor Castillo 2024-08-16 15:54:04 +02:00
commit b60be6e56a
No known key found for this signature in database
GPG Key ID: 8EF1FC8B7182F608
116 changed files with 15913 additions and 10323 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

188
conf/File_input/MultiCons/gnss-sdr_labsat_kf.conf
View File

@ -1,3 +1,190 @@
<<<<<<< HEAD:conf/gnss-sdr_labsat_kf.conf
; This is a GNSS-SDR configuration file
; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/
; SPDX-License-Identifier: GPL-3.0-or-later
; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors)
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
GNSS-SDR.internal_fs_sps=5456000
GNSS-SDR.use_acquisition_resampler=true
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=Labsat_Signal_Source
SignalSource.selected_channel=1
;#filename: path to file with the captured GNSS signal samples to be processed
;# Labsat sile source automatically increments the file name when the signal is split in several files
;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on
;# in this example, the first file complete path will be ../signals/GPS_025_
SignalSource.filename=/home/javier/signals/satgen_30m/output/output
SignalSource.item_type=gr_complex
SignalSource.sampling_frequency=16368000
SignalSource.samples=0
SignalSource.repeat=false
SignalSource.dump=false
SignalSource.dump_filename=./out.dat
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
;######### DATA_TYPE_ADAPTER CONFIG ############
DataTypeAdapter.implementation=Pass_Through
DataTypeAdapter.item_type=gr_complex
;######### INPUT_FILTER CONFIG ############
InputFilter.implementation=Freq_Xlating_Fir_Filter
InputFilter.dump=false
InputFilter.dump_filename=/media/javier/WDNASNTFS/output_5.456Msps_gr_complex.dat
InputFilter.input_item_type=gr_complex
InputFilter.output_item_type=gr_complex
InputFilter.taps_item_type=float
InputFilter.number_of_taps=5
InputFilter.number_of_bands=2
InputFilter.band1_begin=0.0
InputFilter.band1_end=0.45
InputFilter.band2_begin=0.55
InputFilter.band2_end=1.0
InputFilter.ampl1_begin=1.0
InputFilter.ampl1_end=1.0
InputFilter.ampl2_begin=0.0
InputFilter.ampl2_end=0.0
InputFilter.band1_error=1.0
InputFilter.band2_error=1.0
InputFilter.filter_type=lowpass
InputFilter.grid_density=16
InputFilter.sampling_frequency=16368000
InputFilter.IF=0
InputFilter.decimation_factor=3
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=6
Channels_1B.count=0
Channels_L5.count=0
Channels_5X.count=0
Channels.in_acquisition=1
;######### GPS ACQUISITION CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.threshold=3.0
Acquisition_1C.use_CFAR_algorithm=false
Acquisition_1C.blocking=true
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=125
Acquisition_1C.dump=false
Acquisition_1C.dump_filename=./acq_dump.dat
;######### GALILEO ACQUISITION CONFIG ############
Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition
Acquisition_1B.item_type=gr_complex
Acquisition_1B.threshold=2.8
Acquisition_1B.use_CFAR_algorithm=false
Acquisition_1B.blocking=false
Acquisition_1B.doppler_max=5000
Acquisition_1B.doppler_step=125
Acquisition_1B.dump=false
Acquisition_1B.dump_filename=./acq_dump.dat
;######### TRACKING GPS CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_KF_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.dump=false
Tracking_1C.dump_filename=./tracking_ch_
Tracking_1C.extend_correlation_symbols=20;
Tracking_1C.early_late_space_chips=0.5;
Tracking_1C.early_late_space_narrow_chips=0.15
;Tracking_1C.code_disc_sd_chips=0.2; // Initial R
;Tracking_1C.carrier_disc_sd_rads=0.3; // Initial R
;Tracking_1C.init_code_phase_sd_chips=0.5; // Initial P_0_0
;Tracking_1C.init_carrier_phase_sd_rad=0.7;
;Tracking_1C.init_carrier_freq_sd_hz=5;
;Tracking_1C.init_carrier_freq_rate_sd_hz_s=1;
;Tracking_1C.code_phase_sd_chips=0.15; // Initial Q
;Tracking_1C.carrier_phase_sd_rad=0.25;
;Tracking_1C.carrier_freq_sd_hz=0.6;
;Tracking_1C.carrier_freq_rate_sd_hz_s=0.01;
;######### TRACKING GALILEO CONFIG ############
Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking
Tracking_1B.item_type=gr_complex
Tracking_1B.pll_bw_hz=15.0;
Tracking_1B.dll_bw_hz=0.75;
Tracking_1B.early_late_space_chips=0.15;
Tracking_1B.very_early_late_space_chips=0.5;
Tracking_1B.early_late_space_narrow_chips=0.10;
Tracking_1B.very_early_late_space_narrow_chips=0.5;
Tracking_1B.pll_bw_narrow_hz=2.5
Tracking_1B.dll_bw_narrow_hz=0.2
Tracking_1B.extend_correlation_symbols=5
Tracking_1B.track_pilot=true
Tracking_1B.enable_fll_pull_in=true;
;Tracking_1B.pull_in_time_s=60
Tracking_1B.enable_fll_steady_state=false
Tracking_1B.fll_bw_hz=10
Tracking_1B.dump=false
Tracking_1B.dump_filename=tracking_ch_
;######### TELEMETRY DECODER GALILEO CONFIG ############
TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder
TelemetryDecoder_1B.dump=false
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
TelemetryDecoder_1C.dump=false
;######### OBSERVABLES CONFIG ############
;#implementation:
Observables.implementation=Hybrid_Observables
Observables.dump=false
Observables.dump_filename=./observables.dat
Observables.enable_carrier_smoothing=false
Observables.smoothing_factor=200
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.enable_rx_clock_correction=false
PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=1000;
PVT.rinexobs_rate_ms=1000;
PVT.display_rate_ms=1000;
PVT.elevation_mask=15;
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=false
PVT.dump_filename=./PVT
PVT.enable_monitor=false
PVT.monitor_udp_port=1337
PVT.monitor_client_addresses=127.0.0.1
;######### MONITOR CONFIG ############
Monitor.enable_monitor=false
Monitor.decimation_factor=1
Monitor.client_addresses=127.0.0.1
Monitor.udp_port=1234
=======
; This is a GNSS-SDR configuration file
; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/
; SPDX-License-Identifier: GPL-3.0-or-later
@ -183,3 +370,4 @@ Monitor.enable_monitor=false
Monitor.decimation_factor=1
Monitor.client_addresses=127.0.0.1
Monitor.udp_port=1234
>>>>>>> next:conf/File_input/MultiCons/gnss-sdr_labsat_kf.conf

187
conf/vtl/gnss-sdr_labsat_kf_vtl.conf Normal file
View File

@ -0,0 +1,187 @@
; This is a GNSS-SDR configuration file
; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/
; SPDX-License-Identifier: GPL-3.0-or-later
; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors)
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
GNSS-SDR.internal_fs_sps=5456000
GNSS-SDR.use_acquisition_resampler=true
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=Labsat_Signal_Source
SignalSource.selected_channel=1
;#filename: path to file with the captured GNSS signal samples to be processed
;# Labsat sile source automatically increments the file name when the signal is split in several files
;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on
;# in this example, the first file complete path will be ../signals/GPS_025_
SignalSource.filename=/home/javier/signals/satgen_30m/output/output
SignalSource.item_type=gr_complex
SignalSource.sampling_frequency=16368000
SignalSource.samples=0
SignalSource.repeat=false
SignalSource.dump=false
SignalSource.dump_filename=./out.dat
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
;######### DATA_TYPE_ADAPTER CONFIG ############
DataTypeAdapter.implementation=Pass_Through
DataTypeAdapter.item_type=gr_complex
;######### INPUT_FILTER CONFIG ############
InputFilter.implementation=Freq_Xlating_Fir_Filter
InputFilter.dump=false
InputFilter.dump_filename=/media/javier/WDNASNTFS/output_5.456Msps_gr_complex.dat
InputFilter.input_item_type=gr_complex
InputFilter.output_item_type=gr_complex
InputFilter.taps_item_type=float
InputFilter.number_of_taps=5
InputFilter.number_of_bands=2
InputFilter.band1_begin=0.0
InputFilter.band1_end=0.45
InputFilter.band2_begin=0.55
InputFilter.band2_end=1.0
InputFilter.ampl1_begin=1.0
InputFilter.ampl1_end=1.0
InputFilter.ampl2_begin=0.0
InputFilter.ampl2_end=0.0
InputFilter.band1_error=1.0
InputFilter.band2_error=1.0
InputFilter.filter_type=lowpass
InputFilter.grid_density=16
InputFilter.sampling_frequency=16368000
InputFilter.IF=0
InputFilter.decimation_factor=3
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=6
Channels_1B.count=0
Channels_L5.count=0
Channels_5X.count=0
Channels.in_acquisition=1
;######### GPS ACQUISITION CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.threshold=3.0
Acquisition_1C.use_CFAR_algorithm=false
Acquisition_1C.blocking=true
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=125
Acquisition_1C.dump=false
Acquisition_1C.dump_filename=./acq_dump.dat
;######### GALILEO ACQUISITION CONFIG ############
Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition
Acquisition_1B.item_type=gr_complex
Acquisition_1B.threshold=2.8
Acquisition_1B.use_CFAR_algorithm=false
Acquisition_1B.blocking=false
Acquisition_1B.doppler_max=5000
Acquisition_1B.doppler_step=125
Acquisition_1B.dump=false
Acquisition_1B.dump_filename=./acq_dump.dat
;######### TRACKING GPS CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_KF_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.dump=false
Tracking_1C.dump_filename=./tracking_ch_
Tracking_1C.extend_correlation_symbols=1;
Tracking_1C.early_late_space_chips=0.5;
Tracking_1C.early_late_space_narrow_chips=0.15
;Tracking_1C.code_disc_sd_chips=0.2; // Initial R
;Tracking_1C.carrier_disc_sd_rads=0.3; // Initial R
;Tracking_1C.init_code_phase_sd_chips=0.5; // Initial P_0_0
;Tracking_1C.init_carrier_phase_sd_rad=0.7;
;Tracking_1C.init_carrier_freq_sd_hz=5;
;Tracking_1C.init_carrier_freq_rate_sd_hz_s=1;
;Tracking_1C.code_phase_sd_chips=0.15; // Initial Q
;Tracking_1C.carrier_phase_sd_rad=0.25;
;Tracking_1C.carrier_freq_sd_hz=0.6;
;Tracking_1C.carrier_freq_rate_sd_hz_s=0.01;
;######### TRACKING GALILEO CONFIG ############
Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking
Tracking_1B.item_type=gr_complex
Tracking_1B.pll_bw_hz=15.0;
Tracking_1B.dll_bw_hz=0.75;
Tracking_1B.early_late_space_chips=0.15;
Tracking_1B.very_early_late_space_chips=0.5;
Tracking_1B.early_late_space_narrow_chips=0.10;
Tracking_1B.very_early_late_space_narrow_chips=0.5;
Tracking_1B.pll_bw_narrow_hz=2.5
Tracking_1B.dll_bw_narrow_hz=0.2
Tracking_1B.extend_correlation_symbols=5
Tracking_1B.track_pilot=true
Tracking_1B.enable_fll_pull_in=true;
;Tracking_1B.pull_in_time_s=60
Tracking_1B.enable_fll_steady_state=false
Tracking_1B.fll_bw_hz=10
Tracking_1B.dump=false
Tracking_1B.dump_filename=tracking_ch_
;######### TELEMETRY DECODER GALILEO CONFIG ############
TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder
TelemetryDecoder_1B.dump=false
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
TelemetryDecoder_1C.dump=false
;######### OBSERVABLES CONFIG ############
;#implementation:
Observables.implementation=Hybrid_Observables
Observables.dump=false
Observables.dump_filename=./observables.dat
Observables.enable_carrier_smoothing=false
Observables.smoothing_factor=200
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.enable_rx_clock_correction=false
PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=100;
PVT.rinexobs_rate_ms=1000;
PVT.display_rate_ms=1000;
PVT.elevation_mask=15;
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=false
PVT.dump_filename=./PVT
PVT.enable_monitor=false
PVT.monitor_udp_port=1337
PVT.monitor_client_addresses=127.0.0.1
PVT.enable_vtl=false
PVT.close_vtl_loop=true
;######### MONITOR CONFIG ############
Monitor.enable_monitor=false
Monitor.decimation_factor=1
Monitor.client_addresses=127.0.0.1
Monitor.udp_port=1234

121
conf/vtl/spirent_usrp_vtl.conf Normal file
View File

@ -0,0 +1,121 @@
; This is a GNSS-SDR configuration file
; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/
; SPDX-License-Identifier: GPL-3.0-or-later
; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors)
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
GNSS-SDR.internal_fs_sps=5000000
GNSS-SDR.telecommand_enabled=false
GNSS-SDR.telecommand_tcp_port=3333
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=File_Signal_Source
SignalSource.filename=/media/sf_shareFolder/myWork/data/AIRING/SPF-LD-05-12H03_CH0.iq
SignalSource.item_type=ishort
SignalSource.sampling_frequency=5000000
SignalSource.samples=0
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
DataTypeAdapter.implementation=Ishort_To_Complex
InputFilter.implementation=Pass_Through
InputFilter.item_type=gr_complex
Resampler.implementation=Direct_Resampler
Resampler.sample_freq_in=5000000
Resampler.sample_freq_out=5000000
Resampler.item_type=gr_complex
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=6
Channels.in_acquisition=6
Channel.signal=1C
Channel0.satellite=28
Channel1.satellite=4
Channel2.satellite=17
Channel3.satellite=15
Channel4.satellite=27
Channel5.satellite=9
;######### ACQUISITION GLOBAL CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.pfa=0.01
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=250
Acquisition_1C.blocking=true
;######### TRACKING GLOBAL CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_KF_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.extend_correlation_symbols=1;
Tracking_1C.early_late_space_chips=0.5;
Tracking_1C.early_late_space_narrow_chips=0.15
;Tracking_1C.code_disc_sd_chips=0.2; // Initial R
;Tracking_1C.carrier_disc_sd_rads=0.3; // Initial R
;Tracking_1C.init_code_phase_sd_chips=0.5; // Initial P_0_0
;Tracking_1C.init_carrier_phase_sd_rad=0.7;
;Tracking_1C.init_carrier_freq_sd_hz=5;
;Tracking_1C.init_carrier_freq_rate_sd_hz_s=1;
;Tracking_1C.code_phase_sd_chips=0.15; // Initial Q
;Tracking_1C.carrier_phase_sd_rad=0.25;
;Tracking_1C.carrier_freq_sd_hz=0.6;
;Tracking_1C.carrier_freq_rate_sd_hz_s=0.01;
Tracking_1C.dump=true
Tracking_1C.dump_filename=./tracking/tracking_raw.dat
Tracking_1C.dump_mat=true
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
;######### OBSERVABLES CONFIG ############
Observables.implementation=Hybrid_Observables
Observables.dump=true
Observables.dump_mat=true
Observables.dump_filename=./observables/observables_raw.dat
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.enable_rx_clock_correction=false
PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=100;
PVT.rinexobs_rate_ms=100;
PVT.display_rate_ms=100;
PVT.elevation_mask=15;
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=true
PVT.dump_mat=true
PVT.dump_filename=./PVT_raw
PVT.gpx_output_enabled=false
PVT.geojson_output_enabled=false
PVT.kml_output_enabled=false
PVT.xml_output_enabled=false
PVT.rinex_output_enabled=false
PVT.nmea_output_file_enabled=false
PVT.enable_vtl=true
PVT.close_vtl_loop=true
;######### PVT MONITOR CONFIG ############
PVT.enable_monitor=false
PVT.monitor_client_addresses=127.0.0.1
PVT.monitor_udp_port=1111
;######### MONITOR CONFIG ############
Monitor.enable_monitor=false
Monitor.decimation_factor=1
Monitor.client_addresses=127.0.0.1
Monitor.udp_port=1112

107
conf/vtl/spirent_usrp_vtl_no_kf_tracking.conf Normal file
View File

@ -0,0 +1,107 @@
; This is a GNSS-SDR configuration file
; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/
; SPDX-License-Identifier: GPL-3.0-or-later
; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors)
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
GNSS-SDR.internal_fs_sps=5000000
GNSS-SDR.telecommand_enabled=false
GNSS-SDR.telecommand_tcp_port=3333
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=File_Signal_Source
SignalSource.filename=/media/sf_shareFolder/Spirent/SPF-LD-05-12H03_CH0.iq
SignalSource.item_type=ishort
SignalSource.sampling_frequency=5000000
SignalSource.samples=0
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
DataTypeAdapter.implementation=Ishort_To_Complex
InputFilter.implementation=Pass_Through
InputFilter.item_type=gr_complex
Resampler.implementation=Direct_Resampler
Resampler.sample_freq_in=5000000
Resampler.sample_freq_out=5000000
Resampler.item_type=gr_complex
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=6
Channels.in_acquisition=6
Channel.signal=1C
Channel0.satellite=28
Channel1.satellite=4
Channel2.satellite=17
Channel3.satellite=15
Channel4.satellite=27
Channel5.satellite=9
;######### ACQUISITION GLOBAL CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.pfa=0.01
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=250
Acquisition_1C.blocking=true
;######### TRACKING GLOBAL CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.pll_bw_hz=40.0;
Tracking_1C.dll_bw_hz=4.0;
Tracking_1C.dump=true
Tracking_1C.dump_filename=./tracking/tracking_raw.dat
Tracking_1C.dump_mat=true
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
;######### OBSERVABLES CONFIG ############
Observables.implementation=Hybrid_Observables
Observables.dump=true
Observables.dump_mat=true
Observables.dump_filename=./observables/observables_raw.dat
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.enable_rx_clock_correction=false
PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=100;
PVT.rinexobs_rate_ms=100;
PVT.display_rate_ms=100;
PVT.elevation_mask=15;
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=true
PVT.dump_mat=true
PVT.dump_filename=./PVT_raw
PVT.gpx_output_enabled=false
PVT.geojson_output_enabled=false
PVT.kml_output_enabled=false
PVT.xml_output_enabled=false
PVT.rinex_output_enabled=false
PVT.nmea_output_file_enabled=false
PVT.enable_vtl=true
PVT.close_vtl_loop=true
;######### PVT MONITOR CONFIG ############
PVT.enable_monitor=false
PVT.monitor_client_addresses=127.0.0.1
PVT.monitor_udp_port=1111
;######### MONITOR CONFIG ############
Monitor.enable_monitor=false
Monitor.decimation_factor=1
Monitor.client_addresses=127.0.0.1
Monitor.udp_port=1112

3
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -919,6 +919,9 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
// Use unhealthy satellites
pvt_output_parameters.use_unhealthy_sats = configuration->property(role + ".use_unhealthy_sats", pvt_output_parameters.use_unhealthy_sats);
// Vector Tracking Loop (VTL)
pvt_output_parameters.enable_vtl = configuration->property(role + ".enable_vtl", pvt_output_parameters.enable_vtl);
pvt_output_parameters.close_vtl_loop = configuration->property(role + ".close_vtl_loop", pvt_output_parameters.close_vtl_loop);
// make PVT object
pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";

95
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -183,6 +183,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
d_enable_rx_clock_correction(conf_.enable_rx_clock_correction),
d_an_printer_enabled(conf_.an_output_enabled),
d_log_timetag(conf_.log_source_timetag),
d_use_e6_for_pvt(conf_.use_e6_for_pvt),
d_enable_vtl(conf_.enable_vtl),
d_close_vtl_loop(conf_.close_vtl_loop),
d_use_has_corrections(conf_.use_has_corrections),
d_use_unhealthy_sats(conf_.use_unhealthy_sats)
{
@ -255,6 +258,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
std::cerr << "GNSS-SDR cannot create dump file for the PVT block. Wrong permissions?\n";
d_dump = false;
}
// TODO: if(vtl_enable) then
// uint end_filename = d_dump_filename.length()-4;
// d_vtl_dump_filename = d_dump_filename.insert(end_filename, "_vtl");
}
// initialize kml_printer
@ -2111,8 +2117,83 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
// old_time_debug = d_gnss_observables_map.cbegin()->second.RX_time * 1000.0;
uint32_t current_RX_time_ms = 0;
// #### solve PVT and store the corrected observable set
if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0))
if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, false, d_enable_vtl, d_close_vtl_loop))
{
// ****** experimental VTL tests
if (d_close_vtl_loop == true and d_enable_vtl == true)
{
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
int idx = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin();
gnss_observables_iter != d_gnss_observables_map.cend();
++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system
{
try
{
d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx).channel_id = gnss_observables_iter->second.Channel_ID;
// todo: VTL loop CAN NOT run every PVT epoch because it is required to wait for the corrections to be applied to the tracking KF.
// currently the VTL runs every PVT epoch which will create inestabilities.
const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx));
this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
idx++;
}
catch (std::exception& ex)
{
std::cout << "VTL Engine problem: " << ex.what() << "\n";
}
}
d_internal_pvt_solver->vtl_engine.trk_cmd_outs.clear();
// Dummy messages for evaluation of msg latency
// std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
// for (gnss_observables_iter = d_gnss_observables_map.cbegin();
// gnss_observables_iter != d_gnss_observables_map.cend();
// ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system
// {
// // test complete loop
// if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0)
// {
// // send new tracking command
// const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
// trk_cmd_test->carrier_freq_hz = 12345.4;
// trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter;
// trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID;
// this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
// d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter;
// std::cout << "msg pvt_to_trk sent.\n";
// }
// else
// {
// // std::cout << "CH " << gnss_observables_iter->second.Channel_ID
// // << " T_RX: " << static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) / static_cast<float>(gnss_observables_iter->second.fs)
// // << " T_last_vtl_trk: " << static_cast<float>(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast<float>(gnss_observables_iter->second.fs)
// // << " T_map: " << static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast<float>(gnss_observables_iter->second.fs)
// // << " T2: " << static_cast<float>(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])
// // << " T3: " << static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n";
//
// // To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map...
// if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])
// {
// std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is "
// << ((static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast<float>(gnss_observables_iter->second.fs)) * 1000.0
// << " [ms]!\n";
// // send new tracking command
// const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
// trk_cmd_test->carrier_freq_hz = 12345.4;
// trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter;
// trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID;
// this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
// d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter;
// std::cout << "msg pvt_to_trk sent.\n";
// }
// }
// }
}
else
{
std::cout << "Loop open!\n";
}
// *****************************
d_pvt_errors_counter = 0; // Reset consecutive PVT error counter
const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s();
@ -2225,18 +2306,14 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
// compute on the fly PVT solution
if (flag_compute_pvt_output == true)
{
flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_output_rate_ms / 1000.0);
// VTP To.Do: Check why get_PVT is triggered twice. Leave only one get_PVT.
// flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false);
flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, false, d_enable_vtl, d_close_vtl_loop);
}
if (flag_pvt_valid == true)
{
// experimental VTL tests
// send tracking command
// const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
// trk_cmd_test->carrier_freq_hz = 12345.4;
// trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter;
// this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
// initialize (if needed) the accumulated phase offset and apply it to the active channels
// required to report accumulated phase cycles comparable to pseudoranges
initialize_and_apply_carrier_phase_offset();

4
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@ -276,6 +276,10 @@ private:
bool d_enable_has_messages;
bool d_an_printer_enabled;
bool d_log_timetag;
bool d_use_e6_for_pvt;
bool d_enable_vtl;
bool d_close_vtl_loop;
std::map<int, uint64_t> d_last_sent_vtl_cmd_samplestamp_map;
bool d_use_has_corrections;
bool d_use_unhealthy_sats;
};

6
src/algorithms/PVT/libs/CMakeLists.txt
View File

@ -22,6 +22,9 @@ set(PVT_LIB_SOURCES
monitor_pvt_udp_sink.cc
monitor_ephemeris_udp_sink.cc
has_simple_printer.cc
vtl_conf.cc
vtl_data.cc
vtl_engine.cc
geohash.cc
pvt_kf.cc
)
@ -45,6 +48,9 @@ set(PVT_LIB_HEADERS
serdes_gps_eph.h
monitor_ephemeris_udp_sink.h
has_simple_printer.h
vtl_conf.h
vtl_data.h
vtl_engine.h
geohash.h
pvt_kf.h
)

2
src/algorithms/PVT/libs/pvt_conf.h
View File

@ -93,6 +93,8 @@ public:
bool dump_mat = true;
bool log_source_timetag = false;
bool use_e6_for_pvt = true;
bool enable_vtl = false;
bool close_vtl_loop = true;
bool use_has_corrections = true;
bool use_unhealthy_sats = false;

509
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -33,15 +33,19 @@
#include "rtklib_solver.h"
#include "Beidou_DNAV.h"
#include "gnss_sdr_filesystem.h"
#include "rtklib_conversions.h"
#include "rtklib_ephemeris.h"
#include "rtklib_rtkpos.h"
#include "rtklib_solution.h"
#include <matio.h>
#include "iostream"
#include <algorithm>
#include <cmath>
#include <exception>
#include <utility>
#include <vector>
using namespace std;
#if USE_GLOG_AND_GFLAGS
#include <glog/logging.h>
#else
@ -60,6 +64,13 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
d_flag_dump_enabled(flag_dump_to_file),
d_flag_dump_mat_enabled(flag_dump_to_mat)
{
// TODO: temporal VTL config parameters are hardcoded here. Move it to PVT adapter config (javi)
Vtl_Conf new_vtl_conf;
// TODO: new_vtl_conf.parameter1=blablabla
vtl_engine.configure(new_vtl_conf);
vtl_engine.reset();
// see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
// function: satwavelen
d_rtklib_freq_index[0] = 0;
@ -156,6 +167,23 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what();
}
}
// TODO: if(vtl_enable) then
if (d_vtl_dump_file.is_open() == false)
{
try
{
d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit);
uint end_filename = d_dump_filename.length() - 4;
d_vtl_dump_filename = d_dump_filename;
d_vtl_dump_filename = d_vtl_dump_filename.insert(end_filename, "_vtl");
d_vtl_dump_file.open(d_vtl_dump_filename, std::ios::out | std::ios::binary);
LOG(INFO) << "PVT VTL dump enabled Log file: " << d_vtl_dump_filename;
}
catch (const std::ofstream::failure &e)
{
LOG(WARNING) << "Exception opening VTL dump file " << e.what();
}
}
}
}
@ -195,6 +223,277 @@ Rtklib_Solver::~Rtklib_Solver()
LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what();
}
}
if (d_flag_dump_mat_enabled)
{
try
{
save_vtl_matfile();
}
catch (const std::exception &ex)
{
LOG(WARNING) << "Exception in destructor saving the PVT VTL .mat dump file " << ex.what();
}
}
}
bool Rtklib_Solver::save_vtl_matfile() const
{
// READ DUMP FILE
const std::string dump_filename = d_vtl_dump_filename;
const int32_t number_of_double_vars = 29;
const int32_t number_of_uint32_vars = 2;
const int32_t number_of_uint8_vars = 1;
const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
sizeof(uint32_t) * number_of_uint32_vars +
sizeof(uint8_t) * number_of_uint8_vars;
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
{
dump_file.open(dump_filename, std::ios::binary | std::ios::ate);
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem opening VTL dump file:" << e.what() << '\n';
return false;
}
// count number of epochs and rewind
int64_t num_epoch = 0LL;
if (dump_file.is_open())
{
std::cout << "Generating .mat file for VTL " << dump_filename << '\n';
const std::ifstream::pos_type size = dump_file.tellg();
num_epoch = static_cast<int64_t>(size) / static_cast<int64_t>(epoch_size_bytes);
dump_file.seekg(0, std::ios::beg);
}
else
{
return false;
}
auto TOW_at_current_symbol_ms = std::vector<uint32_t>(num_epoch);
auto week = std::vector<uint32_t>(num_epoch);
auto RX_time = std::vector<double>(num_epoch);
auto user_clk_offset = std::vector<double>(num_epoch);
auto user_clk_offset_drift = std::vector<double>(num_epoch);
auto rtklib_user_clk_offset_drift = std::vector<double>(num_epoch);
auto pos_x = std::vector<double>(num_epoch);
auto pos_y = std::vector<double>(num_epoch);
auto pos_z = std::vector<double>(num_epoch);
auto vel_x = std::vector<double>(num_epoch);
auto vel_y = std::vector<double>(num_epoch);
auto vel_z = std::vector<double>(num_epoch);
auto acc_x = std::vector<double>(num_epoch);
auto acc_y = std::vector<double>(num_epoch);
auto acc_z = std::vector<double>(num_epoch);
auto cov_xx = std::vector<double>(num_epoch);
auto cov_yy = std::vector<double>(num_epoch);
auto cov_zz = std::vector<double>(num_epoch);
auto cov_vx = std::vector<double>(num_epoch);
auto cov_vy = std::vector<double>(num_epoch);
auto cov_vz = std::vector<double>(num_epoch);
auto cov_ax = std::vector<double>(num_epoch);
auto cov_ay = std::vector<double>(num_epoch);
auto cov_az = std::vector<double>(num_epoch);
auto latitude = std::vector<double>(num_epoch);
auto longitude = std::vector<double>(num_epoch);
auto height = std::vector<double>(num_epoch);
auto valid_sats = std::vector<uint8_t>(num_epoch);
auto gdop = std::vector<double>(num_epoch);
auto pdop = std::vector<double>(num_epoch);
auto hdop = std::vector<double>(num_epoch);
auto vdop = std::vector<double>(num_epoch);
try
{
if (dump_file.is_open())
{
for (int64_t i = 0; i < num_epoch; i++)
{
dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_ms[i]), sizeof(uint32_t));
dump_file.read(reinterpret_cast<char *>(&week[i]), sizeof(uint32_t));
dump_file.read(reinterpret_cast<char *>(&RX_time[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&user_clk_offset[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&user_clk_offset_drift[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&rtklib_user_clk_offset_drift[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&acc_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&acc_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&acc_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_xx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_yy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_zz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_vz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_ax[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_ay[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_az[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&latitude[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&longitude[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&height[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&valid_sats[i]), sizeof(uint8_t));
dump_file.read(reinterpret_cast<char *>(&gdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&hdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vdop[i]), sizeof(double));
}
}
dump_file.close();
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading VTL dump file:" << e.what() << '\n';
return false;
}
// WRITE MAT FILE
mat_t *matfp;
matvar_t *matvar;
std::string filename = dump_filename;
filename.erase(filename.length() - 4, 4);
filename.append(".mat");
matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73);
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
std::array<size_t, 2> dims{1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), week.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), RX_time.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("user_clk_offset_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset_drift.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("rtklib_user_clk_offset_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), rtklib_user_clk_offset_drift.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_y.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_z.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_x.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_y.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_x.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_y.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_z.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xx.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yy.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vx.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vy.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_vz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_ax", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_ay", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_ay.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_az", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), longitude.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), height.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), valid_sats.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), gdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), hdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
return true;
}
@ -465,7 +764,6 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const
return d_monitor_pvt;
}
void Rtklib_Solver::store_has_data(const Galileo_HAS_data &new_has_data)
{
// Compute time of application HAS SIS ICD, Issue 1.0, Section 7.7
@ -905,8 +1203,7 @@ void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, ui
}
}
bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s)
bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s, bool flag_averaging, bool enable_vtl, bool close_vtl_loop)
{
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@ -1502,8 +1799,13 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
d_nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &d_nav_data);
}
}
result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data);
std::vector<double> tropo_vec;
std::vector<double> iono_vec;
std::vector<double> pr_corrected_code_bias_vec;
std::vector<double> pr_residual_vec;
std::vector<double> doppler_residual_vec;
result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, tropo_vec,
iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec);
if (result == 0)
{
@ -1548,6 +1850,9 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
dops(index_aux, azel.data(), 0.0, d_dop.data());
}
this->set_valid_position(true);
// this->set_averaging_flag(true);
// this->set_averaging_depth(100);
// this->perform_pos_averaging();
std::array<double, 4> rx_position_and_time{};
if (d_conf.enable_pvt_kf == true)
@ -1599,6 +1904,97 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
}
this->set_rx_pos({rx_position_and_time[0], rx_position_and_time[1], rx_position_and_time[2]}); // save ECEF position for the next iteration
if (enable_vtl == true)
{
// VTL input data extraction from rtklib structures
/* satellite positions, velocities and clocks */
prcopt_t *opt = &d_rtk.opt;
/* count rover/base station observations */
int n_sats = valid_obs + glo_valid_obs;
int nu;
int nr;
for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++)
{
}
for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++)
{
}
double *rs;
double *dts;
double *var;
std::vector<int> svh(MAXOBS * 2);
rs = mat(6, n_sats);
dts = mat(2, n_sats);
var = mat(1, n_sats);
/* satellite positions, velocities and clocks */
satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data());
// Vtl_Data vtl_data;
vtl_data.init_storage(n_sats);
vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time;
vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands
vtl_data.sat_number = n_sats;
for (int n = 0; n < n_sats; n++)
{
vtl_data.sat_p(n, 0) = rs[0 + 6 * n];
vtl_data.sat_p(n, 1) = rs[1 + 6 * n];
vtl_data.sat_p(n, 2) = rs[2 + 6 * n];
vtl_data.sat_v(n, 0) = rs[3 + 6 * n];
vtl_data.sat_v(n, 1) = rs[4 + 6 * n];
vtl_data.sat_v(n, 2) = rs[5 + 6 * n];
vtl_data.sat_dts(n, 0) = dts[0 + 2 * n];
vtl_data.sat_dts(n, 1) = dts[1 + 2 * n];
vtl_data.sat_var(n) = var[n];
vtl_data.sat_health_flag(n) = svh.at(n);
vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0] * 0.25; //(0.25 dBHz)
// TODO: first version of VTL works only with ONE frequency band (band #0 is L1)
// To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp);
// corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts)
vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n] + SPEED_OF_LIGHT_M_S * dts[n * 2];
vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0] - SPEED_OF_LIGHT_M_S * dts[1 + 2 * n] / Lambda_GPS_L1;
vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0];
vtl_data.pr_res(n) = pr_residual_vec[n];
}
// VTL input data extraction from rtklib structures
/* Receiver position, velocity and clock */
/* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */
vtl_data.rx_p(0) = pvt_sol.rr[0];
vtl_data.rx_p(1) = pvt_sol.rr[1];
vtl_data.rx_p(2) = pvt_sol.rr[2];
vtl_data.rx_v(0) = pvt_sol.rr[3];
vtl_data.rx_v(1) = pvt_sol.rr[4];
vtl_data.rx_v(2) = pvt_sol.rr[5];
/* Receiver position, velocity and clock variances*/
vtl_data.rx_pvt_var[0] = pvt_sol.qr[0];
vtl_data.rx_pvt_var[1] = pvt_sol.qr[1];
vtl_data.rx_pvt_var[2] = pvt_sol.qr[2];
// TODO: get direct estimations for V T variances, instead:
vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]; // in general minor than position.
vtl_data.rx_pvt_var[4] = pvt_sol.qr[1];
vtl_data.rx_pvt_var[5] = pvt_sol.qr[2];
vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; // time
vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; // doppler
// receiver clock offset and receiver clock drift
vtl_data.rx_dts(0) = rx_position_and_time[3];
vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
vtl_engine.vtl_loop(vtl_data);
}
else
{
// MAGL: the code should not enter here once the vtl has started
// .. but it does!
// and not only that but pvt_sol.rr seems to have NOT reasonable values
// pvt_sol.rr[0]=rx_position_and_time[0]; // [m]
// pvt_sol.rr[1]=rx_position_and_time[1]; // [m]
// pvt_sol.rr[2]=rx_position_and_time[2]; // [m]
// pvt_sol.rr[3]=4.2e6;
// pvt_sol.rr[4]=4.2e6;
// pvt_sol.rr[5]=4.2e6;
}
// compute Ground speed and COG
double ground_speed_ms = 0.0;
std::array<double, 3> pos{};
@ -1803,6 +2199,109 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what();
}
// VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file
// if (enable_vtl == true)
try
{
double tmp_double;
uint32_t tmp_uint32;
// TOW
tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms;
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_uint32), sizeof(uint32_t));
// WEEK
tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009());
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_uint32), sizeof(uint32_t));
// PVT GPS time
tmp_double = gnss_observables_map.cbegin()->second.RX_time;
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// VTL filtered User clock offset [s]
tmp_double = vtl_engine.get_user_clock_offset_s();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// VTL filtered User clock offset drift[s/s]
tmp_double = vtl_engine.get_user_clock_offset_drift_s_s();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// rtklib User clock offset drift[s/s]
tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double)
std::vector<double> p_vec_m = vtl_engine.get_position_ecef_m();
tmp_double = p_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s();
tmp_double = v_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2();
tmp_double = a_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// position/velocity/acceleration variance/ (units^2) (9 x double)
std::vector<double> p_var_vec_m = vtl_engine.get_position_var_ecef_m();
tmp_double = p_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s();
tmp_double = v_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
vector<double> a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2();
tmp_double = a_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Latitude [rad]
vector<double> geo_vec_m = vtl_engine.get_geodetic_rad_m();
tmp_double = geo_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Longitude [rad]
tmp_double = geo_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Height [m]
tmp_double = geo_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// NUMBER OF VALID SATS
d_vtl_dump_file.write(reinterpret_cast<char *>(&pvt_sol.ns), sizeof(uint8_t));
// GDOP / PDOP / HDOP / VDOP
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[0]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[1]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[2]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[3]), sizeof(double));
}
catch (const std::ofstream::failure &e)
{
LOG(WARNING) << "Exception writing VTL dump file " << e.what();
}
}
}
}

11
src/algorithms/PVT/libs/rtklib_solver.h Normal file → Executable file
View File

@ -61,6 +61,8 @@
#include "pvt_solution.h"
#include "rtklib.h"
#include "rtklib_conversions.h"
#include "vtl_data.h"
#include "vtl_engine.h"
#include <array>
#include <cstdint>
#include <fstream>
@ -90,7 +92,9 @@ public:
~Rtklib_Solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double kf_update_interval_s);
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double kf_update_interval_s, bool flag_averaging, bool enable_vtl, bool close_vtl_loop);
Vtl_Data vtl_data;
double get_hdop() const override;
double get_vdop() const override;
@ -127,8 +131,11 @@ public:
Beidou_Dnav_Iono beidou_dnav_iono;
std::map<int, Beidou_Dnav_Almanac> beidou_dnav_almanac_map;
Vtl_Engine vtl_engine;
private:
bool save_matfile() const;
bool save_vtl_matfile() const;
void check_has_orbit_clock_validity(const std::map<int, Gnss_Synchro>& obs_map);
void get_has_biases(const std::map<int, Gnss_Synchro>& obs_map);
@ -148,7 +155,9 @@ private:
std::map<std::string, std::map<int, HAS_obs_corrections>> d_has_obs_corr_map; // first key is signal, second key is PRN
std::string d_dump_filename;
std::string d_vtl_dump_filename;
std::ofstream d_dump_file;
std::ofstream d_vtl_dump_file;
rtk_t d_rtk{};
nav_t d_nav_data{};
Monitor_Pvt d_monitor_pvt{};

22
src/algorithms/PVT/libs/vtl_conf.cc Normal file
View File

@ -0,0 +1,22 @@
/*!
* \file vtl_conf.c
* \brief Class that contains all the parameters to configure the Vector Tracking Loop (VTL) Kalman filter engine
* \author Javier Arribas, 2022. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "vtl_conf.h"
Vtl_Conf::Vtl_Conf()
{
}

40
src/algorithms/PVT/libs/vtl_conf.h Normal file
View File

@ -0,0 +1,40 @@
/*!
* \file vtl_conf.h
* \brief Class that contains all the parameters to configure the Vector Tracking Loop (VTL) Kalman filter engine
* \author Javier Arribas, 2022. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_VTL_CONF_H
#define GNSS_SDR_VTL_CONF_H
#include <cstdint>
#include <map>
#include <string>
/** \addtogroup PVT
* \{ */
/** \addtogroup PVT_libs
* \{ */
class Vtl_Conf
{
public:
Vtl_Conf();
// TODO: VTL control parameters and config options here
};
/** \} */
/** \} */
#endif // GNSS_SDR_VTL_CONF_H

68
src/algorithms/PVT/libs/vtl_data.cc Normal file
View File

@ -0,0 +1,68 @@
/*!
* \file vtl_data.c
* \brief Class that exchange information to and from the Vector Tracking Loop (VTL) Kalman filter engine
* \author Javier Arribas, 2022. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "vtl_data.h"
#include "armadillo"
#include "vector"
Vtl_Data::Vtl_Data()
{
epoch_tow_s = 0;
sample_counter = 0;
}
void Vtl_Data::init_storage(int n_sats)
{
sat_p = arma::mat(n_sats, 3);
sat_v = arma::mat(n_sats, 3);
sat_dts = arma::mat(n_sats, 2);
sat_var = arma::vec(n_sats);
sat_health_flag = arma::vec(n_sats);
sat_CN0_dB_hz = arma::colvec(n_sats);
sat_LOS = arma::mat(n_sats, 3);
int sat_number = n_sats;
pr_m = arma::vec(n_sats);
doppler_hz = arma::vec(n_sats);
carrier_phase_rads = arma::vec(n_sats);
pr_res = arma::vec(n_sats);
rx_p = arma::mat(1, 3);
rx_v = arma::mat(1, 3);
rx_dts = arma::mat(1, 2);
rx_var = arma::vec(1);
rx_pvt_var = arma::vec(8);
epoch_tow_s = 0;
sample_counter = 0;
}
void Vtl_Data::debug_print()
{
std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n";
sat_p.print("VTL Sat Positions");
sat_v.print("VTL Sat Velocities");
sat_dts.print("VTL Sat clocks");
sat_var.print("VTL Sat clock variances");
sat_health_flag.print("VTL Sat health");
sat_LOS.print("VTL SAT LOS");
kf_state.print("EKF STATE");
pr_m.print("Satellite Code pseudoranges [m]");
doppler_hz.print("satellite Carrier Dopplers [Hz]");
carrier_phase_rads.print("satellite accumulated carrier phases [rad]");
}

70
src/algorithms/PVT/libs/vtl_data.h Normal file
View File

@ -0,0 +1,70 @@
/*!
* \file vtl_data.h
* \brief Class that exchange information to and from the Vector Tracking Loop (VTL) Kalman filter engine
* \author Javier Arribas, 2022. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_VTL_DATA_H
#define GNSS_SDR_VTL_DATA_H
// constants definition
#define Lambda_GPS_L1 0.1902937
#include <armadillo>
#include <cstdint>
#include <string>
#include <vector>
/** \addtogroup PVT
* \{ */
/** \addtogroup PVT_libs
* \{ */
class Vtl_Data
{
public:
Vtl_Data();
void init_storage(int n_sats);
arma::mat sat_p; // Satellite ECEF Position [m]
arma::mat sat_v; // Satellite Velocity [m/s]
arma::mat sat_dts; // Satellite clock bias and drift [s,s/s]
arma::colvec sat_var; // sat position and clock error variance [m^2]
arma::colvec sat_health_flag; // sat health flag (0 is ok)
arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz
arma::mat sat_LOS; // sat LOS
int sat_number; // on-view sat number
arma::colvec pr_m; // Satellite Code pseudoranges [m]
arma::colvec doppler_hz; // satellite Carrier Dopplers [Hz]
arma::colvec carrier_phase_rads; // satellite accumulated carrier phases [rads]
arma::colvec pr_res; // pseudorange residual
arma::mat rx_p; // Receiver ENU Position [m]
arma::mat rx_v; // Receiver Velocity [m/s]
arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s]
arma::mat rx_dts; // Receiver clock bias and drift [s,s/s]
arma::colvec rx_var; // Receiver position and clock error variance [m^2]
arma::mat kf_state; // KF STATE
arma::mat kf_P; // KF STATE covariance
// time handling
double PV[6]; // position and Velocity
double epoch_tow_s; // current observation RX time [s]
uint64_t sample_counter; // current sample counter associated with RX time [samples from start]
void debug_print();
};
/** \} */
/** \} */
#endif // GNSS_SDR_VTL_DATA_H

473
src/algorithms/PVT/libs/vtl_engine.cc Normal file
View File

@ -0,0 +1,473 @@
/*!
* \file vtl_engine.h
* \brief Class that implements a Vector Tracking Loop (VTL) Kalman filter engine
* \author Javier Arribas, 2022. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "vtl_engine.h"
#include "rtklib_rtkcmn.h"
#include "iostream"
#include <fstream>
using namespace std;
Vtl_Engine::Vtl_Engine()
{
counter = 0;
refSampleCounter = 0;
n_of_states = 11;
delta_t_cmd = 0;
kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; // TODO: use a real value.;
kf_x = arma::zeros(n_of_states, 1);
}
Vtl_Engine::~Vtl_Engine()
{
}
bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
{
// TODO: Implement main VTL loop here
using arma::as_scalar;
if (refSampleCounter == 0)
{
refSampleCounter = new_data.sample_counter;
}
double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0;
refSampleCounter = new_data.sample_counter;
bool flag_cmd = false;
bool flag_time_cmd = false;
delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command
if (delta_t_cmd >= 0.3)
{
flag_cmd = true;
delta_t_cmd = 0; // reset timer for vtl trk command
}
// ################## Kalman filter initialization ######################################
// State variables
arma::mat kf_dx = arma::zeros(n_of_states, 1);
// covariances (static)
kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number);
double kf_dt = delta_t_vtl; // 0.05;
kf_Q = arma::eye(n_of_states, n_of_states);
kf_F = arma::eye(n_of_states, n_of_states);
kf_y = arma::zeros(3 * new_data.sat_number, 1);
kf_yerr = arma::zeros(3 * new_data.sat_number, 1);
kf_xerr = arma::zeros(n_of_states, 1);
kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix
kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number);
d = arma::zeros(new_data.sat_number, 1);
rho_pri = arma::zeros(new_data.sat_number, 1);
rhoDot_pri = arma::zeros(new_data.sat_number, 1);
rhoDot2_pri = arma::zeros(new_data.sat_number, 1);
rho_pri_filt = arma::zeros(new_data.sat_number, 1);
rhoDot_pri_filt = arma::zeros(new_data.sat_number, 1);
doppler_hz_filt = arma::zeros(new_data.sat_number, 1);
a_x = arma::zeros(new_data.sat_number, 1);
a_y = arma::zeros(new_data.sat_number, 1);
a_z = arma::zeros(new_data.sat_number, 1);
// ################## Kalman Tracking ######################################
counter++; // uint64_t
if (counter > 2500)
{
flag_time_cmd = true;
}
uint32_t closure_point = 3;
// State error Covariance Matrix Q (PVT)
kf_Q(0, 0) = 100.0;
kf_Q(1, 1) = 100.0;
kf_Q(2, 2) = 100.0;
kf_Q(3, 3) = 8.0;
kf_Q(4, 4) = 8.0;
kf_Q(5, 5) = 8.0;
kf_Q(6, 6) = .10;
kf_Q(7, 7) = .10;
kf_Q(8, 8) = .10;
kf_Q(9, 9) = 4.0;
kf_Q(10, 10) = 10.0;
// Measurement error Covariance Matrix R assembling
for (int32_t i = 0; i < new_data.sat_number; i++)
{
kf_R(i, i) = 80.0;
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;
kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 40.0;
if (i == 6)
{
kf_R(i, i) = 10e5;
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e5;
kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 10e6;
}
}
//**************************************
// Kalman state prediction (time update)
//**************************************
if (counter < closure_point)
{
// receiver solution from rtklib_solver
kf_dx = kf_x;
kf_x(0) = new_data.rx_p(0);
kf_x(1) = new_data.rx_p(1);
kf_x(2) = new_data.rx_p(2);
kf_x(3) = new_data.rx_v(0);
kf_x(4) = new_data.rx_v(1);
kf_x(5) = new_data.rx_v(2);
kf_x(6) = 0;
kf_x(7) = 0;
kf_x(8) = 0;
kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S;
kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S;
kf_dx = kf_x - kf_dx;
kf_dx = kf_F * kf_dx; // state prediction/* */
}
else
{
// receiver solution from previous KF step
kf_x = kf_x;
// acceleration model
double acc_x = 0;
double acc_y = 0;
double acc_z = 0;
kf_x(6) = acc_x;
kf_x(7) = acc_y;
kf_x(8) = acc_z;
// kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt;
// kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt;
// kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt;
}
kf_F_fill(kf_F, kf_dt, kf_x);
obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x);
kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction
//**************************************
// Kalman estimation (measurement update)
//**************************************
kf_H = arma::zeros(3 * new_data.sat_number, n_of_states);
kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot2_pri, new_data.pr_m, new_data.doppler_hz, kf_x);
//**************************************
// Kalman filter update step
//**************************************
kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S)
arma::mat B = (kf_P_x * kf_H.t());
kf_K = B * arma::inv(kf_S); // Kalman gain
kf_xerr = kf_K * (kf_yerr); // Error state estimation
arma::mat A = (arma::eye(size(kf_P_x)) - kf_K * kf_H);
kf_P_x = A * kf_P_x * A.t() + kf_K * kf_R * kf_K.t(); // update state estimation error covariance matrix
kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error)
kf_dx = kf_x;
//*************************
// Geometric Transformation
//*************************
obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x);
kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
// Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
kf_yerr = kf_H * kf_xerr;
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
TrackingCmd trk_cmd;
for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector
{
rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered
rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1) + kf_yerr(channel + new_data.sat_number); // now filtered
doppler_hz_filt(channel) = (rhoDot_pri_filt(channel)) / Lambda_GPS_L1;
trk_cmd.carrier_phase_rads = 0; // difficult of calculation
trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); // this is el doppler WITHOUTH sintony correction
trk_cmd.carrier_freq_rate_hz_s = -(a_x(channel) * kf_x(6) + a_y(channel) * kf_x(7) + a_z(channel) * kf_x(8)) / Lambda_GPS_L1;
trk_cmd.code_phase_chips = 0; // kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
if (flag_time_cmd)
{
if (flag_cmd)
{
trk_cmd.enable_carrier_nco_cmd = true;
}
else
{
trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue
}
}
else
{
trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue
}
trk_cmd.sample_counter = new_data.sample_counter;
trk_cmd.channel_id = channel;
trk_cmd_outs.push_back(trk_cmd);
}
fstream dump_vtl_file;
dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app);
dump_vtl_file.precision(15);
if (!dump_vtl_file)
{
cout << "File not created!";
}
else
{
dump_vtl_file << "kf_state"
<< "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7) << "," << kf_x(8) << "," << kf_x(9) << "," << kf_x(10) << endl;
dump_vtl_file << "kf_xerr"
<< "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7) << "," << kf_xerr(8) << "," << kf_xerr(9) << "," << kf_xerr(10) << endl;
dump_vtl_file << "rtklib_state"
<< "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << "," << delta_t_vtl << endl;
// dump_vtl_file << "filt_dopp_sat"
// << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) <<endl;
dump_vtl_file.close();
}
return true;
}
void Vtl_Engine::reset()
{
// TODO
}
void Vtl_Engine::debug_print()
{
// TODO
}
void Vtl_Engine::configure(Vtl_Conf config_)
{
config = config_;
// TODO: initialize internal variables
}
void Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt)
{
for (int32_t i = 0; i < sat_number; i++) // Measurement matrix H assembling
{
// It has n_of_states columns (n_of_states states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(i, 0) = ax(i);
kf_H(i, 1) = ay(i);
kf_H(i, 2) = az(i);
kf_H(i, 9) = 1.0;
kf_H(i, 10) = kf_dt;
kf_H(i + sat_number, 3) = ax(i);
kf_H(i + sat_number, 4) = ay(i);
kf_H(i + sat_number, 5) = az(i);
kf_H(i + sat_number, 6) = ax(i) * kf_dt;
kf_H(i + sat_number, 7) = ay(i) * kf_dt;
kf_H(i + sat_number, 8) = az(i) * kf_dt;
kf_H(i + sat_number, 10) = 1.0;
kf_H(i + 2 * sat_number, 3) = 0; // ax(i);
kf_H(i + 2 * sat_number, 4) = 0; // ay(i);
kf_H(i + 2 * sat_number, 5) = 0; // az(i);
kf_H(i + 2 * sat_number, 6) = ax(i);
kf_H(i + 2 * sat_number, 7) = ay(i);
kf_H(i + 2 * sat_number, 8) = az(i);
kf_H(i + 2 * sat_number, 10) = kf_dt;
}
}
void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x)
{
// modulo de la velocidad
double vx = kf_x(3);
double vy = kf_x(4);
double vz = kf_x(5);
double u = norm(kf_x.rows(3, 5), 2);
kf_F(0, 3) = kf_dt;
kf_F(0, 6) = kf_dt * kf_dt / 2;
kf_F(1, 4) = kf_dt;
kf_F(1, 7) = kf_dt * kf_dt / 2;
kf_F(2, 5) = kf_dt;
kf_F(2, 8) = kf_dt * kf_dt / 2;
kf_F(3, 6) = kf_dt;
kf_F(4, 7) = kf_dt;
kf_F(5, 8) = kf_dt;
kf_F(6, 3) = (vx * vx / u + u);
kf_F(7, 4) = (vy * vy / u + u);
kf_F(8, 5) = (vz * vz / u + u);
kf_F(9, 10) = kf_dt;
}
void Vtl_Engine::kf_F_fill_rocket(arma::mat &kf_F, double kf_dt, arma::mat &kf_x)
{
double densidad = 1.0;
double ballistic_coef = 0.007;
double diam_cohete = 120.0e-3; // 120 mm
double beta = (GNSS_PI * densidad * diam_cohete * diam_cohete / 8) * ballistic_coef;
// modulo de la velocidad
double vx = kf_x(3);
double vy = kf_x(4);
double vz = kf_x(5);
double u = norm(kf_x.rows(3, 5), 2);
kf_F(0, 3) = kf_dt;
kf_F(0, 6) = kf_dt * kf_dt / 2;
kf_F(1, 4) = kf_dt;
kf_F(1, 7) = kf_dt * kf_dt / 2;
kf_F(2, 5) = kf_dt;
kf_F(2, 8) = kf_dt * kf_dt / 2;
kf_F(3, 6) = kf_dt;
kf_F(4, 7) = kf_dt;
kf_F(5, 8) = kf_dt;
kf_F(6, 3) = -beta * (vx * vx / u + u);
kf_F(7, 4) = -beta * (vy * vy / u + u);
kf_F(8, 5) = -beta * (vz * vz / u + u);
kf_F(9, 10) = kf_dt;
}
void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x)
{
for (int32_t i = 0; i < sat_number; i++) // neccesary quantities
{
// d(i) is the distance sat(i) to receiver
d(i) = (sat_p(i, 0) - kf_x(0)) * (sat_p(i, 0) - kf_x(0));
d(i) = d(i) + (sat_p(i, 1) - kf_x(1)) * (sat_p(i, 1) - kf_x(1));
d(i) = d(i) + (sat_p(i, 2) - kf_x(2)) * (sat_p(i, 2) - kf_x(2));
d(i) = sqrt(d(i));
// compute pseudorange estimation OUTPUT
rho_pri(i) = d(i) + kf_x(9);
// compute LOS sat-receiver vector componentsx
ax(i) = -(sat_p(i, 0) - kf_x(0)) / d(i);
ay(i) = -(sat_p(i, 1) - kf_x(1)) / d(i);
az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i);
// compute pseudorange rate estimation OUTPUT
rhoDot_pri(i) = (sat_v(i, 0) - kf_x(3)) * a_x(i) + (sat_v(i, 1) - kf_x(4)) * a_y(i) + (sat_v(i, 2) - kf_x(5)) * a_z(i);
rhoDot2_pri(i) = (kf_x(6)) * a_x(i) + (kf_x(7)) * a_y(i) + (kf_x(8)) * a_z(i);
}
}
void Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x)
{
for (int32_t i = 0; i < sat_number; i++) // Measurement vector OUTPUT
{
kf_yerr(i) = rho_pri(i) - pr_m(i);
kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1 + kf_x(10)) - rhoDot_pri(i);
kf_yerr(i + 2 * sat_number) = -rhoDot2_pri(i);
}
}
std::vector<double> Vtl_Engine::get_position_ecef_m()
{
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[0];
temp[1] = kf_x[1];
temp[2] = kf_x[2];
return temp;
}
std::vector<double> Vtl_Engine::get_velocity_ecef_m_s()
{
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[3];
temp[1] = kf_x[4];
temp[2] = kf_x[5];
return temp;
}
std::vector<double> Vtl_Engine::get_accel_ecef_m_s2()
{
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_x[6];
temp[1] = kf_x[7];
temp[2] = kf_x[8];
return temp;
}
std::vector<double> Vtl_Engine::get_position_var_ecef_m()
{
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(0, 0);
temp[1] = kf_P_x(1, 1);
temp[2] = kf_P_x(2, 2);
return temp;
}
std::vector<double> Vtl_Engine::get_velocity_var_ecef_m_s()
{
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(3, 3);
temp[1] = kf_P_x(4, 4);
temp[2] = kf_P_x(5, 5);
return temp;
}
std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2()
{
std::vector<double> temp = {42, 42, 42};
temp[0] = kf_P_x(6, 6);
temp[1] = kf_P_x(7, 7);
temp[2] = kf_P_x(8, 8);
return temp;
}
std::vector<double> Vtl_Engine::get_geodetic_rad_m()
{
std::array<double, 3> temp_ecef = {kf_x[0], kf_x[1], kf_x[2]};
std::array<double, 3> temp_geo = {42, 42, 42};
ecef2pos(temp_ecef.data(), temp_geo.data());
std::vector<double> dest;
dest.insert(dest.begin(), std::begin(temp_geo), std::end(temp_geo));
return dest;
}
double Vtl_Engine::get_user_clock_offset_s()
{
double temp = 0;
temp = kf_x[9];
return temp;
}
double Vtl_Engine::get_user_clock_offset_drift_s_s()
{
double temp = 0;
temp = kf_x[10];
return temp;
}

113
src/algorithms/PVT/libs/vtl_engine.h Normal file
View File

@ -0,0 +1,113 @@
/*!
* \file vtl_engine.h
* \brief Class that implements a Vector Tracking Loop (VTL) Kalman filter engine
* \author Javier Arribas, 2022. jarribas(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_VTL_ENGINE_H
#define GNSS_SDR_VTL_ENGINE_H
#include "MATH_CONSTANTS.h"
#include "trackingcmd.h"
#include "vtl_conf.h"
#include "vtl_data.h"
#include <armadillo>
#include <cstdint>
#include <string>
#include <vector>
/** \addtogroup PVT
* \{ */
/** \addtogroup PVT_libs
* \{ */
class Vtl_Engine
{
public:
Vtl_Engine();
~Vtl_Engine();
void configure(Vtl_Conf config_); // set config parameters
// TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...)
bool vtl_loop(Vtl_Data new_data);
void reset(); // reset all internal states
void debug_print(); // print debug information
std::vector<TrackingCmd> trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs
std::vector<double> get_position_ecef_m(); // get_position_ecef_m
std::vector<double> get_velocity_ecef_m_s(); // get_velocity_ecef_m_s
std::vector<double> get_accel_ecef_m_s2(); // get_accel_ecef_m_s2
std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m
std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s
std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2
std::vector<double> get_geodetic_rad_m(); // get_geodetic_rad_m
double get_user_clock_offset_s(); // get_user_clock_offset_s;
double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s;
private:
Vtl_Conf config;
// TODO: Internal VTL persistent variables here
// Transformation variables
arma::colvec d;
arma::colvec rho_pri;
arma::colvec rhoDot_pri;
arma::colvec rhoDot2_pri;
arma::colvec rho_pri_filt;
arma::colvec rhoDot_pri_filt;
arma::colvec doppler_hz_filt;
arma::colvec a_x;
arma::colvec a_y;
arma::colvec a_z;
// Kalman filter matrices
arma::mat kf_P_x_ini; // initial state error covariance matrix
// arma::mat kf_P_x; // state error covariance matrix
arma::mat kf_P_x_pre; // Predicted state error covariance matrix
arma::mat kf_P_x;
arma::mat kf_S; // innovation covariance matrix
arma::mat kf_F; // state transition matrix
arma::mat kf_H; // system matrix
arma::mat kf_R; // measurement error covariance matrix
arma::mat kf_Q; // system error covariance matrix
arma::mat kf_x; // state vector
arma::mat kf_x_pre; // predicted state vector
arma::mat kf_y; // measurement vector
arma::mat kf_yerr; // ERROR measurement vector
arma::mat kf_xerr; // ERROR state vector
arma::mat kf_K; // Kalman gain matrix
// Gaussian estimator
arma::mat kf_R_est; // measurement error covariance
uint32_t counter;
int n_of_states;
uint64_t refSampleCounter;
double delta_t_cmd = 0;
void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); /* */ // Observation Matrix constructor
void kf_F_fill_rocket(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor
void kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor
void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation
void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
};
/** \} */
/** \} */
#endif // GNSS_SDR_VTL_ENGINE_H

1
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@ -261,6 +261,7 @@ void pcps_acquisition::init()
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->last_vtl_cmd_sample_counter = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;

50
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -40,7 +40,7 @@
#include <armadillo>
#include <cmath>
#include <cstring>
#include <vector>
/* pseudorange measurement error variance ------------------------------------*/
double varerr(const prcopt_t *opt, double el, int sys)
@ -409,7 +409,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
double *v, double *H, double *var, double *azel, int *vsat,
double *resp, int *ns)
double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias)
{
double r;
double dion;
@ -506,6 +506,11 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
/* pseudorange residual */
v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp);
/* MOD ARRIBAS */
pr_corrected_code_bias[i] = P;
tropo_m[i] = dtrp;
iono_m[i] = dion;
/* design matrix */
for (j = 0; j < NX; j++)
{
@ -684,7 +689,12 @@ arma::vec rough_bancroft(const arma::mat &B_pass)
int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
double *resp, char *msg,
std::vector<double> &tropo_vec,
std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec)
{
double x[NX] = {0};
double dx[NX];
@ -702,6 +712,12 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
int ns;
char msg_aux[128];
double *tropo_m, *iono_m, *pr_corrected_code_bias;
tropo_m = mat(n + 4, 1);
iono_m = mat(n + 4, 1);
resp = mat(n + 4, 1);
pr_corrected_code_bias = mat(n + 4, 1);
trace(3, "estpos : n=%d\n", n);
v = mat(n + 4, 1);
@ -744,7 +760,8 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
for (i = 0; i < MAXITR; i++)
{
/* pseudorange residuals */
nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, &ns);
nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp,
&ns, tropo_m, iono_m, pr_corrected_code_bias);
if (nv < NX)
{
@ -799,6 +816,14 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
{
sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
}
for (k = 0; k < n; k++)
{
tropo_vec.push_back(tropo_m[k]);
iono_vec.push_back(iono_m[k]);
pr_corrected_code_bias_vec.push_back(pr_corrected_code_bias[k]);
pr_residual_vec.push_back(resp[k]);
}
free(v);
free(H);
free(var);
@ -875,9 +900,14 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
vare_e[k] = vare[j];
svh_e[k++] = svh[j];
}
std::vector<double> tropo_vec;
std::vector<double> iono_vec;
std::vector<double> pr_corrected_code_bias_vec;
std::vector<double> pr_residual_vec;
std::vector<double> doppler_residual_vec;
/* estimate receiver position without a satellite */
if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e,
vsat_e, resp_e, msg_e))
vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec))
{
trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg);
continue;
@ -1087,7 +1117,11 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
*-----------------------------------------------------------------------------*/
int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)
char *msg, std::vector<double> &tropo_vec,
std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec)
{
prcopt_t opt_ = *opt;
double *rs;
@ -1131,8 +1165,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh.data());
/* estimate receiver position with pseudorange */
stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg);
stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg,
iono_vec, tropo_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec);
/* raim fde */
if (!stat && n >= 6 && opt->posopt[4])
{

12
src/algorithms/libs/rtklib/rtklib_pntpos.h
View File

@ -35,6 +35,7 @@
#include "rtklib.h"
#include "rtklib_rtkcmn.h"
#include <vector>
/* constants -----------------------------------------------------------------*/
const int NX = 4 + 3; //!< # of estimated parameters
@ -92,7 +93,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
double *v, double *H, double *var, double *azel, int *vsat,
double *resp, int *ns);
double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias);
/* validate solution ---------------------------------------------------------*/
int valsol(const double *azel, const int *vsat, int n,
@ -103,7 +104,8 @@ int valsol(const double *azel, const int *vsat, int n,
int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg);
double *resp, char *msg, std::vector<double> &tropo_vec,
std::vector<double> &iono_vec, std::vector<double> &pr_corrected_code_bias_vec);
/* raim fde (failure detection and exclution) -------------------------------*/
int raim_fde(const obsd_t *obs, int n, const double *rs,
@ -140,6 +142,10 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
*/
int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg);
char *msg, std::vector<double> &tropo_vec,
std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual,
std::vector<double> &doppler_residual);
#endif // GNSS_SDR_RTKLIB_PNTPOS_H

14
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

@ -38,7 +38,6 @@
#include <cmath>
#include <cstring>
#include <string>
#include <vector>
static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)),
@ -2762,7 +2761,12 @@ void rtkfree(rtk_t *rtk)
* notes : before calling function, base station position rtk->sol.rb[] should
* be properly set for relative mode except for moving-baseline
*-----------------------------------------------------------------------------*/
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
std::vector<double> &tropo_vec,
std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec)
{
prcopt_t *opt = &rtk->opt;
sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
@ -2797,7 +2801,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
time = rtk->sol.time; /* previous epoch */
/* rover position by single point positioning */
if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg))
if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec,
iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec))
{
errmsg(rtk, "point pos error (%s)\n", msg);
if (!rtk->opt.dynamics)
@ -2839,7 +2844,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
if (opt->mode == PMODE_MOVEB)
{ /* moving baseline */
/* estimate position/velocity of base station */
if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg))
if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec,
iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec))
{
errmsg(rtk, "base station position error (%s)\n", msg);
return 0;

8
src/algorithms/libs/rtklib/rtklib_rtkpos.h
View File

@ -34,6 +34,7 @@
#include "rtklib.h"
#include "rtklib_rtkcmn.h"
#include <vector>
/** \addtogroup PVT
* \{ */
@ -174,7 +175,12 @@ void rtkinit(rtk_t *rtk, const prcopt_t *opt);
void rtkfree(rtk_t *rtk);
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
std::vector<double> &tropo_vec,
std::vector<double> &iono_vec,
std::vector<double> &pr_corrected_code_bias_vec,
std::vector<double> &pr_residual_vec,
std::vector<double> &doppler_residual_vec);
/** \} */

9
src/algorithms/libs/rtklib/rtklib_rtksvr.cc
View File

@ -38,6 +38,7 @@
#include "rtklib_solution.h"
#include "rtklib_stream.h"
#include <cstring>
#include <vector>
/* write solution header to output stream ------------------------------------*/
void writesolhead(stream_t *stream, const solopt_t *solopt)
@ -595,7 +596,13 @@ void *rtksvrthread(void *arg)
}
/* rtk positioning */
rtksvrlock(svr);
rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav);
std::vector<double> tropo_vec;
std::vector<double> iono_vec;
std::vector<double> pr_corrected_code_bias_vec;
std::vector<double> pr_residual;
std::vector<double> doppler_residual;
rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec,
iono_vec, pr_corrected_code_bias_vec, pr_residual, doppler_residual);
rtksvrunlock(svr);
if (svr->rtk.sol.stat != SOLQ_NONE)

6
src/algorithms/libs/trackingcmd.h
View File

@ -28,14 +28,14 @@
class TrackingCmd
{
public:
TrackingCmd();
bool enable_carrier_nco_cmd = false;
bool enable_code_nco_cmd = false;
double code_freq_chips = 0.0;
double code_phase_chips = 0.0;
double carrier_phase_rads = 0.0;
double carrier_freq_hz = 0.0;
double carrier_freq_rate_hz_s = 0.0;
uint64_t sample_counter = 0UL;
uint32_t channel_id = 0;
};
/** \} */

64
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@ -48,10 +48,12 @@
#include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp
#include <volk_gnsssdr/volk_gnsssdr.h>
#include "iostream"
#include <algorithm> // for fill_n
#include <array>
#include <cmath> // for fmod, round, floor
#include <exception> // for exception
#include <fstream>
#include <iostream> // for cout, cerr
#include <map>
#include <numeric>
@ -134,7 +136,9 @@ kf_tracking::kf_tracking(const Kf_Conf &conf_)
d_cloop(true),
d_dump(d_trk_parameters.dump),
d_dump_mat(d_trk_parameters.dump_mat && d_dump),
d_acc_carrier_phase_initialized(false)
d_acc_carrier_phase_initialized(false),
d_vtl_cmd_applied_now(false),
d_vtl_cmd_samplestamp(0LL)
{
#if GNURADIO_GREATER_THAN_38
this->set_relative_rate(1, static_cast<uint64_t>(d_trk_parameters.vector_length));
@ -635,8 +639,55 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr<TrackingCmd>).hash_code())
{
const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg));
// std::cout << "RX pvt-to-trk cmd with delay: "
// << static_cast<double>(nitems_read(0) - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n";
// std::cout<< "test cast CH "<<cmd->sample_counter <<"\n";
if (cmd->channel_id == this->d_channel)
{
arma::vec x_tmp;
arma::mat F_tmp;
gr::thread::scoped_lock lock(d_setlock);
// To.Do: apply VTL corrections to the KF states
double delta_t_s = static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in;
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s
x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s};
// ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces
F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0},
{0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)},
{0.0, 0.0, 1.0, delta_t_s},
{0.0, 0.0, 0.0, 1.0}};
// TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz)
arma::vec tmp_x = F_tmp * x_tmp;
double old_doppler = d_x_old_old(2);
double old_doppler_rate = d_x_old_old(3);
double old_code_phase_chips = d_x_old_old(0);
if (cmd->enable_carrier_nco_cmd)
{
d_x_old_old(2) = tmp_x(2); // replace DOPPLER
}
else
{
// std::cout << "correction not applied" << std::endl;
}
// set vtl corrections flag to inform VTL from gnss_synchro object
d_vtl_cmd_applied_now = true;
d_vtl_cmd_samplestamp = cmd->sample_counter;
std::fstream dump_tracking_file;
dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app);
dump_tracking_file.precision(15);
if (!dump_tracking_file)
{
std::cout << "dump_tracking_file not created!";
}
else
{
dump_tracking_file << "doppler_corr"
<< "," << this->d_channel << "," << tmp_x(2) << "," << old_doppler << "," << tmp_x(3) << "," << old_doppler_rate << "\n";
dump_tracking_file.close();
}
}
}
else
{
@ -1162,6 +1213,7 @@ void kf_tracking::run_Kf()
// Kalman loop
// Prediction
// d_x_old_old(0)=0; // reset error estimation because the NCO corrects the code phase
d_x_new_old = d_F * d_x_old_old;
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
@ -1177,7 +1229,6 @@ void kf_tracking::run_Kf()
// new code phase estimation
d_code_error_kf_chips = d_x_new_new(0);
d_x_new_new(0) = 0; // reset error estimation because the NCO corrects the code phase
// new carrier phase estimation
d_carrier_phase_kf_rad = d_x_new_new(1);
@ -2080,6 +2131,11 @@ int kf_tracking::general_work(int noutput_items __attribute__((unused)), gr_vect
{
current_synchro_data.fs = static_cast<int64_t>(d_trk_parameters.fs_in);
current_synchro_data.Tracking_sample_counter = d_sample_counter;
if (d_vtl_cmd_applied_now == true)
{
d_vtl_cmd_applied_now = false;
}
current_synchro_data.last_vtl_cmd_sample_counter = d_vtl_cmd_samplestamp;
*out[0] = std::move(current_synchro_data);
return 1;
}

3
src/algorithms/tracking/gnuradio_blocks/kf_tracking.h
View File

@ -230,6 +230,9 @@ private:
bool d_dump;
bool d_dump_mat;
bool d_acc_carrier_phase_initialized;
// VTL cmd control
bool d_vtl_cmd_applied_now;
uint64_t d_vtl_cmd_samplestamp;
bool d_enable_extended_integration;
};

6
src/core/system_parameters/gnss_synchro.h
View File

@ -80,6 +80,9 @@ public:
bool Flag_valid_pseudorange{}; //!< Set by Observables processing block
bool Flag_PLL_180_deg_phase_locked{}; //!< Set by Telemetry Decoder processing block
// VTL
uint64_t last_vtl_cmd_sample_counter{}; //!< Set by Tracking processing block
/// Copy constructor
Gnss_Synchro(const Gnss_Synchro& other) noexcept = default;
@ -117,6 +120,7 @@ public:
this->Flag_valid_word = rhs.Flag_valid_word;
this->Flag_valid_pseudorange = rhs.Flag_valid_pseudorange;
this->Flag_PLL_180_deg_phase_locked = rhs.Flag_PLL_180_deg_phase_locked;
this->last_vtl_cmd_sample_counter = rhs.last_vtl_cmd_sample_counter;
}
return *this;
};
@ -157,7 +161,7 @@ public:
this->Flag_valid_word = other.Flag_valid_word;
this->Flag_valid_pseudorange = other.Flag_valid_pseudorange;
this->Flag_PLL_180_deg_phase_locked = other.Flag_PLL_180_deg_phase_locked;
this->last_vtl_cmd_sample_counter = other.last_vtl_cmd_sample_counter;
// Leave the source object in a valid but unspecified state
other.Signal[0] = '\0';
other.Signal[1] = '\0';

75
src/utils/matlab/vtl/CN0_sat_plotting.m Normal file
View File

@ -0,0 +1,75 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
time_reference_spirent_obs=129780;%s
time_vtl_dump_file=linspace(38,157,length(vtlSolution.CN0_sat));
%%
%--------------------------------------------------------
Rx_Dopp_30=figure('Name','RX_Carrier_Doppler_hz');
subplot(2,3,1);
xlim([0,160]);
xlabel('')
ylabel('CN0 sat (dB-Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
title('PRN 30 GNSS-SDR')
plot(time_vtl_dump_file,vtlSolution.CN0_sat(1,:),'o','DisplayName','filtered VTL')
hold off;grid minor
%--------------------------------------------------------
subplot(2,3,2);
xlim([0,160]);
xlabel('')
ylabel('CN0 sat (dB-Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
title('PRN 29 GNSS-SDR')
plot(time_vtl_dump_file,vtlSolution.CN0_sat(2,:),'o','DisplayName','filtered VTL')
hold off;grid minor
%--------------------------------------------------------
subplot(2,3,3);
xlim([0,160]);
xlabel('')
ylabel('CN0 sat (dB-Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
% legend('PRN 17 GNSS-SDR','Location','eastoutside')
title('PRN 24 GNSS-SDR')
plot(time_vtl_dump_file,vtlSolution.CN0_sat(3,:),'o','DisplayName','filtered VTL')
hold off;grid minor
%--------------------------------------------------------
subplot(2,3,4);
xlim([0,160]);
xlabel('')
ylabel('CN0 sat (dB-Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
title('PRN 12 GNSS-SDR')
plot(time_vtl_dump_file,vtlSolution.CN0_sat(4,:),'o','DisplayName','filtered VTL')
hold off;grid minor
%--------------------------------------------------------
subplot(2,3,5);
xlim([0,160]);
xlabel('')
ylabel('CN0 sat (dB-Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
title('PRN 10 GNSS-SDR')
plot(time_vtl_dump_file,vtlSolution.CN0_sat(5,:),'o','DisplayName','filtered VTL')
hold off;grid minor

98
src/utils/matlab/vtl/GnssSDR2struct.m Normal file
View File

@ -0,0 +1,98 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
% GnssSDR2struct Convert GNSS-SDR output .mat file PVT.mat to a struct.
% refSolution = SpirentMotion2struct(path_to_motion_V1_csv) Convert PVT.mat to a struct navSolution
% refSolution has the following fields:
% navSolution.solution_status=solution_status;
% navSolution.solution_type=solution_type;
% navSolution.valid_sats=valid_sats;
%
% navSolution.RX_time=RX_time;
% navSolution.TOW_at_current_symbol_ms=TOW_at_current_symbol_ms;
%
% navSolution.X=pos_x;
% navSolution.Y=pos_y;
% navSolution.Z=pos_z;
%
% navSolution.latitude=latitude;
% navSolution.longitude=longitude;
% navSolution.height=height;
%
% navSolution.pdop=pdop;
% navSolution.gdop=gdop;
% navSolution.hdop=hdop;
%
% navSolution.vX=vel_x;
% navSolution.vY=vel_y;
% navSolution.vZ=vel_z;
%
% navSolution.vdop=vdop;
%
% navSolution.week=week;
%
% -------------------------------------------------------------------------
% USE EXAMPLE: navSolution = GnssSDR2struct('PVT_raw.mat')
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
% SPDX-FileCopyrightText: 2022 gomezlma(at)inta.es
% SPDX-License-Identifier: GPL-3.0-or-later
function [navSolution] = GnssSDR2struct(PVT_file_Path)
load(PVT_file_Path, ...
'RX_time' ,'TOW_at_current_symbol_ms' ,'pos_x','pos_y' ,'pos_z',...
'latitude' ,'longitude','height' ,'pdop' ,'gdop' ,'hdop' ,'vel_x' ,...
'vel_y' ,'vel_z' ,'vdop' ,'week','solution_status','solution_type',...
'valid_sats')
%% GNSS SDR SOLUTION
navSolution.solution_status=solution_status;
navSolution.solution_type=solution_type;
navSolution.valid_sats=valid_sats;
navSolution.RX_time=RX_time;
navSolution.TOW_at_current_symbol_ms=TOW_at_current_symbol_ms;
navSolution.X=pos_x;
navSolution.Y=pos_y;
navSolution.Z=pos_z;
navSolution.latitude=latitude;
navSolution.longitude=longitude;
navSolution.height=height;
navSolution.pdop=pdop;
navSolution.gdop=gdop;
navSolution.hdop=hdop;
navSolution.vX=vel_x;
navSolution.vY=vel_y;
navSolution.vZ=vel_z;
navSolution.vdop=vdop;
navSolution.week=week;
%% clear tmp variables
clearvars -except navSolution
end

114
src/utils/matlab/vtl/SpirentMotion2struct.m Normal file
View File

@ -0,0 +1,114 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
% SpirentMotion2struct Convert CSV file motionV1.csv to a struct.
% refSolution = SpirentMotion2struct(path_to_motion_V1_csv) parse the CSV motionV1.CSV to a struct refSolution
% refSolution has the following fields:
%
% - refSolution.SIM_time: simulation time from 0 (in ms)
% - refSolution.X: UTM referenced position X (in m)
% - refSolution.Y: UTM referenced position Y (in m)
% - refSolution.Z: UTM referenced position Z (in m)
% - refSolution.vX: referenced Velocity X (in m/s)
% - refSolution.vY: referenced Velocity Y (in m/s)
% - refSolution.vZ: referenced Velocity Z (in m/s)
% - refSolution.aX: referenced Aceleration X (in m/s2)
% - refSolution.aY: referenced Aceleration Y (in m/s2)
% - refSolution.aZ: referenced Aceleration Z (in m/s2)
% - refSolution.jX: referenced Jerk X (in m/s3)
% - refSolution.jY: referenced Jerk Y (in m/s3)
% - refSolution.jZ: referenced Jerk Z (in m/s3)
% - refSolution.latitude:reference Latitude(in degrees),
% ellipsoid WGS84?
% - refSolution.longitude:reference Longitude(in degrees),
% ellipsoid WGS84?
% - refSolution.height: referenced Heigh (in m),
% ellipsoid WGS84?
% - refSolution.dop: referenced antenna DOP
%
% -------------------------------------------------------------------------
% USE EXAMPLE: refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv')
% -------------------------------------------------------------------------
function [refSolution] = SpirentMotion2struct(path_to_motion_V1_csv)
% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file')
% addpath('./libs')
% end
%
% if ~exist('cat2geo.m', 'file')
% addpath('./libs/geoFunctions')
% end
%% ===== Import data from text file motion_V1.csv 2 ARRAY ============================
% Script for importing data from the following text file:
%
% filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\log_spirent\motion_V1.csv
%
% Auto-generated by MATLAB on 20-Nov-2022 12:31:17
% Set up the Import Options and import the data
opts = delimitedTextImportOptions("NumVariables", 38);
% Specify range and delimiter
opts.DataLines = [3, Inf];
opts.Delimiter = ",";
% Specify column names and types
opts.VariableNames = ["Time_ms", "Pos_X", "Pos_Y", "Pos_Z", "Vel_X", "Vel_Y", "Vel_Z", "Acc_X", "Acc_Y", "Acc_Z", "Jerk_X", "Jerk_Y", "Jerk_Z", "Lat", "Long", "Height", "Heading", "Elevation", "Bank", "Angvel_X", "Angvel_Y", "Angvel_Z", "Angacc_X", "Angacc_Y", "Angacc_Z", "Ant1_Pos_X", "Ant1_Pos_Y", "Ant1_Pos_Z", "Ant1_Vel_X", "Ant1_Vel_Y", "Ant1_Vel_Z", "Ant1_Acc_X", "Ant1_Acc_Y", "Ant1_Acc_Z", "Ant1_Lat", "Ant1_Long", "Ant1_Height", "Ant1_DOP"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
% Specify file level properties
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";
% Import the data
motionV1 = readtable(path_to_motion_V1_csv, opts);
% Convert to output type
motionV1 = table2array(motionV1);
% Clear temporary variables
clear opts
%% SPIRENT REFERENCE SOLUTION
refSolution.SIM_time=motionV1(:,1);
refSolution.X=motionV1(:,2);
refSolution.Y=motionV1(:,3);
refSolution.Z=motionV1(:,4);
refSolution.vX=motionV1(:,5);
refSolution.vY=motionV1(:,6);
refSolution.vZ=motionV1(:,7);
refSolution.aX=motionV1(:,8);
refSolution.aY=motionV1(:,9);
refSolution.aZ=motionV1(:,10);
refSolution.jX=motionV1(:,11);
refSolution.jY=motionV1(:,12);
refSolution.jZ=motionV1(:,13);
refSolution.latitude=rad2deg(motionV1(:,14));
refSolution.longitude=rad2deg(motionV1(:,15));
refSolution.height=motionV1(:,16);
refSolution.dop=motionV1(:,38);
% Clear temporary variables
clear motionV1
end

196
src/utils/matlab/vtl/SpirentSatData2struct.m Normal file
View File

@ -0,0 +1,196 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
% SpirentSatData2struct Convert CSV file sat_data_V1A1.csv to a struct.
% refSolution = SpirentSatData2struct(path_to_sat_V1_csv) parse the CSV sat_data_V1A1.CSV to a struct refSatData
% refSatData has the following fields:
%
% refSatData.GALILEO.series(i).Sat_ID(j) = GAL(k,3);
% refSatData.GALILEO.series(i).Sat_ID(j)
% refSatData.GALILEO.series(i).Sat_PRN(j)
% refSatData.GALILEO.series(i).sat_X(j)
% refSatData.GALILEO.series(i).sat_Y(j)
% refSatData.GALILEO.series(i).sat_Z(j)
% refSatData.GALILEO.series(i).sat_vX(j)
% refSatData.GALILEO.series(i).sat_vY(j)
% refSatData.GALILEO.series(i).sat_vZ(j)
% refSatData.GALILEO.series(i).azimuth(j)
% refSatData.GALILEO.series(i).elevation(j)
% refSatData.GALILEO.series(i).range(j)
% refSatData.GALILEO.series(i).pr_m(j)
% refSatData.GALILEO.series(i).pr_rate(j)
% refSatData.GALILEO.series(i).iono_delay(j)
% refSatData.GALILEO.series(i).tropo_delay(j)
% refSatData.GALILEO.series(i).pr_error(j)
% refSatData.GALILEO.series(i).signal_dB(j)
% refSatData.GALILEO.series(i).ant_azimuth(j)
% refSatData.GALILEO.series(i).ant_elevation(j)
% refSatData.GALILEO.series(i).range_rate(j)
% refSatData.GALILEO.series(i).pr_Error_rate(j)
% refSatData.GALILEO.series(i).doppler_shift(j)
% refSatData.GALILEO.series(i).delta_range(j)
% refSatData.GALILEO.series(i).sat_Acc_X(j)
% refSatData.GALILEO.series(i).sat_Acc_Y(j)
% refSatData.GALILEO.series(i).sat_Acc_Z(j)
% %
% -------------------------------------------------------------------------
% USE EXAMPLE: refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv')
% -------------------------------------------------------------------------
function [refSatData] = SpirentSatData2struct(path_to_sat_V1_csv)
% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file')
% addpath('./libs')
% end
%
% if ~exist('cat2geo.m', 'file')
% addpath('./libs/geoFunctions')
% end
%% ===== Import data from text file motion_V1.csv 2 ARRAY ============================
% Set up the Import Options and import the data
opts = delimitedTextImportOptions("NumVariables", 50);
% Specify range and delimiter
opts.DataLines = [6, Inf];
opts.Delimiter = ",";
% Specify column names and types
opts.VariableNames = ["Time_ms", "Channel", "Sat_type", "Sat_ID", "Sat_PRN", "Echo_Num", "Sat_Pos_X", "Sat_Pos_Y", "Sat_Pos_Z", "Sat_Vel_X", "Sat_Vel_Y", "Sat_Vel_Z", "Azimuth", "Elevation", "Range", "PRangeGroupA", "PRangeGroupB", "PRangeGroupC", "PRangeGroupD", "PRangeGroupE", "PR_rate", "Iono_delayGroupA", "Iono_delayGroupB", "Iono_delayGroupC", "Iono_delayGroupD", "Iono_delayGroupE", "Tropo_delay", "PR_Error", "Signal_dBGroupA", "Signal_dBGroupB", "Signal_dBGroupC", "Signal_dBGroupD", "Signal_dBGroupE", "Ant_azimuth", "Ant_elevation", "Range_rate", "PR_Error_rate", "Doppler_shiftGroupA", "Doppler_shiftGroupB", "Doppler_shiftGroupC", "Doppler_shiftGroupD", "Doppler_shiftGroupE", "Delta_rangeGroupA", "Delta_rangeGroupB", "Delta_rangeGroupC", "Delta_rangeGroupD", "Delta_rangeGroupE", "Sat_Acc_X", "Sat_Acc_Y", "Sat_Acc_Z"];
opts.VariableTypes = ["double", "double", "char", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
% Specify file level properties
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";
% Specify variable properties
opts = setvaropts(opts, "Sat_type", "WhitespaceRule", "preserve");
opts = setvaropts(opts, "Sat_type", "EmptyFieldRule", "auto");
% Import the data
satdataV1A1SPFLD1 = readtable(path_to_sat_V1_csv, opts);
%% Convert to output type
satdataV1A1SPFLD1 = table2cell(satdataV1A1SPFLD1);
numIdx = cellfun(@(x) ~isnan(str2double(x)), satdataV1A1SPFLD1);
satdataV1A1SPFLD1(numIdx) = cellfun(@(x) {str2double(x)}, satdataV1A1SPFLD1(numIdx));
%% Clear temporary variables
clear opts
%% initialize
refSatData.GPS=[];
refSatData.GALILEO=[];
refSatData.GLONASS=[];
refSatData.BEIDOU=[];
%% split by constellations
[indGALILEO,~]= find(strcmp(satdataV1A1SPFLD1, 'GALILEO'));
[indGPS,~]= find(strcmp(satdataV1A1SPFLD1, 'GPS'));
if(~isempty(indGALILEO))
GAL=satdataV1A1SPFLD1(indGALILEO,:);GAL(:,3)=[];
GAL=cell2mat(GAL);
end
if(~isempty(indGPS))
GPS=satdataV1A1SPFLD1(indGPS,:); GPS(:,3)=[];
GPS=cell2mat(GPS);
end
if(~isempty(indGALILEO))
refSatData.GALILEO.SIM_time=unique(GAL(:,1),'first');
k=0;
for i=1:length(refSatData.GALILEO.SIM_time)
nsats=length(find(GAL(:,1)==refSatData.GALILEO.SIM_time(i)));
refSatData.GALILEO.series(i).nsats=nsats;
for j=1:nsats
k=k+1;
refSatData.GALILEO.series(i).Sat_ID(j) = GAL(k,3);
refSatData.GALILEO.series(i).Sat_ID(j) = GAL(i,3);
refSatData.GALILEO.series(i).Sat_PRN(j) = GAL(i,4);
refSatData.GALILEO.series(i).sat_X(j) = GAL(k,6);
refSatData.GALILEO.series(i).sat_Y(j) = GAL(k,7);
refSatData.GALILEO.series(i).sat_Z(j) = GAL(k,8);
refSatData.GALILEO.series(i).sat_vX(j) = GAL(k,9);
refSatData.GALILEO.series(i).sat_vY(j) = GAL(k,10);
refSatData.GALILEO.series(i).sat_vZ(j) = GAL(k,11);
refSatData.GALILEO.series(i).azimuth(j) = GAL(k,12);
refSatData.GALILEO.series(i).elevation(j) = GAL(k,13);
refSatData.GALILEO.series(i).range(j) = GAL(k,14);
refSatData.GALILEO.series(i).pr_m(j) = GAL(k,15);
refSatData.GALILEO.series(i).pr_rate(j) = GAL(k,20);
refSatData.GALILEO.series(i).iono_delay(j) = GAL(k,21);
refSatData.GALILEO.series(i).tropo_delay(j) = GAL(k,26);%
refSatData.GALILEO.series(i).pr_error(j) = GAL(k,27);
refSatData.GALILEO.series(i).signal_dB(j) = GAL(k,28);
refSatData.GALILEO.series(i).ant_azimuth(j) = GAL(k,33);
refSatData.GALILEO.series(i).ant_elevation(j) = GAL(k,34);
refSatData.GALILEO.series(i).range_rate(j) = GAL(k,35);
refSatData.GALILEO.series(i).pr_Error_rate(j) = GAL(k,36);
refSatData.GALILEO.series(i).doppler_shift(j) = GAL(k,37);
refSatData.GALILEO.series(i).delta_range(j) = GAL(k,42);
refSatData.GALILEO.series(i).sat_Acc_X(j) = GAL(k,47);
refSatData.GALILEO.series(i).sat_Acc_Y(j) = GAL(k,48);
refSatData.GALILEO.series(i).sat_Acc_Z(j) = GAL(k,49);
end
end
end
%% -------------------------------------
if(~isempty(indGPS))
refSatData.GPS.SIM_time=unique(GPS(:,1),'first');
k=0;
for i=1:length(refSatData.GPS.SIM_time)
nsats=length(find(GPS(:,1)==refSatData.GPS.SIM_time(i)));
refSatData.GPS.series(i).nsats=nsats;
for j=1:nsats
k=k+1;
refSatData.GPS.series(i).Sat_ID(j) = GPS(k,3);
refSatData.GPS.series(i).Sat_PRN(j) = GPS(k,4);
refSatData.GPS.series(i).sat_X(j) = GPS(k,6);
refSatData.GPS.series(i).sat_Y(j) = GPS(k,7);
refSatData.GPS.series(i).sat_Z(j) = GPS(k,8);
refSatData.GPS.series(i).sat_vX(j) = GPS(k,9);
refSatData.GPS.series(i).sat_vY(j) = GPS(k,10);
refSatData.GPS.series(i).sat_vZ(j) = GPS(k,11);
refSatData.GPS.series(i).azimuth(j) = GPS(k,12);
refSatData.GPS.series(i).elevation(j) = GPS(k,13);
refSatData.GPS.series(i).range(j) = GPS(k,14);
refSatData.GPS.series(i).pr_m(j) = GPS(k,15);
refSatData.GPS.series(i).pr_rate(j) = GPS(k,20);
refSatData.GPS.series(i).iono_delay(j) = GPS(k,21);
refSatData.GPS.series(i).tropo_delay(j) = GPS(k,26);%
refSatData.GPS.series(i).pr_error(j) = GPS(k,27);
refSatData.GPS.series(i).signal_dB(j) = GPS(k,28);
refSatData.GPS.series(i).ant_azimuth(j) = GPS(k,33);
refSatData.GPS.series(i).ant_elevation(j) = GPS(k,34);
refSatData.GPS.series(i).range_rate(j) = GPS(k,35);
refSatData.GPS.series(i).pr_Error_rate(j) = GPS(k,36);
refSatData.GPS.series(i).doppler_shift(j) = GPS(k,37);
refSatData.GPS.series(i).delta_range(j) = GPS(k,42);
refSatData.GPS.series(i).sat_Acc_X(j) = GPS(k,47);
refSatData.GPS.series(i).sat_Acc_Y(j) = GPS(k,48);
refSatData.GPS.series(i).sat_Acc_Z(j) = GPS(k,49);
end
end
end
% Clear temporary variables
clear satdataV1A1SPFLD05 GPS GAL
end

115
src/utils/matlab/vtl/Vtl2struct.m Normal file
View File

@ -0,0 +1,115 @@
%% Import data from text file
% Script for importing data from the following text file:
%
% filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\spirent_usrp_airing\dump_vtl_file.csv
%
% %
% -------------------------------------------------------------------------
% USE EXAMPLE: vtlSolution = Vtl2struct('dump_vtl_file.csv')
% -------------------------------------------------------------------------
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function [vtlSolution] = Vtl2struct(path_to_vtl_csv)
%% Set up the Import Options and import the data
opts = delimitedTextImportOptions("NumVariables", 9);
% Specify range and delimiter
opts.DataLines = [1, Inf];
opts.Delimiter = ",";
% Specify column names and types
opts.VariableNames = ["kf_state", "e06", "VarName3", "e06_1", "VarName5", "VarName6", "VarName7", "VarName8", "VarName9"];
opts.VariableTypes = ["char", "double", "double", "double", "double", "double", "double", "double", "double"];
% Specify file level properties
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";
% Specify variable properties
opts = setvaropts(opts, "kf_state", "WhitespaceRule", "preserve");
opts = setvaropts(opts, "kf_state", "EmptyFieldRule", "auto");
% Import the data
dumpvtlfile = readtable(path_to_vtl_csv, opts);
%% Convert to output type
dumpvtlfile = table2cell(dumpvtlfile);
numIdx = cellfun(@(x) ~isnan(str2double(x)), dumpvtlfile);
dumpvtlfile(numIdx) = cellfun(@(x) {str2double(x)}, dumpvtlfile(numIdx));
%% Clear temporary variables
clear opts
%%
vtlSolution.kfpvt=[];
vtlSolution.rtklibpvt=[];
%% split by solution type
[indKF,~]= find(strcmp(dumpvtlfile, 'kf_state'));
[indRTKlib,~]= find(strcmp(dumpvtlfile, 'rtklib_state'));
[indkf_err,~]= find(strcmp(dumpvtlfile, 'kf_xerr'));
[ind_filt_dop_sat,~]= find(strcmp(dumpvtlfile, 'filt_dopp_sat'));
[ind_CN0_sat,~]= find(strcmp(dumpvtlfile, 'CN0_sat'));
kfpvt=dumpvtlfile(indKF,:);kfpvt(:,1)=[];
rtklibpvt=dumpvtlfile(indRTKlib,:); rtklibpvt(:,1)=[];
kferr=dumpvtlfile(indkf_err,:); kferr(:,1)=[];
filt_dop_sat=dumpvtlfile(ind_filt_dop_sat,:); filt_dop_sat(:,1)=[];
CN0_sat=dumpvtlfile(ind_CN0_sat,:); CN0_sat(:,1)=[];
kfpvt=cell2mat(kfpvt);
rtklibpvt=cell2mat(rtklibpvt);
kferr=cell2mat(kferr);
filt_dop_sat=cell2mat(filt_dop_sat);
CN0_sat=cell2mat(CN0_sat);
vtlSolution.kfpvt.X=kfpvt(:,1);
vtlSolution.kfpvt.Y=kfpvt(:,2);
vtlSolution.kfpvt.Z=kfpvt(:,3);
vtlSolution.kfpvt.vX=kfpvt(:,4);
vtlSolution.kfpvt.vY=kfpvt(:,5);
vtlSolution.kfpvt.vZ=kfpvt(:,6);
vtlSolution.kfpvt.biasclock=kfpvt(:,7);
vtlSolution.kfpvt.rateblock=kfpvt(:,8);
vtlSolution.rtklibpvt.X=rtklibpvt(:,1);
vtlSolution.rtklibpvt.Y=rtklibpvt(:,2);
vtlSolution.rtklibpvt.Z=rtklibpvt(:,3);
vtlSolution.rtklibpvt.vX=rtklibpvt(:,4);
vtlSolution.rtklibpvt.vY=rtklibpvt(:,5);
vtlSolution.rtklibpvt.vZ=rtklibpvt(:,6);
vtlSolution.rtklibpvt.biasclock=rtklibpvt(:,7);
vtlSolution.rtklibpvt.rateblock=rtklibpvt(:,8);
vtlSolution.kferr.X=kferr(:,1);
vtlSolution.kferr.Y=kferr(:,2);
vtlSolution.kferr.Z=kferr(:,3);
vtlSolution.kferr.vX=kferr(:,4);
vtlSolution.kferr.vY=kferr(:,5);
vtlSolution.kferr.vZ=kferr(:,6);
vtlSolution.kferr.biasclock=kferr(:,7);
vtlSolution.kferr.rateblock=kferr(:,8);
vtlSolution.filt_dop_sat(1,:)=filt_dop_sat(:,1);
vtlSolution.filt_dop_sat(2,:)=filt_dop_sat(:,2);
vtlSolution.filt_dop_sat(3,:)=filt_dop_sat(:,3);
vtlSolution.filt_dop_sat(4,:)=filt_dop_sat(:,4);
vtlSolution.filt_dop_sat(5,:)=filt_dop_sat(:,5);
vtlSolution.CN0_sat(1,:)=CN0_sat(:,1);
vtlSolution.CN0_sat(2,:)=CN0_sat(:,2);
vtlSolution.CN0_sat(3,:)=CN0_sat(:,3);
vtlSolution.CN0_sat(4,:)=CN0_sat(:,4);
vtlSolution.CN0_sat(5,:)=CN0_sat(:,5);
end

76
src/utils/matlab/vtl/dopp_filtered_plotting.m Normal file
View File

@ -0,0 +1,76 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
time_reference_spirent_obs=129780;%s
time_vtl_dump_file=linspace(38,157,length(vtlSolution.filt_dop_sat));
% rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05.
%%
Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz');
subplot(2,2,1);
plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s')
xlim([0,140]);
ylim([-2340,-2220]);
xlabel('')
ylabel('Doppler (Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend('PRN 28 GNSS-SDR','Location','eastoutside')
plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference')
plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(1,:),'o','DisplayName','filtered VTL')
hold off;grid minor
% Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz');
subplot(2,2,2);
plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s')
xlim([0,140]);
ylim([2540,2640]);
xlabel('')
ylabel('Doppler (Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend('PRN 4 GNSS-SDR','Location','eastoutside')
plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','reference')
plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(2,:),'o','DisplayName','filtered VTL')
hold off;grid minor
% Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz');
subplot(2,2,3);
plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s')
xlim([0,140]);
ylim([-1800,-1730]);
xlabel('')
ylabel('Doppler (Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend('PRN 17 GNSS-SDR','Location','eastoutside')
plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','reference')
plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(3,:),'o','DisplayName','filtered VTL')
hold off;grid minor
% Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz');
subplot(2,2,4);
plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s')
xlim([0,140]);
ylim([-2680,-2620]);
xlabel('')
ylabel('Doppler (Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend('PRN 15 GNSS-SDR','Location','eastoutside')
plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','reference')
plot(time_vtl_dump_file,vtlSolution.filt_dop_sat(4,:),'o','DisplayName','filtered VTL')
hold off;grid minor

46
src/utils/matlab/vtl/doppler_vtl_plot.m Normal file
View File

@ -0,0 +1,46 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% LOAD
Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),channels);
for i=1:length(refSatData.GPS.SIM_time)
Carrier_Doppler_hz_sim(i,1)=refSatData.GPS.series(i).doppler_shift(4);%PRN 28
Carrier_Doppler_hz_sim(i,2)=refSatData.GPS.series(i).doppler_shift(3);%PRN 20
Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(8);%PRN 17
Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(6);%PRN 12
Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(5);%PRN 9
Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(2);%PRN 5
Carrier_Doppler_hz_sim(i,7)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4
Carrier_Doppler_hz_sim(i,8)=refSatData.GPS.series(i).doppler_shift(7);%PRN 2
end
%%
figure('Name','RX_Carrier_Doppler_hz');
for channel_cnt=1:channels
subplot(3,3,channel_cnt)
plot(linspace(0,tFinal,length(Carrier_Doppler_hz(channel_cnt,:)')), Carrier_Doppler_hz(channel_cnt,:)','s')
xlim([0,tFinal]);
ylim([min(Carrier_Doppler_hz_sim(:,channel_cnt+1)),max(Carrier_Doppler_hz_sim(:,channel_cnt+1))]);
xlabel('')
ylabel('Doppler (Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench
plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,channel_cnt+1)','.','DisplayName','SPIRENT reference')
plot(linspace(TTFF_sec+23,tFinal,length(sat_dopp_hz_filt(channel_cnt,:))),sat_dopp_hz_filt(channel_cnt,:),'o','DisplayName','filtered VTL')
hold off;grid minor
end

195
src/utils/matlab/vtl/general_raw_plot.m Normal file
View File

@ -0,0 +1,195 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%general_raw_plot.m
% navSolution.X(navSolution.X==0) = [];
%% ====== GNSS-SDR Plot all figures =======================================================
close all
%--- LLH POSITION: GNSS SDR plot on map --------------------------------------
LLH=figure('Name','LLH system on map');
geoplot([navSolution.latitude],[navSolution.longitude],'.')
geobasemap satellite
hold on
geoplot(LAT_FILT,LON_FILT,'r.')
title('Position in LLH on map ')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
% geobasemap streets
% geobasemap topographic
% geobasemap streets-dark
%---VELOCITY: GNSS SDR plot --------------------------------------
VEL=figure('Name','velocities and heigh');
subplot(2,2,1);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),vX_FILT,'r.');
ylabel('vX (m/s)')
xlabel('time from First FIX in (seconds)')
title('Subplot 1: vX GOR')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),vY_FILT,'r.');
ylabel('vY (m/s)')
xlabel('time from First FIX in (seconds)')
title('Subplot 2: vY')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),vZ_FILT,'r.');
ylabel('vZ (m/s)')
xlabel('time from First FIX in (seconds)')
title('Subplot 3: vZ')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
subplot(2,2,4);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.');
ylabel('HEIGH (m)')
xlabel('time from First FIX in (seconds)')
title('Subplot 4: HEIGH')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
sgtitle('velocities and heigh')
% --- UTM centered POSITION: GNSS SDR plot --------------------------------------
POS=figure('Name','UTM COORD CENTERED IN 1^{ST} POSITION');
subplot(2,2,1);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X-refSolution.X(1),'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),X_FILT-refSolution.X(1),'r.');
ylabel('X (m)')
xlabel('time from First FIX in (seconds)')
% ylim([-55 100])
title('Subplot 1: X GOR')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y-refSolution.Y(1),'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),Y_FILT-refSolution.Y(1),'r.');
ylabel('Y (m)')
xlabel('time from First FIX in (seconds)')
% ylim([-140 -20])
title('Subplot 2: Y')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z-refSolution.Z(1),'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),Z_FILT-refSolution.Z(1),'r.');
ylabel('Z (m)')
xlabel('time from First FIX in (seconds)')
% ylim([-150 20])
title('Subplot 3: Z')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
sgtitle('UTM COORD CENTERED IN 1^{ST} POSITION')
% --- UTM full POSITION: GNSS SDR plot --------------------------------------
POS_utm=figure('Name','UTM COORD IN 1^{ST} POSITION');
subplot(2,2,1);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),X_FILT,'r.');
ylabel('X (m)')
xlabel('time from First FIX in (seconds)')
% ylim([-55 100])
title('Subplot 1: X GOR')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),Y_FILT,'r.');
ylabel('Y (m)')
xlabel('time from First FIX in (seconds)')
% ylim([-140 -20])
title('Subplot 2: Y')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),Z_FILT,'r.');
ylabel('Z (m)')
xlabel('time from First FIX in (seconds)')
% ylim([-150 20])
title('Subplot 3: Z')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
sgtitle('UTM COORD FULL IN 1^{ST} POSITION')
%% ====== SPIRENT Plot all figures =======================================================
if(plot_reference)
%--- LLH POSITION: SPIRENT plot on map --------------------------------------
figure(LLH)
geoplot([refSolution.latitude],[refSolution.longitude],'.','DisplayName','reference')
geobasemap satellite
hold off
% geobasemap streets
% geobasemap topographic
% geobasemap streets-dark
%---VELOCITY: SPIRENT plot --------------------------------------
figure(VEL)
subplot(2,2,1);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX,'.','DisplayName','reference');
hold off;grid on
subplot(2,2,2);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY,'.','DisplayName','reference');
hold on;grid on
subplot(2,2,3);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ,'.','DisplayName','reference');
hold on;grid on
subplot(2,2,4);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.height,'.','DisplayName','reference');
hold on;grid on
%---UTM centered POSITION: SPIRENT plot --------------------------------------
figure(POS)
subplot(2,2,1);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X-refSolution.X(1),'.','DisplayName','reference');
hold off;grid on
subplot(2,2,2);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y-refSolution.Y(1),'.','DisplayName','reference');
hold on;grid on
subplot(2,2,3);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z-refSolution.Z(1),'.','DisplayName','reference');
hold on;grid on
%---UTM POSITION: SPIRENT plot --------------------------------------
figure(POS_utm)
subplot(2,2,1);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X,'.','DisplayName','reference');
hold off;grid on
subplot(2,2,2);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y,'.','DisplayName','reference');
hold on;grid on
subplot(2,2,3);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z,'.','DisplayName','reference');
hold on;grid on
end

467
src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m Normal file
View File

@ -0,0 +1,467 @@
% Read PVG raw dump
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
clc
close all;
clear all;
% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file')
% addpath('./libs')
% end
%
% if ~exist('cat2geo.m', 'file')
% addpath('./libs/geoFunctions')
% end
%%
samplingFreq=25000000;
channels=6;
TTFF_sec=41.48;
path='';
pvt_raw_log_path=[path 'PVT_raw.dat'];
% GNSS_PVT_raw= gps_l1_ca_read_pvt_raw_dump(channels,pvt_raw_log_path);
GnssSDR2struct('PVT_raw.mat')
%% ===== Import data from text file motion_V1.csv 2 ARRAY ============================
% Script for importing data from the following text file:
%
% filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\log_spirent\motion_V1.csv
%
% Auto-generated by MATLAB on 20-Nov-2022 12:31:17
% Set up the Import Options and import the data
opts = delimitedTextImportOptions("NumVariables", 38);
% Specify range and delimiter
opts.DataLines = [3, Inf];
opts.Delimiter = ",";
% Specify column names and types
opts.VariableNames = ["Time_ms", "Pos_X", "Pos_Y", "Pos_Z", "Vel_X", "Vel_Y", "Vel_Z", "Acc_X", "Acc_Y", "Acc_Z", "Jerk_X", "Jerk_Y", "Jerk_Z", "Lat", "Long", "Height", "Heading", "Elevation", "Bank", "Angvel_X", "Angvel_Y", "Angvel_Z", "Angacc_X", "Angacc_Y", "Angacc_Z", "Ant1_Pos_X", "Ant1_Pos_Y", "Ant1_Pos_Z", "Ant1_Vel_X", "Ant1_Vel_Y", "Ant1_Vel_Z", "Ant1_Acc_X", "Ant1_Acc_Y", "Ant1_Acc_Z", "Ant1_Lat", "Ant1_Long", "Ant1_Height", "Ant1_DOP"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
% Specify file level properties
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";
% Import the data
motionV1 = readtable("..\log_spirent\motion_V1_SPF_LD_05.csv", opts);
% Convert to output type
motionV1 = table2array(motionV1);
% Clear temporary variables
clear opts
%% ============================ PARSER TO STRUCT ============================
plot_skyplot=1;
%% GNSS SDR SOLUTION
% navSolution.samplingFreq=25000000;
% navSolution.channels=6;
navSolution.solution_status=solution_status;
navSolution.solution_type=solution_type;
navSolution.valid_sats=valid_sats;
navSolution.RX_time=RX_time;
navSolution.TOW_at_current_symbol_ms=TOW_at_current_symbol_ms;
navSolution.X=pos_x;
navSolution.Y=pos_y;
navSolution.Z=pos_z;
navSolution.latitude=latitude;
navSolution.longitude=longitude;
navSolution.height=height;
navSolution.pdop=pdop;
navSolution.gdop=gdop;
navSolution.hdop=hdop;
navSolution.vX=vel_x;
navSolution.vY=vel_y;
navSolution.vZ=vel_z;
navSolution.vdop=vdop;
navSolution.week=week;
%% SPIRENT REFERENCE SOLUTION
refSolution.SIM_time=motionV1(:,1);
refSolution.X=motionV1(:,2);
refSolution.Y=motionV1(:,3);
refSolution.Z=motionV1(:,4);
refSolution.vX=motionV1(:,5);
refSolution.vY=motionV1(:,6);
refSolution.vZ=motionV1(:,7);
refSolution.aX=motionV1(:,8);
refSolution.aY=motionV1(:,9);
refSolution.aZ=motionV1(:,10);
refSolution.jX=motionV1(:,11);
refSolution.jY=motionV1(:,12);
refSolution.jZ=motionV1(:,13);
refSolution.latitude=rad2deg(motionV1(:,14));
refSolution.longitude=rad2deg(motionV1(:,15));
refSolution.height=motionV1(:,16);
refSolution.dop=motionV1(:,38);
% Clear temporary variables
clear motionV1
%% === Convert to UTM coordinate system =============================
% Scenario latitude is xx.xxxxxxx N37 49 9.98
% Scenario longitude is -xx.xxxxxxx W122 28 42.58
% Scenario elevation is 35 meters.
% lat=[37 49 9.98];
% long=[-122 -28 -42.58];
% lat_deg=dms2deg(lat);
% long_deg=dms2deg(long);
% h=35;
lat_deg=navSolution.latitude(1);
lon_deg=navSolution.longitude(1);
lat=degrees2dms(lat_deg);
lon=degrees2dms(lon_deg);
h=navSolution.height(1);
utmstruct = defaultm('utm');
utmstruct.zone = utmzone(lat_deg, lon_deg);
utmstruct = defaultm(utmstruct);
[utmstruct.latlim,utmstruct.lonlim] = utmzone(utmstruct.zone );
%Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
utmstruct.geoid = wgs84Ellipsoid;
% [X, Y] = projfwd(utmstruct,lat_deg,lon_deg);
% Z=h; % geographical to cartesian conversion
% for k=1:1:length(navSolution.X)
% [navSolution.E(k), ...
% navSolution.N(k), ...
% navSolution.U(k)]=cart2utm(navSolution.X(k), navSolution.Y(k), navSolution.Z(k), utmZone);
% end
%% ====== FILTERING =======================================================
moving_avg_factor= 500;
LAT_FILT = movmean(navSolution.latitude,moving_avg_factor);
LON_FILT = movmean(navSolution.longitude,moving_avg_factor);
HEIGH_FILT = movmean(navSolution.height,moving_avg_factor);
X_FILT = movmean(navSolution.X,moving_avg_factor);
Y_FILT = movmean(navSolution.Y,moving_avg_factor);
Z_FILT = movmean(navSolution.Z,moving_avg_factor);
vX_FILT = movmean(navSolution.vX,moving_avg_factor);
vY_FILT = movmean(navSolution.vY,moving_avg_factor);
vZ_FILT = movmean(navSolution.vZ,moving_avg_factor);
%% ====== GNSS-SDR Plot all figures =======================================================
close all
%--- LLH POSITION: GNSS SDR plot on map --------------------------------------
LLH=figure('Name','LLH system on map');
geoplot([navSolution.latitude],[navSolution.longitude],'.')
geobasemap satellite
hold on
geoplot([LAT_FILT],[LON_FILT],'r.')
title('Position in LLH on map ')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
% geobasemap streets
% geobasemap topographic
% geobasemap streets-dark
%---VELOCITY: GNSS SDR plot --------------------------------------
VEL=figure('Name','velocities and heigh');
subplot(2,2,1);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),vX_FILT,'r.');
ylabel('vX (m/s)')
xlabel('time from First FIX in (seconds)')
title('Subplot 1: vX GOR')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),vY_FILT,'r.');
ylabel('vY (m/s)')
xlabel('time from First FIX in (seconds)')
title('Subplot 2: vY')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),vZ_FILT,'r.');
ylabel('vZ (m/s)')
xlabel('time from First FIX in (seconds)')
title('Subplot 3: vZ')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
subplot(2,2,4);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.');
ylabel('HEIGH (m)')
xlabel('time from First FIX in (seconds)')
title('Subplot 4: HEIGH')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
sgtitle('velocities and heigh')
% --- UTM centered POSITION: GNSS SDR plot --------------------------------------
POS=figure('Name','UTM COORD CENTERED IN 1^{ST} POSITION');
subplot(2,2,1);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X-refSolution.X(1),'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),X_FILT-refSolution.X(1),'r.');
ylabel('X (m)')
xlabel('time from First FIX in (seconds)')
title('Subplot 1: X GOR')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y-refSolution.Y(1),'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),Y_FILT-refSolution.Y(1),'r.');
ylabel('Y (m)')
xlabel('time from First FIX in (seconds)')
title('Subplot 2: Y')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z-refSolution.Z(1),'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),Z_FILT-refSolution.Z(1),'r.');
ylabel('Z (m)')
xlabel('time from First FIX in (seconds)')
title('Subplot 3: Z')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','east')
sgtitle('UTM COORD CENTERED IN 1^{ST} POSITION')
%% ====== SPIRENT Plot all figures =======================================================
%--- LLH POSITION: SPIRENT plot on map --------------------------------------
figure(LLH)
geoplot([refSolution.latitude],[refSolution.longitude],'.','DisplayName','reference')
geobasemap satellite
hold off
% geobasemap streets
% geobasemap topographic
% geobasemap streets-dark
%---VELOCITY: SPIRENT plot --------------------------------------
figure(VEL)
subplot(2,2,1);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX,'.','DisplayName','reference');
hold off;grid on
subplot(2,2,2);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY,'.','DisplayName','reference');
hold on;grid on
subplot(2,2,3);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ,'.','DisplayName','reference');
hold on;grid on
subplot(2,2,4);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.height,'.','DisplayName','reference');
hold on;grid on
%---UTM centered POSITION: SPIRENT plot --------------------------------------
figure(POS)
subplot(2,2,1);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X-refSolution.X(1),'.','DisplayName','reference');
hold off;grid on
subplot(2,2,2);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y-refSolution.Y(1),'.','DisplayName','reference');
hold on;grid on
subplot(2,2,3);
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z-refSolution.Z(1),'.','DisplayName','reference');
hold on;grid on
%% ==== ERRORS Plots Navigation errors =======================================================
% figureNumber = 300;
% % The 300 is chosen for more convenient handling of the open
% % figure windows, when many figures are closed and reopened. Figures
% % drawn or opened by the user, will not be "overwritten" by this
% % function if the auto numbering is not used.
%
% %=== Select (or create) and clear the figure ==========================
% figure(figureNumber);
% clf (figureNumber);
% set (figureNumber, 'Name', 'Navigation errors');
%
% %--- Draw axes --------------------------------------------------------
% handles(1, 1) = subplot(4, 2, 1 : 4);
% handles(3, 1) = subplot(4, 2, [5, 7]);
% handles(3, 2) = subplot(4, 2, [6, 8]);
%
%
% %
% idx_syn=max(find(TTFF_sec*1000>refSolution.SIM_time));
% %--- VELOCITY differences -----------------------------
% plot([(navSolution.vX - refSolution.vX(idx_syn:end))'; ...
% (navSolution.vY - refSolution.vY)';...
% (navSolution.vZ - refSolution.vZ)']);
%
% title (handles(1, 1), 'VELOCITY variations');
% legend(handles(1, 1), 'vX', 'vY', 'vZ');
% xlabel(handles(1, 1), ['Measurement period: ', ...
% num2str(424242), 'ms']);
% ylabel(handles(1, 1), 'Variations (m/s)');
% grid (handles(1, 1));
% axis (handles(1, 1), 'tight');
%%
% plotNavigation(navSolutions,settings,plot_skyplot);
%
% %% Plot results in the necessary data exists ==============================
% if (~isempty(navSolutions))
%
% %% If reference position is not provided, then set reference position
% %% to the average position
% if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ...
% || isnan(settings.truePosition.U)
%
% %=== Compute mean values ==========================================
% % Remove NaN-s or the output of the function MEAN will be NaN.
% refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E)));
% refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N)));
% refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U)));
%
% %Also convert geodetic coordinates to deg:min:sec vector format
% meanLongitude = dms2mat(deg2dms(...
% mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5);
% meanLatitude = dms2mat(deg2dms(...
% mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5);
%
% LatLong_str=[num2str(meanLatitude(1)), '??', ...
% num2str(meanLatitude(2)), '''', ...
% num2str(meanLatitude(3)), '''''', ...
% ',', ...
% num2str(meanLongitude(1)), '??', ...
% num2str(meanLongitude(2)), '''', ...
% num2str(meanLongitude(3)), '''''']
%
%
%
% refPointLgText = ['Mean Position\newline Lat: ', ...
% num2str(meanLatitude(1)), '{\circ}', ...
% num2str(meanLatitude(2)), '{\prime}', ...
% num2str(meanLatitude(3)), '{\prime}{\prime}', ...
% '\newline Lng: ', ...
% num2str(meanLongitude(1)), '{\circ}', ...
% num2str(meanLongitude(2)), '{\prime}', ...
% num2str(meanLongitude(3)), '{\prime}{\prime}', ...
% '\newline Hgt: ', ...
% num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')];
%
% else
% % compute the mean error for static receiver
% mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E)));
% mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N)));
% mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U)));
% refCoord.E = settings.truePosition.E;
% refCoord.N = settings.truePosition.N;
% refCoord.U = settings.truePosition.U;
%
% error_meters=sqrt((mean_position.E-refCoord.E)^2+(mean_position.N-refCoord.N)^2+(mean_position.U-refCoord.U)^2);
%
% refPointLgText = ['Reference Position, Mean 3D error = ' num2str(error_meters) ' [m]'];
% end
%
% figureNumber = 300;
% % The 300 is chosen for more convenient handling of the open
% % figure windows, when many figures are closed and reopened. Figures
% % drawn or opened by the user, will not be "overwritten" by this
% % function if the auto numbering is not used.
%
% %=== Select (or create) and clear the figure ==========================
% figure(figureNumber);
% clf (figureNumber);
% set (figureNumber, 'Name', 'Navigation solutions');
%
% %--- Draw axes --------------------------------------------------------
% handles(1, 1) = subplot(4, 2, 1 : 4);
% handles(3, 1) = subplot(4, 2, [5, 7]);
% handles(3, 2) = subplot(4, 2, [6, 8]);
%% Plot all figures =======================================================
%
% %--- Coordinate differences in UTM system -----------------------------
% plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ...
% (navSolutions.N - refCoord.N)',...
% (navSolutions.U - refCoord.U)']);
%
% title (handles(1, 1), 'Coordinates variations in UTM system');
% legend(handles(1, 1), 'E', 'N', 'U');
% xlabel(handles(1, 1), ['Measurement period: ', ...
% num2str(settings.navSolPeriod), 'ms']);
% ylabel(handles(1, 1), 'Variations (m)');
% grid (handles(1, 1));
% axis (handles(1, 1), 'tight');
%
% %--- Position plot in UTM system --------------------------------------
% plot3 (handles(3, 1), navSolutions.E - refCoord.E, ...
% navSolutions.N - refCoord.N, ...
% navSolutions.U - refCoord.U, '+');
% hold (handles(3, 1), 'on');
%
% %Plot the reference point
% plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10);
% hold (handles(3, 1), 'off');
%
% view (handles(3, 1), 0, 90);
% axis (handles(3, 1), 'equal');
% grid (handles(3, 1), 'minor');
%
% legend(handles(3, 1), 'Measurements', refPointLgText);
%
% title (handles(3, 1), 'Positions in UTM system (3D plot)');
% xlabel(handles(3, 1), 'East (m)');
% ylabel(handles(3, 1), 'North (m)');
% zlabel(handles(3, 1), 'Upping (m)');
%
% if (plot_skyplot==1)
% %--- Satellite sky plot -----------------------------------------------
% skyPlot(handles(3, 2), ...
% navSolution.channel.az, ...
% navSolution.channel.el, ...
% navSolution.channel.PRN(:, 1));
%
% % title (handles(3, 2), ['Sky plot (mean PDOP: ', ...
% % num2str(mean(navSolution.DOP(2,:))), ')']);
% end
%
% else
% disp('plotNavigation: No navigation data to plot.');
% end % if (~isempty(navSolutions))

247
src/utils/matlab/vtl/kf_prototype.m Normal file
View File

@ -0,0 +1,247 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% vtl KF
%%
sat_number=5;
%% ################## Kalman filter initialization ######################################
st_nmbr=8;
% covariances (static)
kf_P_x = eye(st_nmbr); %TODO: use a real value.
kf_x = zeros(st_nmbr, 1);
kf_R = zeros(2*sat_number, 2*sat_number);
kf_dt=0.1;
% kf_F = eye(st_nmbr, st_nmbr);
% kf_F(1, 4) = kf_dt;
% kf_F(2, 5) = kf_dt;
% kf_F(3, 6) = kf_dt;
% kf_F(7, 8) = kf_dt;
% kf_F=[ 1 0 0 kf_dt 0 0 0 0 0
% 0 1 0 0 kf_dt 0 0 0 0
% 0 0 1 0 0 kf_dt 0 0 0
% 0 0 0 1 0 0 0 0 0
% 0 0 0 0 1 0 0 0 0
% 0 0 0 0 0 1 0 0 0
% 0 0 0 0 0 0 1 kf_dt kf_dt^2/2
% 0 0 0 0 0 0 0 kf_dt 1];
kf_F=[ 1 0 0 kf_dt 0 0 0 0
0 1 0 0 kf_dt 0 0 0
0 0 1 0 0 kf_dt 0 0
0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 kf_dt
0 0 0 0 0 0 0 1];
kf_H = zeros(2*sat_number,st_nmbr);
kf_y = zeros(2*sat_number, 1);
kf_yerr = zeros(2*sat_number, 1);
% kf_xerr = zeros(8, 1);
kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
%% pre-allocate for speed
d = zeros(sat_number, 1);
rho_pri = zeros(sat_number, 1);
rhoDot_pri = zeros(sat_number, 1);
a_x = zeros(sat_number, 1);
a_y = zeros(sat_number, 1);
a_z = zeros(sat_number, 1);
c_pr_m=zeros(sat_number,length(navSolution.RX_time));
pr_m_filt=zeros(sat_number,length(navSolution.RX_time));
rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time));
sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time));
%% ################## Kalman Tracking ######################################
% receiver solution from rtklib_solver
for t=2:length(navSolution.RX_time)
%% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS)
if (t<length(navSolution.RX_time)-point_of_closure)
%% State error Covariance Matrix Q (PVT)
kf_Q = eye(st_nmbr);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate.
%%
% Measurement error Covariance Matrix R assembling
kf_R = blkdiag(eye(sat_number)*40,eye(sat_number)*10);
else
kf_Q = blkdiag(eye(3)*pos_var,eye(3)*vel_var,clk_bias_var,clk_drift_var);
kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var);
end
clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g
if (t<length(navSolution.RX_time)-point_of_closure)
kf_x(1,t) = navSolution.X(t);
kf_x(2,t) = navSolution.Y(t);
kf_x(3,t) = navSolution.Z(t);
kf_x(4,t) = navSolution.vX(t);
kf_x(5,t) = navSolution.vY(t);
kf_x(6,t) = navSolution.vZ(t);
kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1);
x_u=kf_x(1,t);
y_u=kf_x(2,t);
z_u=kf_x(3,t);
xDot_u=kf_x(4,t);
yDot_u=kf_x(5,t);
zDot_u=kf_x(6,t);
cdeltat_u(t)=kf_x(7,t);
cdeltatDot_u=kf_x(8,t);
% Kalman state prediction (time update)
kf_P_x = eye(st_nmbr)*100; %TODO: use a real value.
kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
else
x_u=corr_kf_state(1,t-1);
y_u=corr_kf_state(2,t-1);
z_u=corr_kf_state(3,t-1);
xDot_u=corr_kf_state(4,t-1);
yDot_u=corr_kf_state(5,t-1);
zDot_u=corr_kf_state(6,t-1);
cdeltat_u(t)=corr_kf_state(7,t-1);
cdeltatDot_u=corr_kf_state(8,t-1);
% Kalman state prediction (time update)
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
end
for chan=1:5 %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u(t);
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
end
for chan=1:5 % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
end
% unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H));
% !!!! Limitaciones
% obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad
% no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los
% sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1].
% Kalman estimation (measurement update)
for chan=1:5 % Measurement matrix H assembling
kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);
if (t<length(navSolution.RX_time)-point_of_closure)
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
else
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t);
end
end
% DOUBLES DIFFERENCES
% kf_yerr = zeros(2*sat_number, 1);
% for (int32_t i = 1; i < sat_number; i++) % Measurement vector
% {
% kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1);
% kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1));
% kf_y(i+sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1;
% kf_yerr(i+sat_number)=kf_y(i+sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1));
% }
% kf_yerr.print("DOUBLES DIFFERENCES");
% Kalman filter update step
kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S)
kf_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain
kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation
kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix
if (t<length(navSolution.RX_time)-point_of_closure)
corr_kf_state(:,t)=kf_xpri(:,t)-kf_xerr(:,t)+kf_x(:,t); %updated state estimation
else
corr_kf_state(:,t)=corr_kf_state(:,t-1)-kf_xerr(:,t); %updated state estimation
end
%% ################## Geometric Transformation ######################################
x_u=corr_kf_state(1,t);
y_u=corr_kf_state(2,t);
z_u=corr_kf_state(3,t);
xDot_u=corr_kf_state(4,t);
yDot_u=corr_kf_state(5,t);
zDot_u=corr_kf_state(6,t);
cdeltat_u_g=corr_kf_state(7,t);
cdeltatDot_u_g=corr_kf_state(8,t);
for chan=1:5 %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u_g;
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u_g;
end
kf_H = zeros(2*sat_number,st_nmbr);
for chan=1:5 % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
end
% Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
kf_yerr_g=kf_H*kf_xerr;
% Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
for chan=1:5 % Measurement vector
pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered
rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t);
%convert rhoDot_pri to doppler shift!
% d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1));
% d_dt_clk_drift=0;
if (t<length(navSolution.RX_time)-point_of_closure)
sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1;
else
sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t))/Lambda_GPS_L1;
end
% carrier_phase_rads = 0;
carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t);
% carrier_freq_rate_hz_s = 0;
% code_phase_chips = 0;
end
% carrier_phase_rads = 0;
% carrier_freq_hz = 0;
% carrier_freq_rate_hz_s = 0;
% code_phase_chips = 0;
end

246
src/utils/matlab/vtl/kf_prototype4acc.m Normal file
View File

@ -0,0 +1,246 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% vtl KF
%%
sat_number=5;
%% ################## Kalman filter initialization ######################################
st_nmbr=9;
% covariances (static)
kf_P_x = eye(st_nmbr); %TODO: use a real value.
kf_x = zeros(st_nmbr, 1);
kf_R = zeros(2*sat_number, 2*sat_number);
kf_dt=0.1;
% kf_F = eye(st_nmbr, st_nmbr);
% kf_F(1, 4) = kf_dt;
% kf_F(2, 5) = kf_dt;
% kf_F(3, 6) = kf_dt;
% kf_F(7, 8) = kf_dt;
kf_F=[ 1 0 0 kf_dt 0 0 0 0 0
0 1 0 0 kf_dt 0 0 0 0
0 0 1 0 0 kf_dt 0 0 0
0 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 kf_dt kf_dt^2/2
0 0 0 0 0 0 0 1 kf_dt
0 0 0 0 0 0 0 0 1];
% kf_F=[ 1 0 0 kf_dt 0 0 0 0
% 0 1 0 0 kf_dt 0 0 0
% 0 0 1 0 0 kf_dt 0 0
% 0 0 0 1 0 0 0 0
% 0 0 0 0 1 0 0 0
% 0 0 0 0 0 1 0 0
% 0 0 0 0 0 0 1 kf_dt
% 0 0 0 0 0 0 0 1];
%
kf_H = zeros(2*sat_number,st_nmbr);
kf_y = zeros(2*sat_number, 1);
kf_yerr = zeros(2*sat_number, 1);
% kf_xerr = zeros(8, 1);
kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
%% pre-allocate for speed
d = zeros(sat_number, 1);
rho_pri = zeros(sat_number, 1);
rhoDot_pri = zeros(sat_number, 1);
a_x = zeros(sat_number, 1);
a_y = zeros(sat_number, 1);
a_z = zeros(sat_number, 1);
c_pr_m=zeros(sat_number,length(navSolution.RX_time));
pr_m_filt=zeros(sat_number,length(navSolution.RX_time));
rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time));
sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time));
%% ################## Kalman Tracking ######################################
% receiver solution from rtklib_solver
for t=2:length(navSolution.RX_time)
%% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS)
if (t<length(navSolution.RX_time)-point_of_closure)
%% State error Covariance Matrix Q (PVT)
kf_Q = eye(st_nmbr);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate.
%%
% Measurement error Covariance Matrix R assembling
kf_R = blkdiag(eye(sat_number)*40,eye(sat_number)*10);
else
kf_Q = blkdiag(eye(3)*pos_var,eye(3)*vel_var,clk_bias_var,clk_drift_var,clk_d_drift_var);
kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var);
end
clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g
if (t<length(navSolution.RX_time)-point_of_closure)
kf_x(1,t) = navSolution.X(t);
kf_x(2,t) = navSolution.Y(t);
kf_x(3,t) = navSolution.Z(t);
kf_x(4,t) = navSolution.vX(t);
kf_x(5,t) = navSolution.vY(t);
kf_x(6,t) = navSolution.vZ(t);
kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1);
kf_x(9,t) = 1.0;
x_u=kf_x(1,t);
y_u=kf_x(2,t);
z_u=kf_x(3,t);
xDot_u=kf_x(4,t);
yDot_u=kf_x(5,t);
zDot_u=kf_x(6,t);
cdeltat_u(t)=kf_x(7,t);
cdeltatDot_u=kf_x(8,t);
% Kalman state prediction (time update)
kf_P_x = eye(st_nmbr)*100; %TODO: use a real value.
kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
else
x_u=corr_kf_state(1,t-1);
y_u=corr_kf_state(2,t-1);
z_u=corr_kf_state(3,t-1);
xDot_u=corr_kf_state(4,t-1);
yDot_u=corr_kf_state(5,t-1);
zDot_u=corr_kf_state(6,t-1);
cdeltat_u(t)=corr_kf_state(7,t-1);
cdeltatDot_u=corr_kf_state(8,t-1);
% Kalman state prediction (time update)
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
end
for chan=1:5 %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u(t);
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
end
for chan=1:5 % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
end
% unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H));
% !!!! Limitaciones
% obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad
% no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los
% sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1].
% Kalman estimation (measurement update)
for chan=1:5 % Measurement matrix H assembling
kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);
if (t<length(navSolution.RX_time)-point_of_closure)
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
else
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t);
end
end
% DOUBLES DIFFERENCES
% kf_yerr = zeros(2*sat_number, 1);
% for (int32_t i = 1; i < sat_number; i++) % Measurement vector
% {
% kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1);
% kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1));
% kf_y(i+sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1;
% kf_yerr(i+sat_number)=kf_y(i+sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1));
% }
% kf_yerr.print("DOUBLES DIFFERENCES");
% Kalman filter update step
kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S)
kf_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain
kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation
kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix
if (t<length(navSolution.RX_time)-point_of_closure)
corr_kf_state(:,t)=kf_xpri(:,t)-kf_xerr(:,t)+kf_x(:,t); %updated state estimation
else
corr_kf_state(:,t)=corr_kf_state(:,t-1)-kf_xerr(:,t); %updated state estimation
end
%% ################## Geometric Transformation ######################################
x_u=corr_kf_state(1,t);
y_u=corr_kf_state(2,t);
z_u=corr_kf_state(3,t);
xDot_u=corr_kf_state(4,t);
yDot_u=corr_kf_state(5,t);
zDot_u=corr_kf_state(6,t);
cdeltat_u_g=corr_kf_state(7,t);
cdeltatDot_u_g=corr_kf_state(8,t);
for chan=1:5 %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u_g;
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
end
kf_H = zeros(2*sat_number,st_nmbr);
for chan=1:5 % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
end
% Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
kf_yerr_g=kf_H*kf_xerr;
% Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
for chan=1:5 % Measurement vector
pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered
rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t);
%convert rhoDot_pri to doppler shift!
% d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1));
% d_dt_clk_drift=0;
if (t<length(navSolution.RX_time)-point_of_closure)
sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1;
else
sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1;
end
err_carrier_phase_rads_filt(chan,t) = trapz(kf_yerr_g(chan+sat_number,1:t)/Lambda_GPS_L1)*2*kf_dt;
carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t);
% carrier_freq_rate_hz_s = 0;
err_code_phase_chips(chan,t) = (kf_yerr_g(chan,t))/SPEED_OF_LIGHT_M_S*1023e3;
end
end

229
src/utils/matlab/vtl/kf_prototype_clk_d.m Normal file
View File

@ -0,0 +1,229 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% vtl KF
%%
sat_number=5;
%% ################## Kalman filter initialization ######################################
st_nmbr=9;
% covariances (static)
kf_P_x = eye(st_nmbr); %TODO: use a real value.
kf_x = zeros(st_nmbr, 1);
kf_R = zeros(2*sat_number, 2*sat_number);
kf_dt=0.1;
% kf_F = eye(st_nmbr, st_nmbr);
% kf_F(1, 4) = kf_dt;
% kf_F(2, 5) = kf_dt;
% kf_F(3, 6) = kf_dt;
% kf_F(7, 8) = kf_dt;
kf_F=[ 1 0 0 kf_dt 0 0 0 0 0
0 1 0 0 kf_dt 0 0 0 0
0 0 1 0 0 kf_dt 0 0 0
0 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 kf_dt kf_dt^2/2
0 0 0 0 0 0 0 1 kf_dt
0 0 0 0 0 0 0 0 1];
% kf_F=[ 1 0 0 kf_dt 0 0 0 0
% 0 1 0 0 kf_dt 0 0 0
% 0 0 1 0 0 kf_dt 0 0
% 0 0 0 1 0 0 0 0
% 0 0 0 0 1 0 0 0
% 0 0 0 0 0 1 0 0
% 0 0 0 0 0 0 1 kf_dt
% 0 0 0 0 0 0 0 1];
kf_H = zeros(2*sat_number,st_nmbr);
kf_y = zeros(2*sat_number, 1);
kf_yerr = zeros(2*sat_number, 1);
% kf_xerr = zeros(8, 1);
kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
%% pre-allocate for speed
d = zeros(sat_number, 1);
rho_pri = zeros(sat_number, 1);
rhoDot_pri = zeros(sat_number, 1);
a_x = zeros(sat_number, 1);
a_y = zeros(sat_number, 1);
a_z = zeros(sat_number, 1);
c_pr_m=zeros(sat_number,length(navSolution.RX_time));
pr_m_filt=zeros(sat_number,length(navSolution.RX_time));
rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time));
sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time));
%% ################## Kalman Tracking ######################################
% receiver solution from rtklib_solver
for t=2:length(navSolution.RX_time)
%% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS)
if (t<3)
%% State error Covariance Matrix Q (PVT)
kf_Q = eye(st_nmbr);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate.
%%
% Measurement error Covariance Matrix R assembling
kf_R = blkdiag(eye(sat_number)*40,eye(sat_number)*10);
else
kf_Q = blkdiag(eye(3)*pos_var,eye(3)*vel_var,clk_bias_var,clk_drift_var,clk_d_drift_var);
kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var);
end
clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g
if (t<3)
kf_x(1,t) = navSolution.X(t);
kf_x(2,t) = navSolution.Y(t);
kf_x(3,t) = navSolution.Z(t);
kf_x(4,t) = navSolution.vX(t);
kf_x(5,t) = navSolution.vY(t);
kf_x(6,t) = navSolution.vZ(t);
kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1);
kf_x(9,t) = 0;%new_data.rx_dts(1);
x_u=kf_x(1,t);
y_u=kf_x(2,t);
z_u=kf_x(3,t);
xDot_u=kf_x(4,t);
yDot_u=kf_x(5,t);
zDot_u=kf_x(6,t);
cdeltat_u(t)=kf_x(7,t);
cdeltatDot_u(t)=kf_x(8,t);
d_cdeltatDot_u(t)=kf_x(9,t);
% Kalman state prediction (time update)
kf_P_x = eye(st_nmbr)*100; %TODO: use a real value.
kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
else
x_u=corr_kf_state(1,t-1);
y_u=corr_kf_state(2,t-1);
z_u=corr_kf_state(3,t-1);
xDot_u=corr_kf_state(4,t-1);
yDot_u=corr_kf_state(5,t-1);
zDot_u=corr_kf_state(6,t-1);
cdeltat_u(t)=corr_kf_state(7,t-1);
cdeltatDot_u(t)=corr_kf_state(8,t-1);
d_cdeltatDot_u(t)=corr_kf_state(9,t-1);
% Kalman state prediction (time update)
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
end
for chan=1:5 %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u(t);
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t)+cdeltatDot_u(t);
end
for chan=1:5 % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
kf_H(chan+sat_number, 9) = 0.0;
end
unobsv(t) = st_nmbr - rank(obsv(kf_F,kf_H));
% !!!! Limitaciones
% obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad
% no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los
% sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1].
% Kalman estimation (measurement update)
for chan=1:5 % Measurement matrix H assembling
kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u(t)+d_cdeltatDot_u(t-1)+d_cdeltatDot_u(t)*kf_dt)-rhoDot_pri(chan,t);
end
% DOUBLES DIFFERENCES
% kf_yerr = zeros(2*sat_number, 1);
% for (int32_t i = 1; i < sat_number; i++) % Measurement vector
% {
% kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1);
% kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1));
% kf_y(i+sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1;
% kf_yerr(i+sat_number)=kf_y(i+sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1));
% }
% kf_yerr.print("DOUBLES DIFFERENCES");
% Kalman filter update step
kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S)
kf_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain
kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation
kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix
if (t<3)
% kf_xerr(9,t)=15;
corr_kf_state(:,t)=kf_xpri(:,t)-kf_xerr(:,t)+kf_x(:,t); %updated state estimation
else
corr_kf_state(:,t)=corr_kf_state(:,t-1)-kf_xerr(:,t); %updated state estimation
end
%% ################## Geometric Transformation ######################################
x_u=corr_kf_state(1,t);
y_u=corr_kf_state(2,t);
z_u=corr_kf_state(3,t);
xDot_u=corr_kf_state(4,t);
yDot_u=corr_kf_state(5,t);
zDot_u=corr_kf_state(6,t);
cdeltat_u_g=corr_kf_state(7,t);
cdeltatDot_u_g=corr_kf_state(8,t);
d_cdeltatDot_u_g=corr_kf_state(9,t);
kf_H = zeros(2*sat_number,st_nmbr);
for chan=1:5 % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
kf_H(chan+sat_number, 9) = kf_dt;
end
% Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
kf_yerr_g=kf_H*kf_xerr;
% Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
for chan=1:5 % Measurement vector
pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered
rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t);
%convert rhoDot_pri to doppler shift!
% d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1));
sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t)+corr_kf_state(9,t-1)+corr_kf_state(9,t)*kf_dt)/Lambda_GPS_L1;
% carrier_phase_rads = 0;
carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t);
% carrier_freq_rate_hz_s = 0;
% code_phase_chips = 0;
end
% carrier_phase_rads = 0;
% carrier_freq_hz = 0;
% carrier_freq_rate_hz_s = 0;
% code_phase_chips = 0;
end

291
src/utils/matlab/vtl/kf_prototype_model.m Normal file
View File

@ -0,0 +1,291 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% vtl KF
load rocket_model_dynamics.mat
%%
sat_number=6; % limit by csv?
%% ################## Kalman filter initialization ######################################
state_number=9;
% covariances (static)
kf_P_x = eye(state_number); %TODO: use a real value.
kf_x = zeros(state_number, 1);
kf_R = zeros(2*sat_number, 2*sat_number);
kf_dt=0.05;
% kf_F = eye(state_number, state_number);
% kf_F(1, 4) = kf_dt;
% kf_F(2, 5) = kf_dt;
% kf_F(3, 6) = kf_dt;
% kf_F(7, 8) = kf_dt;
kf_F=[ 1 0 0 kf_dt 0 0 0 0 0
0 1 0 0 kf_dt 0 0 0 0
0 0 1 0 0 kf_dt 0 0 0
0 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 kf_dt kf_dt^2/2
0 0 0 0 0 0 0 1 kf_dt
0 0 0 0 0 0 0 0 1];
% kf_F=[ 1 0 0 kf_dt 0 0 0 0
% 0 1 0 0 kf_dt 0 0 0
% 0 0 1 0 0 kf_dt 0 0
% 0 0 0 1 0 0 0 0
% 0 0 0 0 1 0 0 0
% 0 0 0 0 0 1 0 0
% 0 0 0 0 0 0 1 kf_dt
% 0 0 0 0 0 0 0 1];
%
kf_H = zeros(2*sat_number,state_number);
kf_y = zeros(2*sat_number, 1);
kf_yerr = zeros(2*sat_number, 1);
% kf_xerr = zeros(8, 1);
kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
%% pre-allocate for speed
d = zeros(sat_number, 1);
rho_pri = zeros(sat_number, 1);
rhoDot_pri = zeros(sat_number, 1);
a_x = zeros(sat_number, 1);
a_y = zeros(sat_number, 1);
a_z = zeros(sat_number, 1);
c_pr_m=zeros(sat_number,length(navSolution.RX_time));
pr_m_filt=zeros(sat_number,length(navSolution.RX_time));
rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time));
sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time));
%% ################## Kalman Tracking ######################################
% receiver solution from rtklib_solver
index=0;
modelflag=0;
t_from_lunch=0;
for t=2:length(navSolution.RX_time)
index=index+1;
%% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS)
if (index < point_of_closure)
%% State error Covariance Matrix Q (PVT)
kf_Q = blkdiag(posx_var,posy_var,posz_var,velx_var,vely_var,velz_var,clk_bias_var,clk_drift_var,clk_d_drift_var);
%%
% Measurement error Covariance Matrix R assembling
kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var);
else
kf_Q = blkdiag(posx_var,posy_var,posz_var,velx_var,vely_var,velz_var,clk_bias_var,clk_drift_var,clk_d_drift_var);
for chan=1:sat_number
kf_R(chan,chan) = pr_var*sat_CN0_dBhz(chan,t)/200;
kf_R(chan+sat_number,chan+sat_number)= pr_dot_var*sat_CN0_dBhz(chan,t)/200;
end
end
if(~modelflag)
clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g
if (index < point_of_closure)
disp('one step')
kf_x(1,t) = navSolution.X(t);
kf_x(2,t) = navSolution.Y(t);
kf_x(3,t) = navSolution.Z(t);
kf_x(4,t) = navSolution.vX(t);
kf_x(5,t) = navSolution.vY(t);
kf_x(6,t) = navSolution.vZ(t);
kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1);
kf_x(9,t) = 1.0;
x_u=kf_x(1,t);
y_u=kf_x(2,t);
z_u=kf_x(3,t);
xDot_u=kf_x(4,t);
yDot_u=kf_x(5,t);
zDot_u=kf_x(6,t);
cdeltat_u(t)=kf_x(7,t);
cdeltatDot_u=kf_x(8,t);
% Kalman state prediction (time update)
kf_P_x = eye(state_number)*100; %TODO: use a real value.
kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
else
disp('feedback')
x_u=corr_kf_state(1,t-1);
y_u=corr_kf_state(2,t-1);
z_u=corr_kf_state(3,t-1);
xDot_u=corr_kf_state(4,t-1);
yDot_u=corr_kf_state(5,t-1);
zDot_u=corr_kf_state(6,t-1);
cdeltat_u(t)=corr_kf_state(7,t-1);
cdeltatDot_u=corr_kf_state(8,t-1);
% Kalman state prediction (time update)
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
end
else
disp('MODEL')
t_from_lunch=t_from_lunch+3;
if(t_from_lunch>length(rocket_model_dynamics.vX))
t_from_lunch=length(rocket_model_dynamics.vX);
end
kf_x(1,t) = rocket_model_dynamics.X(t_from_lunch);
kf_x(2,t) = rocket_model_dynamics.Y(t_from_lunch);
kf_x(3,t) = rocket_model_dynamics.Z(t_from_lunch);
kf_x(4,t) = rocket_model_dynamics.vX(t_from_lunch);
kf_x(5,t) = rocket_model_dynamics.vY(t_from_lunch);
kf_x(6,t) = rocket_model_dynamics.vZ(t_from_lunch);
kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1);
kf_x(9,t) = 1.0;
x_u=kf_x(1,t);
y_u=kf_x(2,t);
z_u=kf_x(3,t);
xDot_u=kf_x(4,t);
yDot_u=kf_x(5,t);
zDot_u=kf_x(6,t);
cdeltat_u(t)=kf_x(7,t);
cdeltatDot_u=kf_x(8,t);
% Kalman state prediction (time update)
kf_P_x = eye(state_number)*1; %TODO: use a real value.
kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction
kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
end
for chan=1:sat_number %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u(t);
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
end
for chan=1:sat_number % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
end
% unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H));
% !!!! Limitaciones
% obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad
% no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los
% sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1].
% Kalman stimation (measurement update)
for chan=1:sat_number % Measurement matrix H assembling
kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);
if (t<length(navSolution.RX_time)-point_of_closure)
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
else
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t);
end
end
% pause
% DOUBLES DIFFERENCES
% kf_yerr = zeros(2*sat_number, 1);
% for (int32_t i = 1; i < sat_number; i++) % Measurement vector
% {
% kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1);
% kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1));
% kf_y(i+sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1;
% kf_yerr(i+sat_number)=kf_y(i+sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1));
% }
% kf_yerr.print("DOUBLES DIFFERENCES");
% Kalman filter update step
kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S)
kf_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain
kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation
kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix
if (index<point_of_closure)
corr_kf_state(:,t)=kf_xpri(:,t)-kf_xerr(:,t)+kf_x(:,t); %updated state estimation
else
corr_kf_state(:,t)=corr_kf_state(:,t-1)-kf_xerr(:,t); %updated state estimation
end
if(modelflag)
corr_kf_state(:,t)=kf_xpri(:,t)-kf_xerr(:,t)+kf_x(:,t); %updated state estimation
end
if(sum(abs([xDot_u;yDot_u;zDot_u])>100)>1)
modelflag=1;
end
%% ################## Geometric Transformation ######################################
x_u=corr_kf_state(1,t);
y_u=corr_kf_state(2,t);
z_u=corr_kf_state(3,t);
xDot_u=corr_kf_state(4,t);
yDot_u=corr_kf_state(5,t);
zDot_u=corr_kf_state(6,t);
cdeltat_u_g=corr_kf_state(7,t);
cdeltatDot_u_g=corr_kf_state(8,t);
for chan=1:sat_number %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u_g;
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
end
kf_H = zeros(2*sat_number,state_number);
for chan=1:sat_number % Measurement matrix H assembling
% It has st_nmbr columns (st_nmbr states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
end
% Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
kf_yerr_g=kf_H*kf_xerr;
disp("kf_yerr_g:")
disp(kf_yerr_g(:,t))
disp('zDot_u:')
disp(zDot_u)
% Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
for chan=1:sat_number % Measurement vector
pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered
rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t);
%convert rhoDot_pri to doppler shift!
% d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1));
% d_dt_clk_drift=0;
sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1;
err_carrier_phase_rads_filt(chan,t) = trapz(kf_yerr_g(chan+sat_number,1:t)/Lambda_GPS_L1)*2*kf_dt;
carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t);
% carrier_freq_rate_hz_s = 0;
err_code_phase_chips(chan,t) = (kf_yerr_g(chan,t))/SPEED_OF_LIGHT_M_S*1023e3;
end
end

60
src/utils/matlab/vtl/pr_m_vtl_plot.m Normal file
View File

@ -0,0 +1,60 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% LOAD
Pseudorange_m_sim=zeros(length(refSatData.GPS.SIM_time),channels);
for i=1:length(refSatData.GPS.SIM_time)
Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(4);%PRN 28
Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(3);%PRN 20
Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(8);%PRN 17
Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(6);%PRN 12
Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(5);%PRN 9
Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(2);%PRN 5
Pseudorange_m_sim(i,7)=refSatData.GPS.series(i).pr_m(1);%PRN 4
Pseudorange_m_sim(i,8)=refSatData.GPS.series(i).pr_m(7);%PRN 2
end
%%
figure('Name','RX_PseudoRange_m');
for channel_cnt=1:channels
subplot(3,3,channel_cnt)
plot(linspace(0,tFinal,length(Pseudorange_m(channel_cnt,:)')), Pseudorange_m(channel_cnt,:)','s')
xlim([0,tFinal]);
% ylim([min(Pseudorange_m_sim(:,channel_cnt)),max(Pseudorange_m_sim(:,channel_cnt))]);
xlabel('')
ylabel('PR [m] (m)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench
% plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,channel_cnt)','.','DisplayName','SPIRENT reference')
% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL')
hold off;grid minor
end
%%
figure('Name','Rsim_PseudoRange_m');
for channel_cnt=1:channels
subplot(3,3,channel_cnt)
plot(linspace(0,tFinal,length(Pseudorange_m(channel_cnt,:)')), Pseudorange_m(channel_cnt,:)','s')
xlim([0,tFinal]);
ylim([min(Pseudorange_m_sim(:,channel_cnt)),max(Pseudorange_m_sim(:,channel_cnt))]);
xlabel('')
ylabel('PR [m] (m)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench
plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,channel_cnt)','.','DisplayName','SPIRENT reference')
% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL')
hold off;grid minor
end

406
src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m Normal file
View File

@ -0,0 +1,406 @@
% Read PVG raw dump
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
clc
close all
clearvars
% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file')
% addpath('./libs')
% end
%
% if ~exist('cat2geo.m', 'file')
% addpath('./libs/geoFunctions')
% end
SPEED_OF_LIGHT_M_S=299792458.0;
Lambda_GPS_L1=0.1902937;
GPS_L1_freq_hz=1575.42e6;
%%
samplingFreq=5000000;
channels=6;
TTFF_sec=41.48;
%% ============================ PARSER TO STRUCT ============================
plot_skyplot=1;
plot_reference=1;
load_observables=1;
advance_vtl_data_available=1;
load_vtl=1;
load_tfk_cmd=1;
navSolution = GnssSDR2struct('PVT_raw.mat');
refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv');
%%
if(load_observables)
load observables\observables_raw.mat
refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv');
rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05.
end
if(advance_vtl_data_available)
load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'...
,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz')
end
if(load_vtl)
vtlSolution = Vtl2struct('dump_vtl_file.csv');
end
if(load_tfk_cmd)
trkSolution=trk2struct('dump_trk_file.csv');
%% split by solution type
figure;sgtitle('real doppler')
for chan=0:4
eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.real==chan);'])
eval(['Dopp_real_CH' num2str(chan) '=trkSolution.dopp.real(indCH' num2str(chan) ',2);'])
eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_real_CH' num2str(chan) ')'])
end
figure;sgtitle('cmd doppler')
for chan=0:4
eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.cmd==chan);'])
eval(['Dopp_cmd_CH' num2str(chan) '=trkSolution.dopp.cmd(indCH' num2str(chan) ',2);'])
eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')'])
end
end
%% calculate LOS Rx-sat if advance_vtl_data_available=1
if(advance_vtl_data_available)
for chan=1:5
for t=1:length(navSolution.RX_time)
d(chan)=(sat_posX_m(chan,t)-navSolution.X(t))^2;
d(chan)=d(chan)+(sat_posY_m(chan,t)-navSolution.Y(t))^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-navSolution.Z(t))^2;
d(chan)=sqrt(d(chan));
pr_m(chan,t)=d(chan)+clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
a_x(chan,t)=-(sat_posX_m(chan,t)-navSolution.X(t))/d(chan);
a_y(chan,t)=-(sat_posY_m(chan,t)-navSolution.Y(t))/d(chan);
a_z(chan,t)=-(sat_posZ_m(chan,t)-navSolution.Z(t))/d(chan);
end
end
end
%%
% figure;plot([a_x(1,:);a_y(1,:);a_z(1,:)]');
% figure;plot([vtlSolution.LOSx vtlSolution.LOSy vtlSolution.LOSz])
%%
if(advance_vtl_data_available)
for chan=1:5
for t=1:length(navSolution.RX_time)
rhoDot_pri(chan,t)=(sat_velX(chan,t)-navSolution.vX(t))*a_x(chan,t)...
+(sat_velY(chan,t)-navSolution.vY(t))*a_y(chan,t)...
+(sat_velZ(chan,t)-navSolution.vZ(t))*a_z(chan,t);
kf_yerr(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
end
end
figure;plot(kf_yerr')
figure;plot(pr_m'-sat_prg_m');hold on
legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
end
%%
%% === Convert to UTM coordinate system =============================
% Scenario latitude is xx.xxxxxxx N37 49 9.98
% Scenario longitude is -xx.xxxxxxx W122 28 42.58
% Scenario elevation is 35 meters.
% lat=[37 49 9.98];
% long=[-122 -28 -42.58];
% lat_deg=dms2deg(lat);
% long_deg=dms2deg(long);
% h=35;
lat_deg=navSolution.latitude(1);
lon_deg=navSolution.longitude(1);
lat=degrees2dms(lat_deg);
lon=degrees2dms(lon_deg);
h=navSolution.height(1);
utmstruct = defaultm('utm');
utmstruct.zone = utmzone(lat_deg, lon_deg);
utmstruct = defaultm(utmstruct);
[utmstruct.latlim,utmstruct.lonlim] = utmzone(utmstruct.zone );
%Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
utmstruct.geoid = wgs84Ellipsoid;
% [X, Y] = projfwd(utmstruct,lat_deg,lon_deg);
% Z=h; % geographical to cartesian conversion
% for k=1:1:length(navSolution.X)
% [navSolution.E(k), ...
% navSolution.N(k), ...
% navSolution.U(k)]=cart2utm(navSolution.X(k), navSolution.Y(k), navSolution.Z(k), utmZone);
% end
%% ====== FILTERING =======================================================
moving_avg_factor= 500;
LAT_FILT = movmean(navSolution.latitude,moving_avg_factor);
LON_FILT = movmean(navSolution.longitude,moving_avg_factor);
HEIGH_FILT = movmean(navSolution.height,moving_avg_factor);
X_FILT = movmean(navSolution.X,moving_avg_factor);
Y_FILT = movmean(navSolution.Y,moving_avg_factor);
Z_FILT = movmean(navSolution.Z,moving_avg_factor);
vX_FILT = movmean(navSolution.vX,moving_avg_factor);
vY_FILT = movmean(navSolution.vY,moving_avg_factor);
vZ_FILT = movmean(navSolution.vZ,moving_avg_factor);
%%
general_raw_plot
%%
if(load_vtl)
%---VTL VELOCITY: GNSS SDR plot --------------------------------------
VTL_VEL=figure('Name','velocities and heigh');
subplot(2,2,1);
plot(vtlSolution.rtklibpvt.vX,'.');
hold on;grid on
plot(vtlSolution.kfpvt.vX,'.');
plot(vtlSolution.kferr.vX,'.');
ylabel('vX (m/s)')
xlabel('time U.A.')
ylim([-5 5])
title('Subplot 1: vX ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
subplot(2,2,2);
plot(vtlSolution.rtklibpvt.vY,'.');
hold on;grid on
plot(vtlSolution.kfpvt.vY,'.');
plot(vtlSolution.kferr.vY,'.');
ylabel('vY (m/s)')
xlabel('time U.A.')
ylim([-5 5])
title('Subplot 1: vY ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
subplot(2,2,3);
plot(vtlSolution.rtklibpvt.vZ,'.');
hold on;grid on
plot(vtlSolution.kfpvt.vZ,'.');
plot(vtlSolution.kferr.vZ,'.');
ylabel('vZ (m/s)')
xlabel('time U.A.')
ylim([-5 5])
title('Subplot 1: vZ ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
subplot(2,2,4);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.');
ylabel('HEIGH (m)')
xlabel('time from First FIX in (seconds)')
title('Subplot 4: HEIGH')
legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
sgtitle('velocities and heigh')
% --- VTL UTM centered POSITION: GNSS SDR plot --------------------------------------
VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION');
subplot(2,2,1);
plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.');
hold on;grid on
plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.');
plot(vtlSolution.kferr.X,'.');
ylabel('X (m)')
xlabel('time U.A.')
ylim([-200 800])
title('Subplot 1: X ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
subplot(2,2,2);
plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.');
hold on;grid on
plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.');
plot(vtlSolution.kferr.Y,'.');
ylabel('Y (m)')
xlabel('time U.A.')
ylim([-200 50])
title('Subplot 1: Y ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
subplot(2,2,3);
plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.');
hold on;grid on
plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.');
plot(vtlSolution.kferr.Z,'.');
ylabel('Z (m)')
xlabel('time U.A.')
ylim([-350 50])
title('Subplot 1: Z ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION')
%%
% --- 'VTL errPV correction --------------------------------------
VTL_errPV=figure('Name','VTL errPV correction');
subplot(2,3,1);
plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.');
hold on;grid on
plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.');
plot(vtlSolution.kferr.X,'.');
ylabel('X (m)')
xlabel('time U.A.')
ylim([-200 800])
title('Subplot 1: X ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
subplot(2,3,2);
plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.');
hold on;grid on
plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.');
plot(vtlSolution.kferr.Y,'.');
ylabel('Y (m)')
xlabel('time U.A.')
ylim([-200 50])
title('Subplot 1: Y ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
subplot(2,3,3);
plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.');
hold on;grid on
plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.');
plot(vtlSolution.kferr.Z,'.');
ylabel('Z (m)')
xlabel('time U.A.')
ylim([-350 50])
title('Subplot 1: Z ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
subplot(2,3,4);
plot(vtlSolution.rtklibpvt.vX,'.');
hold on;grid on
plot(vtlSolution.kfpvt.vX,'.');
plot(vtlSolution.kferr.vX,'.');
ylabel('vX (m/s)')
xlabel('time U.A.')
ylim([-5 5])
title('Subplot 1: vX ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
subplot(2,3,5);
plot(vtlSolution.rtklibpvt.vY,'.');
hold on;grid on
plot(vtlSolution.kfpvt.vY,'.');
plot(vtlSolution.kferr.vY,'.');
ylabel('vY (m/s)')
xlabel('time U.A.')
ylim([-5 5])
title('Subplot 1: vY ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
subplot(2,3,6);
plot(vtlSolution.rtklibpvt.vZ,'.');
hold on;grid on
plot(vtlSolution.kfpvt.vZ,'.');
plot(vtlSolution.kferr.vZ,'.');
ylabel('vZ (m/s)')
xlabel('time U.A.')
ylim([-5 5])
title('Subplot 1: vZ ')
legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
sgtitle('VTL errPV correction')
end
%% ============================================== ==============================================
time_reference_spirent_obs=129780;%s
if(load_observables)
% figure;plot(Carrier_phase_cycles','.')
% figure;plot(Pseudorange_m','.')
%%%
Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),6);
for i=1:length(refSatData.GPS.SIM_time)
Carrier_Doppler_hz_sim(i,1)=refSatData.GPS.series(i).doppler_shift(4);%PRN 28
Carrier_Doppler_hz_sim(i,2)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4
Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(6);%PRN 17
Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(7);%PRN 15
Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(8);%PRN 27
Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(9);%PRN 9
end
Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','.')
xlim([0,140]);
xlabel('')
ylabel('Doppler (Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.')
legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
hold off
grid on
Rx_Dopp_one=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','.')
xlim([0,140]);
ylim([-2340,-2220]);
xlabel('')
ylabel('Doppler (Hz)')
xlabel('time from simulation init (seconds)')
grid on
hold on
legend('PRN 28 GNSS-SDR','Location','eastoutside')
plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference')
hold off
grid on
end
%%
if(load_vtl)
dopp_filtered_plotting
end
%%
labels = ["G1", "G22", "E17", "E11"]; %inizialitation neccesary
if(plot_skyplot)
for i=1:length(refSatData.GPS.SIM_time)
for j=1:length(refSatData.GPS.series(i).Sat_PRN)
labels{i,j} = ['G' num2str(refSatData.GPS.series(i).Sat_PRN(j))];
if(refSatData.GPS.series(i).azimuth(j)<0)
az(i,j) = rad2deg(refSatData.GPS.series(i).azimuth(j))+360;
else
az(i,j) = rad2deg(refSatData.GPS.series(i).azimuth(j));
end
el(i,j) = rad2deg(refSatData.GPS.series(i).elevation(j));
end
groups(i,:) = categorical([0 0 1 1], [0 1], ["GPS", "Galileo"]);
end
% skyplot(az(i,:), el(i,:), labels, "GroupData", groups)
figure;
%skyplot(az(500,:), el(500,:), labels(500,:))
sp = skyplot(az(1,:),el(1,:),labels(1,:));
for idx = 1:length(az)
set(sp,AzimuthData=az(1:idx,:),ElevationData=el(1:idx,:));
drawnow limitrate
end
legend('GPS')
end

68
src/utils/matlab/vtl/trk2struct.m Normal file
View File

@ -0,0 +1,68 @@
%% Import data from text file
% Script for importing data from the following text file:
%
% filename: D:\virtualBOX_VM\ubuntu20\ubuntu20\shareFolder\myWork\results\spirent_usrp_airing\dump_vtl_file.csv
%
% %
% -------------------------------------------------------------------------
% USE EXAMPLE: vtlSolution = Vtl2struct('dump_vtl_file.csv')
% -------------------------------------------------------------------------
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [trkSolution] = trk2struct(path_to_trk_csv)
%% Set up the Import Options and import the data
opts = delimitedTextImportOptions("NumVariables", 5);
% Specify range and delimiter
opts.DataLines = [1, Inf];
opts.Delimiter = ",";
% Specify column names and types
opts.VariableNames = ["doppler_corr", "VarName2", "VarName3", "VarName4","VarName5","VarName5"];
opts.VariableTypes = ["char", "double", "double", "double", "double", "double"];
% Specify file level properties
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";
% Specify variable properties
opts = setvaropts(opts, "doppler_corr", "WhitespaceRule", "preserve");
opts = setvaropts(opts, "doppler_corr", "EmptyFieldRule", "auto");
% Import the data
dumptrackingfile = readtable(path_to_trk_csv, opts);
%% Convert to output type
dumptrackingfile = table2cell(dumptrackingfile);
numIdx = cellfun(@(x) ~isnan(str2double(x)), dumptrackingfile);
dumptrackingfile(numIdx) = cellfun(@(x) {str2double(x)}, dumptrackingfile(numIdx));
%% Clear temporary variables
clear numIdx opts
trkSolution.dopp=[];
%% split by solution type
[indDopp,~]= find(strcmp(dumptrackingfile, 'doppler_corr'));
trk_dopp=dumptrackingfile(indDopp,:);trk_dopp(:,1)=[];
trk_dopp=cell2mat(trk_dopp);
for i=1:length(trk_dopp)
trkSolution.dopp.real=[trk_dopp(:,1) trk_dopp(:,3)];
trkSolution.dopp.cmd=[trk_dopp(:,1) trk_dopp(:,2)];
trkSolution.doppShift.real=[trk_dopp(:,1) trk_dopp(:,4)];
trkSolution.tao.real=[trk_dopp(:,1) trk_dopp(:,5)];
end
end

204
src/utils/matlab/vtl/vtl_general_plot.m Normal file
View File

@ -0,0 +1,204 @@
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%%
% vtl_general_plot.m
%%
%
% if(load_observables)
% for i=1:length(refSatData.GPS.SIM_time)
% Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(9);%PRN 30
% Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(3);%PRN 29
% Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(5);%PRN 24
% Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(2);%PRN 12
% Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(8);%PRN 10
% Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(6);%PRN 5
% end
% end
% -------------------------------------
% if(load_observables)
% Rx_pseudo_all=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m','s')
% xlim([0,140]);
% xlabel('')
% ylabel('Pseudorange (m)')
% xlabel('time from simulation init (seconds)')
% grid on
% hold on
% plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim','.')
% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, pr_m_filt,'o')
% legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
% hold off
% grid on
%
% Rx_pseudo_one=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m(1,:)','s')
% xlim([0,140]);
% xlabel('')
% ylabel('Pseudorange (m)')
% xlabel('time from simulation init (seconds)')
% grid on
% hold on
% legend('PRN 28 GNSS-SDR','Location','eastoutside')
% plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,1)','.','DisplayName','SPIRENT reference')
% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, pr_m_filt(1,:),'o','DisplayName','filtered VTL')
% hold off
% grid on
% end
%% ---VTL VELOCITY: GNSS SDR plot --------------------------------------
VTL_VEL=figure('Name','velocities');
subplot(2,2,1);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(4,:),'.');
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX...
,'.','DisplayName','SPIRENT reference')
% plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(4,:),'.');
ylabel('vX (m/s)')
xlabel('time U.A.')
% ylim([-5 5])
title('Subplot 1: vX ')
legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside')
subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(5,:),'.');
% plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(5,:),'.');
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY...
,'.','DisplayName','SPIRENT reference')
ylabel('vY (m/s)')
xlabel('time U.A.')
% ylim([-5 5])
title('Subplot 1: vY ')
legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside')
subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(6,:),'.');
% plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(6,:),'.');
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ...
,'.','DisplayName','SPIRENT reference')
ylabel('vZ (m/s)')
xlabel('time U.A.')
% ylim([-5 5])
title('Subplot 1: vZ ')
legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside')
sgtitle('velocities')
%% --- VTL UTM centered POSITION: GNSS SDR plot --------------------------------------
VTL_POS=figure('Name','VTL UTM COORD ');
subplot(2,2,1);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X,'.');
hold on;grid on
plot(navSolution.RX_time(3:end)-navSolution.RX_time(1),corr_kf_state(1,3:end))
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.X,'.')
ylabel('X (m)')
xlabel('time U.A.')
% ylim([-200 800])
title('Subplot 1: X ')
legend ('raw navSolution','raw kf state','kferr','Location','eastoutside')
subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Y,'.');
hold on;grid on
plot(navSolution.RX_time(3:end)-navSolution.RX_time(1),corr_kf_state(2,3:end))
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Y,'.')
ylabel('Y (m)')
xlabel('time U.A.')
% ylim([-200 50])
title('Subplot 1: Y ')
legend ('raw navSolution','raw kf state','kferr','Location','eastoutside')
subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.Z,'.');
hold on;grid on
plot(navSolution.RX_time(3:end)-navSolution.RX_time(1),corr_kf_state(3,3:end))
plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.Z,'.')
ylabel('Z (m)')
xlabel('time U.A.')
% ylim([-350 50])
title('Subplot 1: Z ')
legend ('raw navSolution','raw kf state','SPIRENT ref','Location','eastoutside')
sgtitle('VTL UTM COORD')
%% STATE PLOT
VTL_STATE=figure('Name','VTL STATE');
subplot(2,3,1);
plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.X-navSolution.X(1);...
navSolution.Y-navSolution.Y(1) ;navSolution.Z-navSolution.Z(1)],...
'b.','DisplayName','RTKLIB solution');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(1:3,:)'-corr_kf_state(1:3,3)',...
'k.','DisplayName','filt VTL');
plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.X-refSolution.X(spirent_index_TTFF)...
refSolution.Y-refSolution.Y(spirent_index_TTFF) refSolution.Z-refSolution.Z(spirent_index_TTFF)],...
'r.','DisplayName','SPIRENT reference');
legend('Location','eastoutside');
% ylim([-200,200])
xlim([0,tFinal])
ylabel('X Y Z (m)')
xlabel('time [s]')
title('Subplot 1: POSITION [m]')
subplot(2,3,2);
plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.vX;...
navSolution.vY; navSolution.vZ],...
'b.','DisplayName','RTKLIB solution');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(4:6,:)',...
'k.','DisplayName','filt VTL');
plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.vX...
refSolution.vY refSolution.vZ],...
'r.','DisplayName','SPIRENT reference');
xlim([0,tFinal])
ylabel('vX vY vZ (m/s)')
xlabel('time [s]')
title('Subplot 1: VELOCITIES [m/s]')
subplot(2,3,3);
plot(navSolution.RX_time-navSolution.RX_time(1),clk_bias_s*SPEED_OF_LIGHT_M_S,...
'b.','DisplayName','RTKLIB solution');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(7,:),...
'k.','DisplayName','filt VTL');
% ylim([3633390, 3634580])
xlim([0,tFinal])
ylabel('clk bias (m)')
xlabel('time [s]')
title('Subplot 1: clk bias [m]')
subplot(2,3,4);
plot(navSolution.RX_time-navSolution.RX_time(1),clk_drift*SPEED_OF_LIGHT_M_S,...
'b.','DisplayName','RTKLIB solution');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(8,:),...
'k.','DisplayName','filt VTL');
xlim([0,tFinal])
ylabel('clk drift (m/s)')
xlabel('time [s]')
title('Subplot 1: clk drift [m/s]')
subplot(2,3,5);
plot(navSolution.RX_time(1:end-1)-navSolution.RX_time(1),diff(clk_drift)/kf_dt*SPEED_OF_LIGHT_M_S,...
'b.','DisplayName','RTKLIB solution');
hold on;grid on
plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(9,:),...
'k.','DisplayName','filt VTL');
xlim([0,tFinal])
ylabel('clk drift (m/s)')
xlabel('time [s]')
title('Subplot 1: clk drift [m/s]')
sgtitle('VTL STATE')

166
src/utils/matlab/vtl/vtl_prototype_clk_d_drift.m Normal file
View File

@ -0,0 +1,166 @@
% VTL prototype
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
clc
close all
clearvars
% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file')
% addpath('./libs')
% end
%
% if ~exist('cat2geo.m', 'file')
% addpath('./libs/geoFunctions')
% end
SPEED_OF_LIGHT_M_S=299792458.0;
Lambda_GPS_L1=0.1902937;
GPS_L1_freq_hz=1575.42e6;
point_of_closure=4060;%4060;
%% ==================== VARIANCES =============================
pos_var=100;%m^2
vel_var=10;%m^2/s^2
clk_bias_var=100;%m^2
clk_drift_var=100;%m^2/s^2
clk_d_drift_var=10;
pr_var=20;%m^2
pr_dot_var=2;%m^2/s^2
% CARLES PAPER LTE GNSS VTL
% pos_var=2;%m^2
% vel_var=0.2;%m^2/s^2
% clk_bias_var=1e-7;%m^2
% clk_drift_var=1e-4;%m^2/s^2
% pr_var=20;%m^2
% pr_dot_var=3;%m^2/s^2
%% ============================================================
samplingFreq=5000000;
channels=6;
TTFF_sec=41.48;
spirent_index_TTFF=416;
plot_skyplot=0;
plot_reference=1;
load_observables=1;
load_tfk_cmd=1;
%% ============================ PARSER TO STRUCT ============================
navSolution = GnssSDR2struct('PVT_raw.mat');
refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv');
%
load observables\observables_raw.mat
% refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv');
rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05.
load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'...
,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz','user_clk_offset')
if(load_observables)
load observables\observables_raw.mat
refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv');
end
%%
if(load_tfk_cmd)
trkSolution=trk2struct('dump_trk_file.csv');
% split by solution type
figure;sgtitle('real doppler')
for chan=0:4
eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.real==chan);'])
eval(['Dopp_real_CH' num2str(chan) '=trkSolution.dopp.real(indCH' num2str(chan) ',2);'])
eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_real_CH' num2str(chan) ')'])
end
figure;sgtitle('cmd doppler')
for chan=0:4
eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.cmd==chan);'])
eval(['Dopp_cmd_CH' num2str(chan) '=trkSolution.dopp.cmd(indCH' num2str(chan) ',2);'])
eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')'])
end
figure;sgtitle('real doppler shift')
for chan=0:4
eval(['[indCH' num2str(chan) ',~]= find(trkSolution.doppShift.real==chan);'])
eval(['doppShift_CH' num2str(chan) '=trkSolution.doppShift.real(indCH' num2str(chan) ',2);'])
eval(['subplot(2,3,' num2str(chan+1) ');plot(doppShift_CH' num2str(chan) ')'])
grid on
end
figure;sgtitle('real code phase')
for chan=0:4
eval(['[indCH' num2str(chan) ',~]= find(trkSolution.tao.real==chan);'])
eval(['tao_real_CH' num2str(chan) '=trkSolution.tao.real(indCH' num2str(chan) ',2);'])
eval(['subplot(2,3,' num2str(chan+1) ');plot(tao_real_CH' num2str(chan) ')'])
end
end
%%
% vtlSolution = Vtl2struct('dump_vtl_file.csv');
%%
kf_prototype4acc
%% ====== FILTERING =======================================================
% moving_avg_factor= 500;
% LAT_FILT = movmean(navSolution.latitude,moving_avg_factor);
% LON_FILT = movmean(navSolution.longitude,moving_avg_factor);
% HEIGH_FILT = movmean(navSolution.height,moving_avg_factor);
%
% X_FILT = movmean(navSolution.X,moving_avg_factor);
% Y_FILT = movmean(navSolution.Y,moving_avg_factor);
% Z_FILT = movmean(navSolution.Z,moving_avg_factor);
%
% vX_FILT = movmean(navSolution.vX,moving_avg_factor);
% vY_FILT = movmean(navSolution.vY,moving_avg_factor);
% vZ_FILT = movmean(navSolution.vZ,moving_avg_factor);
%
%%
%general_raw_plot
vtl_general_plot
%% ============================================== ==============================================
%%
figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(1:5,:)');title('c pr m-error');xlabel('t U.A');ylabel('pr m [m]');grid minor
legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(6:10,:)');title('c pr m DOT-error');xlabel('t U.A');ylabel('pr m dot [m/s]');grid minor
legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
%%
figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(9,:)');title('kf 9 xerr');xlabel('t U.A');ylabel('kf 9 xerr');grid minor
% %%
%
% figure;sgtitle(' error in phase')
% for chan=0:4
% eval(['subplot(2,3,' num2str(chan+1) ');plot(err_carrier_phase_rads_filt(' num2str(chan+1) ',:))'])
% hold on; grid minor;
% end
%%
figure;sgtitle(' error in code phase')
for chan=0:4
eval(['subplot(2,3,' num2str(chan+1) ');plot(err_code_phase_chips(' num2str(chan+1) ',:))'])
hold on; grid minor;
end
%%
% figure;sgtitle('real vs cmd phase')
% for chan=0:4
% eval(['subplot(2,3,' num2str(chan+1) ');plot(Phase_real_CH' num2str(chan) ')'])
% hold on; grid minor;
% eval(['subplot(2,3,' num2str(chan+1) ');plot(carrier_phase_rads_filt(' num2str(chan+1) ',1:3425)+Phase_real_CH' num2str(chan) '(1)' ')'])
% legend('real','cmd')
% end
% %%
% figure;sgtitle(' error in cmd to actual phase')
% for chan=0:4
% eval(['subplot(2,3,' num2str(chan+1) ');plot(Phase_real_CH' num2str(chan) '-transpose(carrier_phase_rads_filt(' num2str(chan+1) ',1:3425))+Phase_real_CH' num2str(chan) '(1)' ')'])
% grid minor;
% end

75
src/utils/python/analysis.py Normal file
View File

@ -0,0 +1,75 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
"""
import numpy as np
import h5py
import matplotlib.pyplot as plt
#%%
f = h5py.File('PVT_raw.mat','r')
#data = f.get('rateQualityOutTrim/date')
#data = np.array(data)
print(f.keys()) # gives the name of the variables stored
RX_time = f.get('RX_time')
vel_x = f.get('vel_x')
vel_y = f.get('vel_y')
vel_z = f.get('vel_z')
pos_x = f.get('pos_x')
pos_y = f.get('pos_y')
pos_z = f.get('pos_z')
#kf_state1=f.get('vtl_kf_state_1')
#%%
for keys in f:
keys=f.get(keys)
#%%
#plt.plot(kf_state1[:,0]-kf_state1[0,0],'o',label='kf_state1')
#plt.show()
#%%
plt.scatter(RX_time,pos_x)
plt.show()
plt.scatter(RX_time,pos_y-pos_y[0])
plt.show()
plt.scatter(RX_time,pos_z-pos_z[0])
plt.show()
#%%
plt.scatter(RX_time,pos_x-pos_x[0])
#plt.ylim([4863484, 4863591])
plt.ylim([-20,110])
plt.ylabel('X [m]')
plt.show()
plt.scatter(RX_time,pos_y-pos_y[0])
plt.ylabel('Y [m]')
plt.ylim([-85, 5])
plt.show()
plt.scatter(RX_time,pos_z-pos_z[0])
plt.ylabel('Z [m]')
plt.ylim([-110,25])
plt.show()
#%%
plt.scatter(RX_time,vel_x)
plt.show()
plt.scatter(RX_time,vel_y-vel_y[0])
plt.show()
plt.scatter(RX_time,vel_z-vel_z[0])
plt.show()

104
src/utils/python/vtl_analysis.py Normal file
View File

@ -0,0 +1,104 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
% Miguel Angel Gomez, 2024. gomezlma(at)inta.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2024 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
"""
#%%
import numpy as np
import matplotlib.pyplot as plt
#%%
with open('dump_vtl_file.csv') as f:
rawdatos=np.genfromtxt(f,dtype=str,delimiter=",")
#%%
idx_kf_err = np.where(rawdatos[:,0]=='kf_xerr') [0]# se indexa
idx_kf_state = np.where(rawdatos[:,0]=='kf_state')[0]
idx_rtklib = np.where(rawdatos[:,0]=='rtklib_state')[0]
#%%
kf_err=rawdatos[idx_kf_err,1:].astype(float)
kf_estate=rawdatos[idx_kf_state,1:].astype(float)
rtklib=rawdatos[idx_rtklib,1:].astype(float)
#%%
plt.close()
plt.plot(kf_err[:,0],'o',label='kf_err')
plt.plot(kf_estate[:,0]-kf_estate[0,0],'o',label='kf_estate')
plt.plot(rtklib[:,0]-rtklib[0,0],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.ylabel('X [m]')
plt.legend()
plt.show()
plt.plot(kf_err[:,1],'o',label='kf_err')
plt.plot(kf_estate[:,1]-kf_estate[0,1],'o',label='kf_estate')
plt.plot(rtklib[:,1]-rtklib[0,1],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.ylabel('Y [m]')
plt.legend()
plt.show()
plt.plot(kf_err[:,2],'o',label='kf_err')
plt.plot(kf_estate[:,2]-kf_estate[0,2],'o',label='kf_estate')
plt.plot(rtklib[:,2]-rtklib[0,2],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.ylabel('Z [m]')
plt.legend()
plt.show()
#%%
plt.close()
plt.plot(kf_estate[:,0]-kf_estate[0,0]+kf_err[:,0],'o',label='kf_corr_estate')
plt.plot(rtklib[:,0]-rtklib[0,0],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.ylabel('X [m]')
plt.title('X corrected')
plt.legend()
plt.show()
plt.plot(kf_estate[:,1]-kf_estate[0,1]+kf_err[:,1],'o',label='kf_corr_estate')
plt.plot(rtklib[:,1]-rtklib[0,1],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.ylabel('Y [m]')
plt.title('Y corrected')
plt.legend()
plt.show()
plt.plot(kf_estate[:,2]-kf_estate[0,2]+kf_err[:,2],'o',label='kf_corr_estate')
plt.plot(rtklib[:,2]-rtklib[0,2],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.ylabel('Z [m]')
plt.title('Z corrected')
plt.legend()
plt.show()
#%%
plt.close()
plt.plot(kf_err[:,3],'o',label='kf_err')
plt.plot(kf_estate[:,3]-kf_estate[0,3],'o',label='kf_estate')
plt.plot(rtklib[:,3]-rtklib[0,3],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.legend()
plt.show()
plt.plot(kf_err[:,4],'o',label='kf_err')
plt.plot(kf_estate[:,4]-kf_estate[0,4],'o',label='kf_estate')
plt.plot(rtklib[:,4]-rtklib[0,4],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.legend()
plt.show()
plt.plot(kf_err[:,5],'o',label='kf_err')
plt.plot(kf_estate[:,5]-kf_estate[0,5],'o',label='kf_estate')
plt.plot(rtklib[:,5]-rtklib[0,5],'o',label='rtklib')
plt.xlabel('time U.A.')
plt.legend()
plt.show()