diff --git a/src/utils/matlab/vtl/GnssSDR2struct.m b/src/utils/matlab/vtl/GnssSDR2struct.m new file mode 100644 index 000000000..170b37f24 --- /dev/null +++ b/src/utils/matlab/vtl/GnssSDR2struct.m @@ -0,0 +1,87 @@ +% 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 diff --git a/src/utils/matlab/vtl/SpirentMotion2struct.m b/src/utils/matlab/vtl/SpirentMotion2struct.m new file mode 100644 index 000000000..b5789330b --- /dev/null +++ b/src/utils/matlab/vtl/SpirentMotion2struct.m @@ -0,0 +1,112 @@ +% 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') +% ------------------------------------------------------------------------- +% +% 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 [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 diff --git a/src/utils/matlab/vtl/SpirentSatData2struct.m b/src/utils/matlab/vtl/SpirentSatData2struct.m new file mode 100644 index 000000000..37e930ebd --- /dev/null +++ b/src/utils/matlab/vtl/SpirentSatData2struct.m @@ -0,0 +1,188 @@ +% 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') +% ------------------------------------------------------------------------- +% +% 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 [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')); + +GAL=satdataV1A1SPFLD1(indGALILEO,:);GAL(:,3)=[]; +GPS=satdataV1A1SPFLD1(indGPS,:); GPS(:,3)=[]; +GAL=cell2mat(GAL); +GPS=cell2mat(GPS); + %% SPIRENT SAT SOLUTION + + refSatData.GALILEO.SIM_time=unique(GAL(:,1),'first'); + refSatData.GPS.SIM_time=unique(GPS(:,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 + %% ------------------------------------- + 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 + + % Clear temporary variables + clear satdataV1A1SPFLD05 GPS GAL +end diff --git a/src/utils/matlab/vtl/Vtl2struct.m b/src/utils/matlab/vtl/Vtl2struct.m new file mode 100644 index 000000000..8c3234e1a --- /dev/null +++ b/src/utils/matlab/vtl/Vtl2struct.m @@ -0,0 +1,93 @@ +%% 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') +% ------------------------------------------------------------------------- + +% Auto-generated by MATLAB on 24-Nov-2022 17:34:27 +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_LOS,~]= find(strcmp(dumpvtlfile, 'sat_first_LOS')); + +kfpvt=dumpvtlfile(indKF,:);kfpvt(:,1)=[]; +rtklibpvt=dumpvtlfile(indRTKlib,:); rtklibpvt(:,1)=[]; +kferr=dumpvtlfile(indkf_err,:); kferr(:,1)=[]; +LOS=dumpvtlfile(ind_LOS,:); LOS(:,1)=[]; + +kfpvt=cell2mat(kfpvt); +rtklibpvt=cell2mat(rtklibpvt); +kferr=cell2mat(kferr); +LOS=cell2mat(LOS); + +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.LOSx=LOS(:,1); +vtlSolution.LOSy=LOS(:,2); +vtlSolution.LOSz=LOS(:,3); +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/general_raw_plot.m b/src/utils/matlab/vtl/general_raw_plot.m new file mode 100644 index 000000000..a9dc27570 --- /dev/null +++ b/src/utils/matlab/vtl/general_raw_plot.m @@ -0,0 +1,198 @@ +%general_raw_plot.m + +%% ====== 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); + +% 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 \ No newline at end of file diff --git a/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m b/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m new file mode 100644 index 000000000..6198d51f9 --- /dev/null +++ b/src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m @@ -0,0 +1,466 @@ +% Read PVG raw dump +% ------------------------------------------------------------------------- +% +% 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 +% +% ------------------------------------------------------------------------- +% +%% +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)) diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m new file mode 100644 index 000000000..8ddf9ff63 --- /dev/null +++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m @@ -0,0 +1,344 @@ +% Read PVG raw dump +% ------------------------------------------------------------------------- +% +% 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 +% +% ------------------------------------------------------------------------- +% +%% +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; +%% +samplingFreq=5000000; +channels=6; +TTFF_sec=41.48; + +%% ============================ PARSER TO STRUCT ============================ +plot_skyplot=0; +plot_reference=1; +load_observables=1; +advance_vtl_data_available=1; +load_vtl=1; + +navSolution = GnssSDR2struct('PVT_raw.mat'); +refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv'); +%% +if(load_observables) + load observables\observable_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','sat_dopp_hz') +end + +if(load_vtl) +vtlSolution = Vtl2struct('dump_vtl_file.csv'); +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 +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') +%% === 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 + +%% +general_raw_plot +%% + +%---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','raw 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','raw 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','raw 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','raw 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','raw 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','raw kf','kferr','Location','east') + +sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') +%% +close all +% --- '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,'.'); +plot(vtlSolution.kfpvt.X-vtlSolution.rtklibpvt.X(1)+vtlSolution.kferr.X,'.'); +ylabel('X (m)') +xlabel('time U.A.') +ylim([-200 800]) +title('Subplot 1: X ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','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,'.'); +plot(vtlSolution.kfpvt.Y-vtlSolution.rtklibpvt.Y(1)+vtlSolution.kferr.Y,'.'); +ylabel('Y (m)') +xlabel('time U.A.') +ylim([-200 50]) +title('Subplot 1: Y ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','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,'.'); +plot(vtlSolution.kfpvt.Z-vtlSolution.rtklibpvt.Z(1)+vtlSolution.kferr.Z,'.'); +ylabel('Z (m)') +xlabel('time U.A.') +ylim([-350 50]) +title('Subplot 1: Z ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,4); +plot(vtlSolution.rtklibpvt.vX,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vX,'.'); +plot(vtlSolution.kferr.vX,'.'); +plot(vtlSolution.kfpvt.vX+vtlSolution.kferr.vX,'.'); +ylabel('vX (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vX ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,5); +plot(vtlSolution.rtklibpvt.vY,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vY,'.'); +plot(vtlSolution.kferr.vY,'.'); +plot(vtlSolution.kfpvt.vY+vtlSolution.kferr.vY,'.'); +ylabel('vY (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vY ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + +subplot(2,3,6); +plot(vtlSolution.rtklibpvt.vZ,'.'); +hold on;grid on +plot(vtlSolution.kfpvt.vZ,'.'); +plot(vtlSolution.kferr.vZ,'.'); +plot(vtlSolution.kfpvt.vZ+vtlSolution.kferr.vZ,'.'); +ylabel('vZ (m/s)') +xlabel('time U.A.') +ylim([-5 5]) +title('Subplot 1: vZ ') +legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside') + + +sgtitle('VTL errPV correction') +%% ============================================== ============================================== +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 \ No newline at end of file