mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 22:34:58 +00:00
ADD: some matlab utils
This commit is contained in:
M.A.Gomez
parent
c2e161efc2
commit
ee6b012ff5
87
src/utils/matlab/vtl/GnssSDR2struct.m
Normal file
87
src/utils/matlab/vtl/GnssSDR2struct.m
Normal 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
112
src/utils/matlab/vtl/SpirentMotion2struct.m
Normal 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
188
src/utils/matlab/vtl/SpirentSatData2struct.m
Normal 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
93
src/utils/matlab/vtl/Vtl2struct.m
Normal 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
198
src/utils/matlab/vtl/general_raw_plot.m
Normal 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
466
src/utils/matlab/vtl/gps_l1_ca_pvt_raw_from_MAT.m
Normal 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
344
src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m
Normal 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
|
||||||
Loading…
Reference in New Issue
Block a user