mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 14:25:00 +00:00
Code Issues Releases Wiki Activity

ADD: some matlab utils

Browse Source
This commit is contained in:
M.A.Gomez 2022-12-04 23:49:39 +00:00
parent c2e161efc2
commit ee6b012ff5
7 changed files with 1488 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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

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

@ -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

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

@ -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

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

@ -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

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

@ -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

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

@ -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))

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

@ -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