mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-18 21:23:02 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-03-30 12:25:24 +02:00
commit 3f93c91111
29 changed files with 2311 additions and 2312 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

166
src/utils/matlab/dll_pll_veml_plot_sample.m
View File

@ -1,83 +1,83 @@
% Reads GNSS-SDR Tracking dump binary file using the provided % Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables % function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es % Javier Arribas, 2011. jarribas(at)cttc.es
% Antonio Ramos, 2018. antonio.ramos(at)cttc.es % Antonio Ramos, 2018. antonio.ramos(at)cttc.es
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) % Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
% %
% GNSS-SDR is a software defined Global Navigation % GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver % Satellite Systems receiver
% %
% This file is part of GNSS-SDR. % This file is part of GNSS-SDR.
% %
% GNSS-SDR is free software: you can redistribute it and/or modify % GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by % it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or % the Free Software Foundation, either version 3 of the License, or
% at your option) any later version. % at your option) any later version.
% %
% GNSS-SDR is distributed in the hope that it will be useful, % GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of % but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details. % GNU General Public License for more details.
% %
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>. % along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% %
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
close all; close all;
clear all; clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs') addpath('./libs')
end end
samplingFreq = 5000000; %[Hz] samplingFreq = 5000000; %[Hz]
coherent_integration_time_ms = 20; %[ms] coherent_integration_time_ms = 20; %[ms]
channels = 5; % Number of channels channels = 5; % Number of channels
first_channel = 0; % Number of the first channel first_channel = 0; % Number of the first channel
path = '/dump_dir/'; %% CHANGE THIS PATH path = '/dump_dir/'; %% CHANGE THIS PATH
for N=1:1:channels for N=1:1:channels
tracking_log_path = [path 'track_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch_ BY YOUR dump_filename tracking_log_path = [path 'track_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch_ BY YOUR dump_filename
GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path);
end end
% GNSS-SDR format conversion to MATLAB GPS receiver % GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels for N=1:1:channels
trackResults(N).status = 'T'; %fake track trackResults(N).status = 'T'; %fake track
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P = GNSS_tracking(N).P.'; trackResults(N).I_P = GNSS_tracking(N).P.';
trackResults(N).Q_P = zeros(1,length(GNSS_tracking(N).P)); trackResults(N).Q_P = zeros(1,length(GNSS_tracking(N).P));
trackResults(N).I_VE = GNSS_tracking(N).VE.'; trackResults(N).I_VE = GNSS_tracking(N).VE.';
trackResults(N).I_E = GNSS_tracking(N).E.'; trackResults(N).I_E = GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.'; trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).I_VL = GNSS_tracking(N).VL.'; trackResults(N).I_VL = GNSS_tracking(N).VL.';
trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE)); trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE));
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L)); trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L));
trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL));
trackResults(N).data_I = GNSS_tracking(N).prompt_I.'; trackResults(N).data_I = GNSS_tracking(N).prompt_I.';
trackResults(N).data_Q = GNSS_tracking(N).prompt_Q.'; trackResults(N).data_Q = GNSS_tracking(N).prompt_Q.';
trackResults(N).PRN = GNSS_tracking(N).PRN.'; trackResults(N).PRN = GNSS_tracking(N).PRN.';
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
% Use original MATLAB tracking plot function % Use original MATLAB tracking plot function
settings.numberOfChannels = channels; settings.numberOfChannels = channels;
settings.msToProcess = length(GNSS_tracking(N).E) * coherent_integration_time_ms; settings.msToProcess = length(GNSS_tracking(N).E) * coherent_integration_time_ms;
plotVEMLTracking(N, trackResults, settings) plotVEMLTracking(N, trackResults, settings)
end end

158
src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m
View File

@ -1,79 +1,79 @@
% Reads GNSS-SDR Tracking dump binary file using the provided % Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables % function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es % Javier Arribas, 2011. jarribas(at)cttc.es
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) % Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
% %
% GNSS-SDR is a software defined Global Navigation % GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver % Satellite Systems receiver
% %
% This file is part of GNSS-SDR. % This file is part of GNSS-SDR.
% %
% GNSS-SDR is free software: you can redistribute it and/or modify % GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by % it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or % the Free Software Foundation, either version 3 of the License, or
% at your option) any later version. % at your option) any later version.
% %
% GNSS-SDR is distributed in the hope that it will be useful, % GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of % but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details. % GNU General Public License for more details.
% %
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>. % along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% %
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
close all; close all;
clear all; clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs') addpath('./libs')
end end
samplingFreq = 5000000; %[Hz] samplingFreq = 5000000; %[Hz]
channels = 7; % Number of channels channels = 7; % Number of channels
first_channel = 0; % Number of the first channel first_channel = 0; % Number of the first channel
path = '/Users/carlesfernandez/git/cttc/build/'; %% CHANGE THIS PATH path = '/Users/carlesfernandez/git/cttc/build/'; %% CHANGE THIS PATH
for N=1:1:channels for N=1:1:channels
tracking_log_path = [path 'track_ch' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch BY YOUR dump_filename tracking_log_path = [path 'track_ch' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch BY YOUR dump_filename
GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path);
end end
% GNSS-SDR format conversion to MATLAB GPS receiver % GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels for N=1:1:channels
trackResults(N).status = 'T'; %fake track trackResults(N).status = 'T'; %fake track
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; trackResults(N).I_P = GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.';
trackResults(N).I_VE = GNSS_tracking(N).VE.'; trackResults(N).I_VE = GNSS_tracking(N).VE.';
trackResults(N).I_E = GNSS_tracking(N).E.'; trackResults(N).I_E = GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.'; trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).I_VL = GNSS_tracking(N).VL.'; trackResults(N).I_VL = GNSS_tracking(N).VL.';
trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE)); trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE));
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L)); trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L));
trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL));
trackResults(N).PRN = GNSS_tracking(N).PRN.'; trackResults(N).PRN = GNSS_tracking(N).PRN.';
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
% Use original MATLAB tracking plot function % Use original MATLAB tracking plot function
settings.numberOfChannels = channels; settings.numberOfChannels = channels;
settings.msToProcess = length(GNSS_tracking(N).E)*4; settings.msToProcess = length(GNSS_tracking(N).E)*4;
plotVEMLTracking(N, trackResults, settings) plotVEMLTracking(N, trackResults, settings)
end end

2
src/utils/matlab/galileo_e5a_dll_pll_plot_sample.m
View File

@ -92,4 +92,4 @@ for N=1:1:channels
t = t/1000; t = t/1000;
plot(t, GNSS_tracking(N).carrier_doppler_hz / 1000); plot(t, GNSS_tracking(N).carrier_doppler_hz / 1000);
xlabel('Time(s)'); ylabel('Doppler(KHz)'); title(['Doppler frequency channel ' num2str(N)]); xlabel('Time(s)'); ylabel('Doppler(KHz)'); title(['Doppler frequency channel ' num2str(N)]);
end end

146
src/utils/matlab/glonass_ca_dll_pll_plot_sample.m
View File

@ -1,73 +1,73 @@
% Reads GNSS-SDR Tracking dump binary file using the provided % Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables % function and plots some internal variables
% Damian Miralles, 2017. dmiralles2009(at)gmail.com % Damian Miralles, 2017. dmiralles2009(at)gmail.com
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) % Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
% %
% GNSS-SDR is a software defined Global Navigation % GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver % Satellite Systems receiver
% %
% This file is part of GNSS-SDR. % This file is part of GNSS-SDR.
% %
% GNSS-SDR is free software: you can redistribute it and/or modify % GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by % it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or % the Free Software Foundation, either version 3 of the License, or
% at your option) any later version. % at your option) any later version.
% %
% GNSS-SDR is distributed in the hope that it will be useful, % GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of % but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details. % GNU General Public License for more details.
% %
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>. % along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% %
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
close all; close all;
clear all; clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs') addpath('./libs')
end end
samplingFreq = 6625000; %[Hz] samplingFreq = 6625000; %[Hz]
channels = 5; channels = 5;
first_channel = 0; first_channel = 0;
path = '/archive/'; %% CHANGE THIS PATH path = '/archive/'; %% CHANGE THIS PATH
for N=1:1:channels for N=1:1:channels
tracking_log_path = [path 'glo_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE glo_tracking_ch_ BY YOUR dump_filename tracking_log_path = [path 'glo_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE glo_tracking_ch_ BY YOUR dump_filename
GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path);
end end
% GNSS-SDR format conversion to MATLAB GPS receiver % GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels for N=1:1:channels
trackResults(N).status = 'T'; %fake track trackResults(N).status = 'T'; %fake track
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_freq_hz.'; trackResults(N).carrFreq = GNSS_tracking(N).carrier_freq_hz.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; trackResults(N).I_P = GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.';
trackResults(N).I_E = GNSS_tracking(N).E.'; trackResults(N).I_E = GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.'; trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
trackResults(N).PRN = ones(1,length(GNSS_tracking(N).E)); trackResults(N).PRN = ones(1,length(GNSS_tracking(N).E));
% Use original MATLAB tracking plot function % Use original MATLAB tracking plot function
settings.numberOfChannels = channels; settings.numberOfChannels = channels;
settings.msToProcess = length(GNSS_tracking(N).E); settings.msToProcess = length(GNSS_tracking(N).E);
plotTracking(N, trackResults, settings) plotTracking(N, trackResults, settings)
end end

150
src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m
View File

@ -1,75 +1,75 @@
% Reads GNSS-SDR Tracking dump binary file using the provided % Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables % function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es % Javier Arribas, 2011. jarribas(at)cttc.es
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) % Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
% %
% GNSS-SDR is a software defined Global Navigation % GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver % Satellite Systems receiver
% %
% This file is part of GNSS-SDR. % This file is part of GNSS-SDR.
% %
% GNSS-SDR is free software: you can redistribute it and/or modify % GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by % it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or % the Free Software Foundation, either version 3 of the License, or
% at your option) any later version. % at your option) any later version.
% %
% GNSS-SDR is distributed in the hope that it will be useful, % GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of % but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details. % GNU General Public License for more details.
% %
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>. % along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% %
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
close all; close all;
clear all; clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs') addpath('./libs')
end end
samplingFreq = 6625000; %[Hz] samplingFreq = 6625000; %[Hz]
channels = 5; channels = 5;
first_channel = 0; first_channel = 0;
path = '/archive/'; %% CHANGE THIS PATH path = '/archive/'; %% CHANGE THIS PATH
for N=1:1:channels for N=1:1:channels
tracking_log_path = [path 'epl_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename tracking_log_path = [path 'epl_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename
GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path);
end end
% GNSS-SDR format conversion to MATLAB GPS receiver % GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels for N=1:1:channels
trackResults(N).status = 'T'; %fake track trackResults(N).status = 'T'; %fake track
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; trackResults(N).I_P = GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.';
trackResults(N).I_E = GNSS_tracking(N).E.'; trackResults(N).I_E = GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.'; trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E)); trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).PRN = ones(1,length(GNSS_tracking(N).E)); trackResults(N).PRN = ones(1,length(GNSS_tracking(N).E));
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
% Use original MATLAB tracking plot function % Use original MATLAB tracking plot function
settings.numberOfChannels = channels; settings.numberOfChannels = channels;
settings.msToProcess = length(GNSS_tracking(N).E); settings.msToProcess = length(GNSS_tracking(N).E);
plotTracking(N, trackResults, settings) plotTracking(N, trackResults, settings)
end end

78
src/utils/matlab/gps_l1_ca_telemetry_plot_sample.m
View File

@ -1,39 +1,39 @@
% Reads GNSS-SDR Tracking dump binary file using the provided % Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables % function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es % Javier Arribas, 2011. jarribas(at)cttc.es
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) % Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
% %
% GNSS-SDR is a software defined Global Navigation % GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver % Satellite Systems receiver
% %
% This file is part of GNSS-SDR. % This file is part of GNSS-SDR.
% %
% GNSS-SDR is free software: you can redistribute it and/or modify % GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by % it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or % the Free Software Foundation, either version 3 of the License, or
% at your option) any later version. % at your option) any later version.
% %
% GNSS-SDR is distributed in the hope that it will be useful, % GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of % but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details. % GNU General Public License for more details.
% %
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>. % along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% %
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
%close all; %close all;
%clear all; %clear all;
samplingFreq = 64e6/16; %[Hz] samplingFreq = 64e6/16; %[Hz]
channels=4; channels=4;
path='/home/javier/workspace/gnss-sdr-ref/trunk/install/'; path='/home/javier/workspace/gnss-sdr-ref/trunk/install/';
clear PRN_absolute_sample_start; clear PRN_absolute_sample_start;
for N=1:1:channels for N=1:1:channels
telemetry_log_path=[path 'telemetry' num2str(N-1) '.dat']; telemetry_log_path=[path 'telemetry' num2str(N-1) '.dat'];
GNSS_telemetry(N)= gps_l1_ca_read_telemetry_dump(telemetry_log_path); GNSS_telemetry(N)= gps_l1_ca_read_telemetry_dump(telemetry_log_path);
end end

2
src/utils/matlab/help_script1.m
View File

@ -55,4 +55,4 @@ error_ms=preambles_timestamp_sdr_ms(common_start_index:(common_start_index+lengt
% %
% plot(GNSS_observables.preamble_delay_ms(channel,:)); % plot(GNSS_observables.preamble_delay_ms(channel,:));
% %
% plot(GNSS_observables.prn_delay_ms(channel,:),'r') % plot(GNSS_observables.prn_delay_ms(channel,:),'r')

116
src/utils/matlab/libs/geoFunctions/cart2geo.m
View File

@ -1,58 +1,58 @@
function [phi, lambda, h] = cart2geo(X, Y, Z, i) function [phi, lambda, h] = cart2geo(X, Y, Z, i)
% CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical % CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
% coordinates (phi, lambda, h) on a selected reference ellipsoid. % coordinates (phi, lambda, h) on a selected reference ellipsoid.
% %
% [phi, lambda, h] = cart2geo(X, Y, Z, i); % [phi, lambda, h] = cart2geo(X, Y, Z, i);
% %
% Choices i of Reference Ellipsoid for Geographical Coordinates % Choices i of Reference Ellipsoid for Geographical Coordinates
% 1. International Ellipsoid 1924 % 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967 % 2. International Ellipsoid 1967
% 3. World Geodetic System 1972 % 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980 % 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984 % 5. World Geodetic System 1984
% Kai Borre 10-13-98 % Kai Borre 10-13-98
% Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
% Revision: 1.0 Date: 1998/10/23 % Revision: 1.0 Date: 1998/10/23
%========================================================================== %==========================================================================
a = [6378388 6378160 6378135 6378137 6378137]; a = [6378388 6378160 6378135 6378137 6378137];
f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563]; f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563];
lambda = atan2(Y,X); lambda = atan2(Y,X);
ex2 = (2-f(i))*f(i)/((1-f(i))^2); ex2 = (2-f(i))*f(i)/((1-f(i))^2);
c = a(i)*sqrt(1+ex2); c = a(i)*sqrt(1+ex2);
phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i)))); phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i))));
h = 0.1; oldh = 0; h = 0.1; oldh = 0;
iterations = 0; iterations = 0;
while abs(h-oldh) > 1.e-12 while abs(h-oldh) > 1.e-12
oldh = h; oldh = h;
N = c/sqrt(1+ex2*cos(phi)^2); N = c/sqrt(1+ex2*cos(phi)^2);
phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i))*f(i)*N/(N+h))))); phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i))*f(i)*N/(N+h)))));
h = sqrt(X^2+Y^2)/cos(phi)-N; h = sqrt(X^2+Y^2)/cos(phi)-N;
iterations = iterations + 1; iterations = iterations + 1;
if iterations > 100 if iterations > 100
fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh); fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh);
break; break;
end end
end end
phi = phi*180/pi; phi = phi*180/pi;
% b = zeros(1,3); % b = zeros(1,3);
% b(1,1) = fix(phi); % b(1,1) = fix(phi);
% b(2,1) = fix(rem(phi,b(1,1))*60); % b(2,1) = fix(rem(phi,b(1,1))*60);
% b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600; % b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600;
lambda = lambda*180/pi; lambda = lambda*180/pi;
% l = zeros(1,3); % l = zeros(1,3);
% l(1,1) = fix(lambda); % l(1,1) = fix(lambda);
% l(2,1) = fix(rem(lambda,l(1,1))*60); % l(2,1) = fix(rem(lambda,l(1,1))*60);
% l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600; % l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600;
%fprintf('\n phi =%3.0f %3.0f %8.5f',b(1),b(2),b(3)) %fprintf('\n phi =%3.0f %3.0f %8.5f',b(1),b(2),b(3))
%fprintf('\n lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3)) %fprintf('\n lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3))
%fprintf('\n h =%14.3f\n',h) %fprintf('\n h =%14.3f\n',h)
%%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%%

346
src/utils/matlab/libs/geoFunctions/cart2utm.m
View File

@ -1,173 +1,173 @@
function [E, N, U] = cart2utm(X, Y, Z, zone) function [E, N, U] = cart2utm(X, Y, Z, zone)
% CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. % CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'.
% %
% [E, N, U] = cart2utm(X, Y, Z, zone); % [E, N, U] = cart2utm(X, Y, Z, zone);
% %
% Inputs: % Inputs:
% X,Y,Z - Cartesian coordinates. Coordinates are referenced % X,Y,Z - Cartesian coordinates. Coordinates are referenced
% with respect to the International Terrestrial Reference % with respect to the International Terrestrial Reference
% Frame 1996 (ITRF96) % Frame 1996 (ITRF96)
% zone - UTM zone of the given position % zone - UTM zone of the given position
% %
% Outputs: % Outputs:
% E, N, U - UTM coordinates (Easting, Northing, Uping) % E, N, U - UTM coordinates (Easting, Northing, Uping)
% Kai Borre -11-1994 % Kai Borre -11-1994
% Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
% This implementation is based upon % This implementation is based upon
% O. Andersson & K. Poder (1981) Koordinattransformationer % O. Andersson & K. Poder (1981) Koordinattransformationer
% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren % ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
% Vol. 30: 552--571 and Vol. 31: 76 % Vol. 30: 552--571 and Vol. 31: 76
% %
% An excellent, general reference (KW) is % An excellent, general reference (KW) is
% R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der % R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
% h\"oheren Geod\"asie und Kartographie. % h\"oheren Geod\"asie und Kartographie.
% Erster Band, Springer Verlag % Erster Band, Springer Verlag
% Explanation of variables used: % Explanation of variables used:
% f flattening of ellipsoid % f flattening of ellipsoid
% a semi major axis in m % a semi major axis in m
% m0 1 - scale at central meridian; for UTM 0.0004 % m0 1 - scale at central meridian; for UTM 0.0004
% Q_n normalized meridian quadrant % Q_n normalized meridian quadrant
% E0 Easting of central meridian % E0 Easting of central meridian
% L0 Longitude of central meridian % L0 Longitude of central meridian
% bg constants for ellipsoidal geogr. to spherical geogr. % bg constants for ellipsoidal geogr. to spherical geogr.
% gb constants for spherical geogr. to ellipsoidal geogr. % gb constants for spherical geogr. to ellipsoidal geogr.
% gtu constants for ellipsoidal N, E to spherical N, E % gtu constants for ellipsoidal N, E to spherical N, E
% utg constants for spherical N, E to ellipoidal N, E % utg constants for spherical N, E to ellipoidal N, E
% tolutm tolerance for utm, 1.2E-10*meridian quadrant % tolutm tolerance for utm, 1.2E-10*meridian quadrant
% tolgeo tolerance for geographical, 0.00040 second of arc % tolgeo tolerance for geographical, 0.00040 second of arc
% B, L refer to latitude and longitude. Southern latitude is negative % B, L refer to latitude and longitude. Southern latitude is negative
% International ellipsoid of 1924, valid for ED50 % International ellipsoid of 1924, valid for ED50
a = 6378388; a = 6378388;
f = 1/297; f = 1/297;
ex2 = (2-f)*f / ((1-f)^2); ex2 = (2-f)*f / ((1-f)^2);
c = a * sqrt(1+ex2); c = a * sqrt(1+ex2);
vec = [X; Y; Z-4.5]; vec = [X; Y; Z-4.5];
alpha = .756e-6; alpha = .756e-6;
R = [ 1 -alpha 0; R = [ 1 -alpha 0;
alpha 1 0; alpha 1 0;
0 0 1]; 0 0 1];
trans = [89.5; 93.8; 127.6]; trans = [89.5; 93.8; 127.6];
scale = 0.9999988; scale = 0.9999988;
v = scale*R*vec + trans; % coordinate vector in ED50 v = scale*R*vec + trans; % coordinate vector in ED50
L = atan2(v(2), v(1)); L = atan2(v(2), v(1));
N1 = 6395000; % preliminary value N1 = 6395000; % preliminary value
B = atan2(v(3)/((1-f)^2*N1), norm(v(1:2))/N1); % preliminary value B = atan2(v(3)/((1-f)^2*N1), norm(v(1:2))/N1); % preliminary value
U = 0.1; oldU = 0; U = 0.1; oldU = 0;
iterations = 0; iterations = 0;
while abs(U-oldU) > 1.e-4 while abs(U-oldU) > 1.e-4
oldU = U; oldU = U;
N1 = c/sqrt(1+ex2*(cos(B))^2); N1 = c/sqrt(1+ex2*(cos(B))^2);
B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) ); B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) );
U = norm(v(1:2))/cos(B)-N1; U = norm(v(1:2))/cos(B)-N1;
iterations = iterations + 1; iterations = iterations + 1;
if iterations > 100 if iterations > 100
fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU); fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU);
break; break;
end end
end end
% Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) % Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
m0 = 0.0004; m0 = 0.0004;
n = f / (2-f); n = f / (2-f);
m = n^2 * (1/4 + n*n/64); m = n^2 * (1/4 + n*n/64);
w = (a*(-n-m0+m*(1-m0))) / (1+n); w = (a*(-n-m0+m*(1-m0))) / (1+n);
Q_n = a + w; Q_n = a + w;
% Easting and longitude of central meridian % Easting and longitude of central meridian
E0 = 500000; E0 = 500000;
L0 = (zone-30)*6 - 3; L0 = (zone-30)*6 - 3;
% Check tolerance for reverse transformation % Check tolerance for reverse transformation
tolutm = pi/2 * 1.2e-10 * Q_n; tolutm = pi/2 * 1.2e-10 * Q_n;
tolgeo = 0.000040; tolgeo = 0.000040;
% Coefficients of trigonometric series % Coefficients of trigonometric series
% ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) % ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52)
% bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45)))); % bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45))));
% bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9))); % bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9)));
% bg[3] = n^3*(-26/15 + n*34/21); % bg[3] = n^3*(-26/15 + n*34/21);
% bg[4] = n^4*1237/630; % bg[4] = n^4*1237/630;
% spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) % spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62)
% gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45))); % gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45)));
% gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45))); % gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45)));
% gb[3] = n^3*(56/15 + n*(-136/35)); % gb[3] = n^3*(56/15 + n*(-136/35));
% gb[4] = n^4*4279/630; % gb[4] = n^4*4279/630;
% spherical to ellipsoidal N, E, KW p. 196, (69) % spherical to ellipsoidal N, E, KW p. 196, (69)
% gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180))); % gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180)));
% gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440)); % gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440));
% gtu[3] = n^3*(61/240 + n*(-103/140)); % gtu[3] = n^3*(61/240 + n*(-103/140));
% gtu[4] = n^4*49561/161280; % gtu[4] = n^4*49561/161280;
% ellipsoidal to spherical N, E, KW p. 194, (65) % ellipsoidal to spherical N, E, KW p. 194, (65)
% utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360))); % utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360)));
% utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440)); % utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440));
% utg[3] = n^3*(-17/480 + n*37/840); % utg[3] = n^3*(-17/480 + n*37/840);
% utg[4] = n^4*(-4397/161280); % utg[4] = n^4*(-4397/161280);
% With f = 1/297 we get % With f = 1/297 we get
bg = [-3.37077907e-3; bg = [-3.37077907e-3;
4.73444769e-6; 4.73444769e-6;
-8.29914570e-9; -8.29914570e-9;
1.58785330e-11]; 1.58785330e-11];
gb = [ 3.37077588e-3; gb = [ 3.37077588e-3;
6.62769080e-6; 6.62769080e-6;
1.78718601e-8; 1.78718601e-8;
5.49266312e-11]; 5.49266312e-11];
gtu = [ 8.41275991e-4; gtu = [ 8.41275991e-4;
7.67306686e-7; 7.67306686e-7;
1.21291230e-9; 1.21291230e-9;
2.48508228e-12]; 2.48508228e-12];
utg = [-8.41276339e-4; utg = [-8.41276339e-4;
-5.95619298e-8; -5.95619298e-8;
-1.69485209e-10; -1.69485209e-10;
-2.20473896e-13]; -2.20473896e-13];
% Ellipsoidal latitude, longitude to spherical latitude, longitude % Ellipsoidal latitude, longitude to spherical latitude, longitude
neg_geo = 'FALSE'; neg_geo = 'FALSE';
if B < 0 if B < 0
neg_geo = 'TRUE '; neg_geo = 'TRUE ';
end end
Bg_r = abs(B); Bg_r = abs(B);
[res_clensin] = clsin(bg, 4, 2*Bg_r); [res_clensin] = clsin(bg, 4, 2*Bg_r);
Bg_r = Bg_r + res_clensin; Bg_r = Bg_r + res_clensin;
L0 = L0*pi / 180; L0 = L0*pi / 180;
Lg_r = L - L0; Lg_r = L - L0;
% Spherical latitude, longitude to complementary spherical latitude % Spherical latitude, longitude to complementary spherical latitude
% i.e. spherical N, E % i.e. spherical N, E
cos_BN = cos(Bg_r); cos_BN = cos(Bg_r);
Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN); Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN);
Ep = atanh(sin(Lg_r) * cos_BN); Ep = atanh(sin(Lg_r) * cos_BN);
%Spherical normalized N, E to ellipsoidal N, E %Spherical normalized N, E to ellipsoidal N, E
Np = 2 * Np; Np = 2 * Np;
Ep = 2 * Ep; Ep = 2 * Ep;
[dN, dE] = clksin(gtu, 4, Np, Ep); [dN, dE] = clksin(gtu, 4, Np, Ep);
Np = Np/2; Np = Np/2;
Ep = Ep/2; Ep = Ep/2;
Np = Np + dN; Np = Np + dN;
Ep = Ep + dE; Ep = Ep + dE;
N = Q_n * Np; N = Q_n * Np;
E = Q_n*Ep + E0; E = Q_n*Ep + E0;
if neg_geo == 'TRUE ' if neg_geo == 'TRUE '
N = -N + 20000000; N = -N + 20000000;
end; end;
%%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%%

50
src/utils/matlab/libs/geoFunctions/check_t.m
View File

@ -1,26 +1,26 @@
function corrTime = check_t(time) function corrTime = check_t(time)
% CHECK_T accounting for beginning or end of week crossover. % CHECK_T accounting for beginning or end of week crossover.
% %
% corrTime = check_t(time); % corrTime = check_t(time);
% %
% Inputs: % Inputs:
% time - time in seconds % time - time in seconds
% %
% Outputs: % Outputs:
% corrTime - corrected time (seconds) % corrTime - corrected time (seconds)
% Kai Borre 04-01-96 % Kai Borre 04-01-96
% Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
%========================================================================== %==========================================================================
half_week = 302400; % seconds half_week = 302400; % seconds
corrTime = time; corrTime = time;
if time > half_week if time > half_week
corrTime = time - 2*half_week; corrTime = time - 2*half_week;
elseif time < -half_week elseif time < -half_week
corrTime = time + 2*half_week; corrTime = time + 2*half_week;
end end
%%%%%%% end check_t.m %%%%%%%%%%%%%%%%% %%%%%%% end check_t.m %%%%%%%%%%%%%%%%%

72
src/utils/matlab/libs/geoFunctions/clksin.m
View File

@ -1,36 +1,36 @@
function [re, im] = clksin(ar, degree, arg_real, arg_imag) function [re, im] = clksin(ar, degree, arg_real, arg_imag)
% Clenshaw summation of sinus with complex argument % Clenshaw summation of sinus with complex argument
% [re, im] = clksin(ar, degree, arg_real, arg_imag); % [re, im] = clksin(ar, degree, arg_real, arg_imag);
% Written by Kai Borre % Written by Kai Borre
% December 20, 1995 % December 20, 1995
% %
% See also WGS2UTM or CART2UTM % See also WGS2UTM or CART2UTM
% %
%========================================================================== %==========================================================================
sin_arg_r = sin(arg_real); sin_arg_r = sin(arg_real);
cos_arg_r = cos(arg_real); cos_arg_r = cos(arg_real);
sinh_arg_i = sinh(arg_imag); sinh_arg_i = sinh(arg_imag);
cosh_arg_i = cosh(arg_imag); cosh_arg_i = cosh(arg_imag);
r = 2 * cos_arg_r * cosh_arg_i; r = 2 * cos_arg_r * cosh_arg_i;
i =-2 * sin_arg_r * sinh_arg_i; i =-2 * sin_arg_r * sinh_arg_i;
hr1 = 0; hr = 0; hi1 = 0; hi = 0; hr1 = 0; hr = 0; hi1 = 0; hi = 0;
for t = degree : -1 : 1 for t = degree : -1 : 1
hr2 = hr1; hr2 = hr1;
hr1 = hr; hr1 = hr;
hi2 = hi1; hi2 = hi1;
hi1 = hi; hi1 = hi;
z = ar(t) + r*hr1 - i*hi - hr2; z = ar(t) + r*hr1 - i*hi - hr2;
hi = i*hr1 + r*hi1 - hi2; hi = i*hr1 + r*hi1 - hi2;
hr = z; hr = z;
end end
r = sin_arg_r * cosh_arg_i; r = sin_arg_r * cosh_arg_i;
i = cos_arg_r * sinh_arg_i; i = cos_arg_r * sinh_arg_i;
re = r*hr - i*hi; re = r*hr - i*hi;
im = r*hi + i*hr; im = r*hi + i*hr;

46
src/utils/matlab/libs/geoFunctions/clsin.m
View File

@ -1,24 +1,24 @@
function result = clsin(ar, degree, argument) function result = clsin(ar, degree, argument)
% Clenshaw summation of sinus of argument. % Clenshaw summation of sinus of argument.
% %
% result = clsin(ar, degree, argument); % result = clsin(ar, degree, argument);
% Written by Kai Borre % Written by Kai Borre
% December 20, 1995 % December 20, 1995
% %
% See also WGS2UTM or CART2UTM % See also WGS2UTM or CART2UTM
%========================================================================== %==========================================================================
cos_arg = 2 * cos(argument); cos_arg = 2 * cos(argument);
hr1 = 0; hr1 = 0;
hr = 0; hr = 0;
for t = degree : -1 : 1 for t = degree : -1 : 1
hr2 = hr1; hr2 = hr1;
hr1 = hr; hr1 = hr;
hr = ar(t) + cos_arg*hr1 - hr2; hr = ar(t) + cos_arg*hr1 - hr2;
end end
result = hr * sin(argument); result = hr * sin(argument);
%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%%

86
src/utils/matlab/libs/geoFunctions/deg2dms.m
View File

@ -1,43 +1,43 @@
function dmsOutput = deg2dms(deg) function dmsOutput = deg2dms(deg)
% DEG2DMS Conversion of degrees to degrees, minutes, and seconds. % DEG2DMS Conversion of degrees to degrees, minutes, and seconds.
% The output format (dms format) is: (degrees*100 + minutes + seconds/100) % The output format (dms format) is: (degrees*100 + minutes + seconds/100)
% Written by Kai Borre % Written by Kai Borre
% February 7, 2001 % February 7, 2001
% Updated by Darius Plausinaitis % Updated by Darius Plausinaitis
%%% Save the sign for later processing %%% Save the sign for later processing
neg_arg = false; neg_arg = false;
if deg < 0 if deg < 0
% Only positive numbers should be used while spliting into deg/min/sec % Only positive numbers should be used while spliting into deg/min/sec
deg = -deg; deg = -deg;
neg_arg = true; neg_arg = true;
end end
%%% Split degrees minutes and seconds %%% Split degrees minutes and seconds
int_deg = floor(deg); int_deg = floor(deg);
decimal = deg - int_deg; decimal = deg - int_deg;
min_part = decimal*60; min_part = decimal*60;
min = floor(min_part); min = floor(min_part);
sec_part = min_part - floor(min_part); sec_part = min_part - floor(min_part);
sec = sec_part*60; sec = sec_part*60;
%%% Check for overflow %%% Check for overflow
if sec == 60 if sec == 60
min = min + 1; min = min + 1;
sec = 0; sec = 0;
end end
if min == 60 if min == 60
int_deg = int_deg + 1; int_deg = int_deg + 1;
min = 0; min = 0;
end end
%%% Construct the output %%% Construct the output
dmsOutput = int_deg * 100 + min + sec/100; dmsOutput = int_deg * 100 + min + sec/100;
%%% Correct the sign %%% Correct the sign
if neg_arg == true if neg_arg == true
dmsOutput = -dmsOutput; dmsOutput = -dmsOutput;
end end
%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%%

24
src/utils/matlab/libs/geoFunctions/dms2deg.m
View File

@ -1,12 +1,12 @@
function deg = dms2deg(dms) function deg = dms2deg(dms)
% DMS2DEG Conversion of degrees, minutes, and seconds to degrees. % DMS2DEG Conversion of degrees, minutes, and seconds to degrees.
% Written by Javier Arribas 2011 % Written by Javier Arribas 2011
% December 7, 2011 % December 7, 2011
%if (dms(1)>=0) %if (dms(1)>=0)
deg=dms(1)+dms(2)/60+dms(3)/3600; deg=dms(1)+dms(2)/60+dms(3)/3600;
%else %else
%deg=dms(1)-dms(2)/60-dms(3)/3600; %deg=dms(1)-dms(2)/60-dms(3)/3600;
%end %end

207
src/utils/matlab/libs/geoFunctions/dms2mat.m
View File

@ -1,104 +1,103 @@
function [dout,mout,sout] = dms2mat(dms,n) function [dout,mout,sout] = dms2mat(dms,n)
% DMS2MAT Converts a dms vector format to a [deg min sec] matrix % DMS2MAT Converts a dms vector format to a [deg min sec] matrix
% %
% [d,m,s] = DMS2MAT(dms) converts a dms vector format to a % [d,m,s] = DMS2MAT(dms) converts a dms vector format to a
% deg:min:sec matrix. The vector format is dms = 100*deg + min + sec/100. % deg:min:sec matrix. The vector format is dms = 100*deg + min + sec/100.
% This allows compressed dms data to be expanded to a d,m,s triple, % This allows compressed dms data to be expanded to a d,m,s triple,
% for easier reporting and viewing of the data. % for easier reporting and viewing of the data.
% %
% [d,m,s] = DMS2MAT(dms,n) uses n digits in the accuracy of the % [d,m,s] = DMS2MAT(dms,n) uses n digits in the accuracy of the
% seconds calculation. n = -2 uses accuracy in the hundredths position, % seconds calculation. n = -2 uses accuracy in the hundredths position,
% n = 0 uses accuracy in the units position. Default is n = -5. % n = 0 uses accuracy in the units position. Default is n = -5.
% For further discussion of the input n, see ROUNDN. % For further discussion of the input n, see ROUNDN.
% %
% mat = DMS2MAT(...) returns a single output argument of mat = [d m s]. % mat = DMS2MAT(...) returns a single output argument of mat = [d m s].
% This is useful only if the input dms is a single column vector. % This is useful only if the input dms is a single column vector.
% %
% See also MAT2DMS % See also MAT2DMS
% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown % Written by: E. Byrns, E. Brown
% Revision: 1.10 $Date: 2002/03/20 21:25:06 % Revision: 1.10 $Date: 2002/03/20 21:25:06
if nargin == 0 if nargin == 0
error('Incorrect number of arguments') error('Incorrect number of arguments')
elseif nargin == 1 elseif nargin == 1
n = -5; n = -5;
end end
% Test for empty arguments % Test for empty arguments
if isempty(dms); dout = []; mout = []; sout = []; return; end if isempty(dms); dout = []; mout = []; sout = []; return; end
% Test for complex arguments % Test for complex arguments
if ~isreal(dms) if ~isreal(dms)
warning('Imaginary parts of complex ANGLE argument ignored') warning('Imaginary parts of complex ANGLE argument ignored')
dms = real(dms); dms = real(dms);
end end
% Don't let seconds be rounded beyond the tens place. % Don't let seconds be rounded beyond the tens place.
% If you did, then 55 seconds rounds to 100, which is not good. % If you did, then 55 seconds rounds to 100, which is not good.
if n == 2; n = 1; end if n == 2; n = 1; end
% Construct a sign vector which has +1 when dms >= 0 and -1 when dms < 0. % Construct a sign vector which has +1 when dms >= 0 and -1 when dms < 0.
signvec = sign(dms); signvec = sign(dms);
signvec = signvec + (signvec == 0); % Ensure +1 when dms = 0 signvec = signvec + (signvec == 0); % Ensure +1 when dms = 0
% Decompress the dms data vector % Decompress the dms data vector
dms = abs(dms); dms = abs(dms);
d = fix(dms/100); % Degrees d = fix(dms/100); % Degrees
m = fix(dms) - abs(100*d); % Minutes m = fix(dms) - abs(100*d); % Minutes
[s,msg] = roundn(100*rem(dms,1),n); % Seconds: Truncate to roundoff error [s,msg] = roundn(100*rem(dms,1),n); % Seconds: Truncate to roundoff error
if ~isempty(msg); error(msg); end if ~isempty(msg); error(msg); end
% Adjust for 60 seconds or 60 minutes. % Adjust for 60 seconds or 60 minutes.
% Test for seconds > 60 to allow for round-off from roundn, % Test for seconds > 60 to allow for round-off from roundn,
% Test for minutes > 60 as a ripple effect from seconds > 60 % Test for minutes > 60 as a ripple effect from seconds > 60
indx = find(s >= 60); indx = find(s >= 60);
if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = s(indx) - 60; end if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = s(indx) - 60; end
indx = find(m >= 60); indx = find(m >= 60);
if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx) - 60; end if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx) - 60; end
% Data consistency checks % Data consistency checks
if any(m > 59) | any (m < 0) if any(m > 59) | any (m < 0)
error('Minutes must be >= 0 and <= 59') error('Minutes must be >= 0 and <= 59')
elseif any(s >= 60) | any( s < 0) elseif any(s >= 60) | any( s < 0)
error('Seconds must be >= 0 and < 60') error('Seconds must be >= 0 and < 60')
end end
% Determine where to store the sign of the angle. It should be % Determine where to store the sign of the angle. It should be
% associated with the largest nonzero component of d:m:s. % associated with the largest nonzero component of d:m:s.
dsign = signvec .* (d~=0); dsign = signvec .* (d~=0);
msign = signvec .* (d==0 & m~=0); msign = signvec .* (d==0 & m~=0);
ssign = signvec .* (d==0 & m==0 & s~=0); ssign = signvec .* (d==0 & m==0 & s~=0);
% In the application of signs below, the comparison with 0 is used so that % In the application of signs below, the comparison with 0 is used so that
% the sign vector contains only +1 and -1. Any zero occurances causes % the sign vector contains only +1 and -1. Any zero occurances causes
% data to be lost when the sign has been applied to a higher component % data to be lost when the sign has been applied to a higher component
% of d:m:s. Use fix function to eliminate potential round-off errors. % of d:m:s. Use fix function to eliminate potential round-off errors.
d = ((dsign==0) + dsign).*fix(d); % Apply signs to the degrees d = ((dsign==0) + dsign).*fix(d); % Apply signs to the degrees
m = ((msign==0) + msign).*fix(m); % Apply signs to minutes m = ((msign==0) + msign).*fix(m); % Apply signs to minutes
s = ((ssign==0) + ssign).*s; % Apply signs to seconds s = ((ssign==0) + ssign).*s; % Apply signs to seconds
% Set the output arguments % Set the output arguments
if nargout <= 1 if nargout <= 1
dout = [d m s]; dout = [d m s];
elseif nargout == 3 elseif nargout == 3
dout = d; mout = m; sout = s; dout = d; mout = m; sout = s;
else else
error('Invalid number of output arguments') error('Invalid number of output arguments')
end end

62
src/utils/matlab/libs/geoFunctions/e_r_corr.m
View File

@ -1,31 +1,31 @@
function X_sat_rot = e_r_corr(traveltime, X_sat) function X_sat_rot = e_r_corr(traveltime, X_sat)
% E_R_CORR Returns rotated satellite ECEF coordinates due to Earth % E_R_CORR Returns rotated satellite ECEF coordinates due to Earth
% rotation during signal travel time % rotation during signal travel time
% %
% X_sat_rot = e_r_corr(traveltime, X_sat); % X_sat_rot = e_r_corr(traveltime, X_sat);
% %
% Inputs: % Inputs:
% travelTime - signal travel time % travelTime - signal travel time
% X_sat - satellite's ECEF coordinates % X_sat - satellite's ECEF coordinates
% %
% Outputs: % Outputs:
% X_sat_rot - rotated satellite's coordinates (ECEF) % X_sat_rot - rotated satellite's coordinates (ECEF)
% Written by Kai Borre % Written by Kai Borre
% Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
%========================================================================== %==========================================================================
Omegae_dot = 7.292115147e-5; % rad/sec Omegae_dot = 7.292115147e-5; % rad/sec
%--- Find rotation angle -------------------------------------------------- %--- Find rotation angle --------------------------------------------------
omegatau = Omegae_dot * traveltime; omegatau = Omegae_dot * traveltime;
%--- Make a rotation matrix ----------------------------------------------- %--- Make a rotation matrix -----------------------------------------------
R3 = [ cos(omegatau) sin(omegatau) 0; R3 = [ cos(omegatau) sin(omegatau) 0;
-sin(omegatau) cos(omegatau) 0; -sin(omegatau) cos(omegatau) 0;
0 0 1]; 0 0 1];
%--- Do the rotation ------------------------------------------------------ %--- Do the rotation ------------------------------------------------------
X_sat_rot = R3 * X_sat; X_sat_rot = R3 * X_sat;
%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%% %%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%%

138
src/utils/matlab/libs/geoFunctions/findUtmZone.m
View File

@ -1,69 +1,69 @@
function utmZone = findUtmZone(latitude, longitude) function utmZone = findUtmZone(latitude, longitude)
% Function finds the UTM zone number for given longitude and latitude. % Function finds the UTM zone number for given longitude and latitude.
% The longitude value must be between -180 (180 degree West) and 180 (180 % The longitude value must be between -180 (180 degree West) and 180 (180
% degree East) degree. The latitude must be within -80 (80 degree South) and % degree East) degree. The latitude must be within -80 (80 degree South) and
% 84 (84 degree North). % 84 (84 degree North).
% %
% utmZone = findUtmZone(latitude, longitude); % utmZone = findUtmZone(latitude, longitude);
% %
% Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not % Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
% 15 deg 30 min). % 15 deg 30 min).
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
% %
% Copyright (C) Darius Plausinaitis % Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis % Written by Darius Plausinaitis
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
%This program is free software; you can redistribute it and/or %This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License %modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2 %as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version. %of the License, or (at your option) any later version.
% %
%This program is distributed in the hope that it will be useful, %This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of %but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the %MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
%GNU General Public License for more details. %GNU General Public License for more details.
% %
%You should have received a copy of the GNU General Public License %You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software %along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, %Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA. %USA.
%========================================================================== %==========================================================================
%% Check value bounds ===================================================== %% Check value bounds =====================================================
if ((longitude > 180) || (longitude < -180)) if ((longitude > 180) || (longitude < -180))
error('Longitude value exceeds limits (-180:180).'); error('Longitude value exceeds limits (-180:180).');
end end
if ((latitude > 84) || (latitude < -80)) if ((latitude > 84) || (latitude < -80))
error('Latitude value exceeds limits (-80:84).'); error('Latitude value exceeds limits (-80:84).');
end end
%% Find zone ============================================================== %% Find zone ==============================================================
% Start at 180 deg west = -180 deg % Start at 180 deg west = -180 deg
utmZone = fix((180 + longitude)/ 6) + 1; utmZone = fix((180 + longitude)/ 6) + 1;
%% Correct zone numbers for particular areas ============================== %% Correct zone numbers for particular areas ==============================
if (latitude > 72) if (latitude > 72)
% Corrections for zones 31 33 35 37 % Corrections for zones 31 33 35 37
if ((longitude >= 0) && (longitude < 9)) if ((longitude >= 0) && (longitude < 9))
utmZone = 31; utmZone = 31;
elseif ((longitude >= 9) && (longitude < 21)) elseif ((longitude >= 9) && (longitude < 21))
utmZone = 33; utmZone = 33;
elseif ((longitude >= 21) && (longitude < 33)) elseif ((longitude >= 21) && (longitude < 33))
utmZone = 35; utmZone = 35;
elseif ((longitude >= 33) && (longitude < 42)) elseif ((longitude >= 33) && (longitude < 42))
utmZone = 37; utmZone = 37;
end end
elseif ((latitude >= 56) && (latitude < 64)) elseif ((latitude >= 56) && (latitude < 64))
% Correction for zone 32 % Correction for zone 32
if ((longitude >= 3) && (longitude < 12)) if ((longitude >= 3) && (longitude < 12))
utmZone = 32; utmZone = 32;
end end
end end

90
src/utils/matlab/libs/geoFunctions/geo2cart.m
View File

@ -1,46 +1,46 @@
function [X, Y, Z] = geo2cart(phi, lambda, h, i) function [X, Y, Z] = geo2cart(phi, lambda, h, i)
% GEO2CART Conversion of geographical coordinates (phi, lambda, h) to % GEO2CART Conversion of geographical coordinates (phi, lambda, h) to
% Cartesian coordinates (X, Y, Z). % Cartesian coordinates (X, Y, Z).
% %
% [X, Y, Z] = geo2cart(phi, lambda, h, i); % [X, Y, Z] = geo2cart(phi, lambda, h, i);
% %
% Format for phi and lambda: [degrees minutes seconds]. % Format for phi and lambda: [degrees minutes seconds].
% h, X, Y, and Z are in meters. % h, X, Y, and Z are in meters.
% %
% Choices i of Reference Ellipsoid % Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924 % 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967 % 2. International Ellipsoid 1967
% 3. World Geodetic System 1972 % 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980 % 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984 % 5. World Geodetic System 1984
% %
% Inputs: % Inputs:
% phi - geocentric latitude (format [degrees minutes seconds]) % phi - geocentric latitude (format [degrees minutes seconds])
% lambda - geocentric longitude (format [degrees minutes seconds]) % lambda - geocentric longitude (format [degrees minutes seconds])
% h - height % h - height
% i - reference ellipsoid type % i - reference ellipsoid type
% %
% Outputs: % Outputs:
% X, Y, Z - Cartesian coordinates (meters) % X, Y, Z - Cartesian coordinates (meters)
% Kai Borre 10-13-98 % Kai Borre 10-13-98
% Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
%========================================================================== %==========================================================================
b = phi(1) + phi(2)/60 + phi(3)/3600; b = phi(1) + phi(2)/60 + phi(3)/3600;
b = b*pi / 180; b = b*pi / 180;
l = lambda(1) + lambda(2)/60 + lambda(3)/3600; l = lambda(1) + lambda(2)/60 + lambda(3)/3600;
l = l*pi / 180; l = l*pi / 180;
a = [6378388 6378160 6378135 6378137 6378137]; a = [6378388 6378160 6378135 6378137 6378137];
f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563]; f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563];
ex2 = (2-f(i))*f(i) / ((1-f(i))^2); ex2 = (2-f(i))*f(i) / ((1-f(i))^2);
c = a(i) * sqrt(1+ex2); c = a(i) * sqrt(1+ex2);
N = c / sqrt(1 + ex2*cos(b)^2); N = c / sqrt(1 + ex2*cos(b)^2);
X = (N+h) * cos(b) * cos(l); X = (N+h) * cos(b) * cos(l);
Y = (N+h) * cos(b) * sin(l); Y = (N+h) * cos(b) * sin(l);
Z = ((1-f(i))^2*N + h) * sin(b); Z = ((1-f(i))^2*N + h) * sin(b);
%%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%%

222
src/utils/matlab/libs/geoFunctions/leastSquarePos.m
View File

@ -1,111 +1,111 @@
function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings)
% Function calculates the Least Square Solution. % Function calculates the Least Square Solution.
% %
% [pos, el, az, dop] = leastSquarePos(satpos, obs, settings); % [pos, el, az, dop] = leastSquarePos(satpos, obs, settings);
% %
% Inputs: % Inputs:
% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - % satpos - Satellites positions (in ECEF system: [X; Y; Z;] -
% one column per satellite) % one column per satellite)
% obs - Observations - the pseudorange measurements to each % obs - Observations - the pseudorange measurements to each
% satellite: % satellite:
% (e.g. [20000000 21000000 .... .... .... .... ....]) % (e.g. [20000000 21000000 .... .... .... .... ....])
% settings - receiver settings % settings - receiver settings
% %
% Outputs: % Outputs:
% pos - receiver position and receiver clock error % pos - receiver position and receiver clock error
% (in ECEF system: [X, Y, Z, dt]) % (in ECEF system: [X, Y, Z, dt])
% el - Satellites elevation angles (degrees) % el - Satellites elevation angles (degrees)
% az - Satellites azimuth angles (degrees) % az - Satellites azimuth angles (degrees)
% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) % dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
%Based on Kai Borre %Based on Kai Borre
%Copyright (c) by Kai Borre %Copyright (c) by Kai Borre
%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen %Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen
%========================================================================== %==========================================================================
%=== Initialization ======================================================= %=== Initialization =======================================================
nmbOfIterations = 7; nmbOfIterations = 7;
dtr = pi/180; dtr = pi/180;
pos = zeros(4, 1); pos = zeros(4, 1);
X = satpos; X = satpos;
nmbOfSatellites = size(satpos, 2); nmbOfSatellites = size(satpos, 2);
A = zeros(nmbOfSatellites, 4); A = zeros(nmbOfSatellites, 4);
omc = zeros(nmbOfSatellites, 1); omc = zeros(nmbOfSatellites, 1);
az = zeros(1, nmbOfSatellites); az = zeros(1, nmbOfSatellites);
el = az; el = az;
%=== Iteratively find receiver position =================================== %=== Iteratively find receiver position ===================================
for iter = 1:nmbOfIterations for iter = 1:nmbOfIterations
for i = 1:nmbOfSatellites for i = 1:nmbOfSatellites
if iter == 1 if iter == 1
%--- Initialize variables at the first iteration -------------- %--- Initialize variables at the first iteration --------------
Rot_X = X(:, i); Rot_X = X(:, i);
trop = 2; trop = 2;
else else
%--- Update equations ----------------------------------------- %--- Update equations -----------------------------------------
rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ...
(X(3, i) - pos(3))^2; (X(3, i) - pos(3))^2;
traveltime = sqrt(rho2) / settings.c ; traveltime = sqrt(rho2) / settings.c ;
%--- Correct satellite position (do to earth rotation) -------- %--- Correct satellite position (do to earth rotation) --------
Rot_X = e_r_corr(traveltime, X(:, i)); Rot_X = e_r_corr(traveltime, X(:, i));
%--- Find the elevation angel of the satellite ---------------- %--- Find the elevation angel of the satellite ----------------
[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
if (settings.useTropCorr == 1) if (settings.useTropCorr == 1)
%--- Calculate tropospheric correction -------------------- %--- Calculate tropospheric correction --------------------
trop = tropo(sin(el(i) * dtr), ... trop = tropo(sin(el(i) * dtr), ...
0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
else else
% Do not calculate or apply the tropospheric corrections % Do not calculate or apply the tropospheric corrections
trop = 0; trop = 0;
end end
end % if iter == 1 ... ... else end % if iter == 1 ... ... else
%--- Apply the corrections ---------------------------------------- %--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
%--- Construct the A matrix --------------------------------------- %--- Construct the A matrix ---------------------------------------
A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ...
(-(Rot_X(2) - pos(2))) / obs(i) ... (-(Rot_X(2) - pos(2))) / obs(i) ...
(-(Rot_X(3) - pos(3))) / obs(i) ... (-(Rot_X(3) - pos(3))) / obs(i) ...
1 ]; 1 ];
end % for i = 1:nmbOfSatellites end % for i = 1:nmbOfSatellites
% These lines allow the code to exit gracefully in case of any errors % These lines allow the code to exit gracefully in case of any errors
if rank(A) ~= 4 if rank(A) ~= 4
pos = zeros(1, 4); pos = zeros(1, 4);
return return
end end
%--- Find position update --------------------------------------------- %--- Find position update ---------------------------------------------
x = A \ omc; x = A \ omc;
%--- Apply position update -------------------------------------------- %--- Apply position update --------------------------------------------
pos = pos + x; pos = pos + x;
end % for iter = 1:nmbOfIterations end % for iter = 1:nmbOfIterations
pos = pos'; pos = pos';
%=== Calculate Dilution Of Precision ====================================== %=== Calculate Dilution Of Precision ======================================
if nargout == 4 if nargout == 4
%--- Initialize output ------------------------------------------------ %--- Initialize output ------------------------------------------------
dop = zeros(1, 5); dop = zeros(1, 5);
%--- Calculate DOP ---------------------------------------------------- %--- Calculate DOP ----------------------------------------------------
Q = inv(A'*A); Q = inv(A'*A);
dop(1) = sqrt(trace(Q)); % GDOP dop(1) = sqrt(trace(Q)); % GDOP
dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP
dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP
dop(4) = sqrt(Q(3,3)); % VDOP dop(4) = sqrt(Q(3,3)); % VDOP
dop(5) = sqrt(Q(4,4)); % TDOP dop(5) = sqrt(Q(4,4)); % TDOP
end end

248
src/utils/matlab/libs/geoFunctions/mat2dms.m
View File

@ -1,124 +1,124 @@
function dmsvec = mat2dms(d,m,s,n) function dmsvec = mat2dms(d,m,s,n)
% MAT2DMS Converts a [deg min sec] matrix to vector format % MAT2DMS Converts a [deg min sec] matrix to vector format
% %
% dms = MAT2DMS(d,m,s) converts a deg:min:sec matrix into a vector % dms = MAT2DMS(d,m,s) converts a deg:min:sec matrix into a vector
% format. The vector format is dms = 100*deg + min + sec/100. % format. The vector format is dms = 100*deg + min + sec/100.
% This allows d,m,s triple to be compressed into a single value, % This allows d,m,s triple to be compressed into a single value,
% which can then be employed similar to a degree or radian vector. % which can then be employed similar to a degree or radian vector.
% The inputs d, m and s must be of equal size. Minutes and % The inputs d, m and s must be of equal size. Minutes and
% second must be between 0 and 60. % second must be between 0 and 60.
% %
% dms = MAT2DMS(mat) assumes and input matrix of [d m s]. This is % dms = MAT2DMS(mat) assumes and input matrix of [d m s]. This is
% useful only for single column vectors for d, m and s. % useful only for single column vectors for d, m and s.
% %
% dms = MAT2DMS(d,m) and dms = MAT2DMS([d m]) assume that seconds % dms = MAT2DMS(d,m) and dms = MAT2DMS([d m]) assume that seconds
% are zero, s = 0. % are zero, s = 0.
% %
% dms = MAT2DMS(d,m,s,n) uses n as the accuracy of the seconds % dms = MAT2DMS(d,m,s,n) uses n as the accuracy of the seconds
% calculation. n = -2 uses accuracy in the hundredths position, % calculation. n = -2 uses accuracy in the hundredths position,
% n = 0 uses accuracy in the units position. Default is n = -5. % n = 0 uses accuracy in the units position. Default is n = -5.
% For further discussion of the input n, see ROUNDN. % For further discussion of the input n, see ROUNDN.
% %
% See also DMS2MAT % See also DMS2MAT
% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown % Written by: E. Byrns, E. Brown
% Revision: 1.10 Date: 2002/03/20 21:25:51 % Revision: 1.10 Date: 2002/03/20 21:25:51
if nargin == 0 if nargin == 0
error('Incorrect number of arguments') error('Incorrect number of arguments')
elseif nargin==1 elseif nargin==1
if size(d,2)== 3 if size(d,2)== 3
s = d(:,3); m = d(:,2); d = d(:,1); s = d(:,3); m = d(:,2); d = d(:,1);
elseif size(d,2)== 2 elseif size(d,2)== 2
m = d(:,2); d = d(:,1); s = zeros(size(d)); m = d(:,2); d = d(:,1); s = zeros(size(d));
elseif size(d,2) == 0 elseif size(d,2) == 0
d = []; m = []; s = []; d = []; m = []; s = [];
else else
error('Single input matrices must be n-by-2 or n-by-3.'); error('Single input matrices must be n-by-2 or n-by-3.');
end end
n = -5; n = -5;
elseif nargin == 2 elseif nargin == 2
s = zeros(size(d)); s = zeros(size(d));
n = -5; n = -5;
elseif nargin == 3 elseif nargin == 3
n = -5; n = -5;
end end
% Test for empty arguments % Test for empty arguments
if isempty(d) & isempty(m) & isempty(s); dmsvec = []; return; end if isempty(d) & isempty(m) & isempty(s); dmsvec = []; return; end
% Don't let seconds be rounded beyond the tens place. % Don't let seconds be rounded beyond the tens place.
% If you did, then 55 seconds rounds to 100, which is not good. % If you did, then 55 seconds rounds to 100, which is not good.
if n == 2; n = 1; end if n == 2; n = 1; end
% Complex argument tests % Complex argument tests
if any([~isreal(d) ~isreal(m) ~isreal(s)]) if any([~isreal(d) ~isreal(m) ~isreal(s)])
warning('Imaginary parts of complex ANGLE argument ignored') warning('Imaginary parts of complex ANGLE argument ignored')
d = real(d); m = real(m); s = real(s); d = real(d); m = real(m); s = real(s);
end end
% Dimension and value tests % Dimension and value tests
if ~isequal(size(d),size(m),size(s)) if ~isequal(size(d),size(m),size(s))
error('Inconsistent dimensions for input arguments') error('Inconsistent dimensions for input arguments')
elseif any(rem(d(~isnan(d)),1) ~= 0 | rem(m(~isnan(m)),1) ~= 0) elseif any(rem(d(~isnan(d)),1) ~= 0 | rem(m(~isnan(m)),1) ~= 0)
error('Degrees and minutes must be integers') error('Degrees and minutes must be integers')
end end
if any(abs(m) > 60) | any (abs(m) < 0) % Actually algorithm allows for if any(abs(m) > 60) | any (abs(m) < 0) % Actually algorithm allows for
error('Minutes must be >= 0 and < 60') % up to exactly 60 seconds or error('Minutes must be >= 0 and < 60') % up to exactly 60 seconds or
% 60 minutes, but the error message % 60 minutes, but the error message
elseif any(abs(s) > 60) | any(abs(s) < 0) % doesn't reflect this so that angst elseif any(abs(s) > 60) | any(abs(s) < 0) % doesn't reflect this so that angst
error('Seconds must be >= 0 and < 60') % is minimized in the user docs error('Seconds must be >= 0 and < 60') % is minimized in the user docs
end end
% Ensure that only one negative sign is present and at the correct location % Ensure that only one negative sign is present and at the correct location
if any((s<0 & m<0) | (s<0 & d<0) | (m<0 & d<0) ) if any((s<0 & m<0) | (s<0 & d<0) | (m<0 & d<0) )
error('Multiple negative entries in a DMS specification') error('Multiple negative entries in a DMS specification')
elseif any((s<0 & (m~=0 | d~= 0)) | (m<0 & d~=0)) elseif any((s<0 & (m~=0 | d~= 0)) | (m<0 & d~=0))
error('Incorrect negative DMS specification') error('Incorrect negative DMS specification')
end end
% Construct a sign vector which has +1 when % Construct a sign vector which has +1 when
% angle >= 0 and -1 when angle < 0. Note that the sign of the % angle >= 0 and -1 when angle < 0. Note that the sign of the
% angle is associated with the largest nonzero component of d:m:s % angle is associated with the largest nonzero component of d:m:s
negvec = (d<0) | (m<0) | (s<0); negvec = (d<0) | (m<0) | (s<0);
signvec = ~negvec - negvec; signvec = ~negvec - negvec;
% Convert to all positive numbers. Allows for easier % Convert to all positive numbers. Allows for easier
% adjusting at 60 seconds and 60 minutes % adjusting at 60 seconds and 60 minutes
d = abs(d); m = abs(m); s = abs(s); d = abs(d); m = abs(m); s = abs(s);
% Truncate seconds to a specified accuracy to eliminate round-off errors % Truncate seconds to a specified accuracy to eliminate round-off errors
[s,msg] = roundn(s,n); [s,msg] = roundn(s,n);
if ~isempty(msg); error(msg); end if ~isempty(msg); error(msg); end
% Adjust for 60 seconds or 60 minutes. If s > 60, this can only be % Adjust for 60 seconds or 60 minutes. If s > 60, this can only be
% from round-off during roundn since s > 60 is already tested above. % from round-off during roundn since s > 60 is already tested above.
% This round-off effect has happened though. % This round-off effect has happened though.
indx = find(s >= 60); indx = find(s >= 60);
if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = 0; end if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = 0; end
% The user can not put minutes > 60 as input. However, the line % The user can not put minutes > 60 as input. However, the line
% above may create minutes > 60 (since the user can put in m == 60), % above may create minutes > 60 (since the user can put in m == 60),
% thus, the test below includes the greater than condition. % thus, the test below includes the greater than condition.
indx = find(m >= 60); indx = find(m >= 60);
if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx)-60; end if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx)-60; end
% Construct the dms vector format % Construct the dms vector format
dmsvec = signvec .* (100*d + m + s/100); dmsvec = signvec .* (100*d + m + s/100);

92
src/utils/matlab/libs/geoFunctions/roundn.m
View File

@ -1,46 +1,46 @@
function [x,msg] = roundn(x,n) function [x,msg] = roundn(x,n)
% ROUNDN Rounds input data at specified power of 10 % ROUNDN Rounds input data at specified power of 10
% %
% y = ROUNDN(x) rounds the input data x to the nearest hundredth. % y = ROUNDN(x) rounds the input data x to the nearest hundredth.
% %
% y = ROUNDN(x,n) rounds the input data x at the specified power % y = ROUNDN(x,n) rounds the input data x at the specified power
% of tens position. For example, n = -2 rounds the input data to % of tens position. For example, n = -2 rounds the input data to
% the 10E-2 (hundredths) position. % the 10E-2 (hundredths) position.
% %
% [y,msg] = ROUNDN(...) returns the text of any error condition % [y,msg] = ROUNDN(...) returns the text of any error condition
% encountered in the output variable msg. % encountered in the output variable msg.
% %
% See also ROUND % See also ROUND
% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown % Written by: E. Byrns, E. Brown
% Revision: 1.9 Date: 2002/03/20 21:26:19 % Revision: 1.9 Date: 2002/03/20 21:26:19
msg = []; % Initialize output msg = []; % Initialize output
if nargin == 0 if nargin == 0
error('Incorrect number of arguments') error('Incorrect number of arguments')
elseif nargin == 1 elseif nargin == 1
n = -2; n = -2;
end end
% Test for scalar n % Test for scalar n
if max(size(n)) ~= 1 if max(size(n)) ~= 1
msg = 'Scalar accuracy required'; msg = 'Scalar accuracy required';
if nargout < 2; error(msg); end if nargout < 2; error(msg); end
return return
elseif ~isreal(n) elseif ~isreal(n)
warning('Imaginary part of complex N argument ignored') warning('Imaginary part of complex N argument ignored')
n = real(n); n = real(n);
end end
% Compute the exponential factors for rounding at specified % Compute the exponential factors for rounding at specified
% power of 10. Ensure that n is an integer. % power of 10. Ensure that n is an integer.
factors = 10 ^ (fix(-n)); factors = 10 ^ (fix(-n));
% Set the significant digits for the input data % Set the significant digits for the input data
x = round(x * factors) / factors; x = round(x * factors) / factors;

276
src/utils/matlab/libs/geoFunctions/satpos.m
View File

@ -1,138 +1,138 @@
function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ... function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ...
eph, settings) eph, settings)
% SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for % SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for
% given ephemeris EPH. Coordinates are computed for each satellite in the % given ephemeris EPH. Coordinates are computed for each satellite in the
% list PRNLIST. % list PRNLIST.
%[ satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); %[ satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings);
% %
% Inputs: % Inputs:
% transmitTime - transmission time % transmitTime - transmission time
% prnList - list of PRN-s to be processed % prnList - list of PRN-s to be processed
% eph - ephemerides of satellites % eph - ephemerides of satellites
% settings - receiver settings % settings - receiver settings
% %
% Outputs: % Outputs:
% satPositions - position of satellites (in ECEF system [X; Y; Z;]) % satPositions - position of satellites (in ECEF system [X; Y; Z;])
% satClkCorr - correction of satellite clocks % satClkCorr - correction of satellite clocks
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% Based on Kai Borre 04-09-96 % Based on Kai Borre 04-09-96
% Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
% Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen % Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen
%% Initialize constants =================================================== %% Initialize constants ===================================================
numOfSatellites = size(prnList, 2); numOfSatellites = size(prnList, 2);
% GPS constatns % GPS constatns
gpsPi = 3.1415926535898; % Pi used in the GPS coordinate gpsPi = 3.1415926535898; % Pi used in the GPS coordinate
% system % system
%--- Constants for satellite position calculation ------------------------- %--- Constants for satellite position calculation -------------------------
Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s] Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s]
GM = 3.986005e14; % Universal gravitational constant times GM = 3.986005e14; % Universal gravitational constant times
% the mass of the Earth, [m^3/s^2] % the mass of the Earth, [m^3/s^2]
F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)] F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)]
%% Initialize results ===================================================== %% Initialize results =====================================================
satClkCorr = zeros(1, numOfSatellites); satClkCorr = zeros(1, numOfSatellites);
satPositions = zeros(3, numOfSatellites); satPositions = zeros(3, numOfSatellites);
%% Process each satellite ================================================= %% Process each satellite =================================================
for satNr = 1 : numOfSatellites for satNr = 1 : numOfSatellites
prn = prnList(satNr); prn = prnList(satNr);
%% Find initial satellite clock correction -------------------------------- %% Find initial satellite clock correction --------------------------------
%--- Find time difference --------------------------------------------- %--- Find time difference ---------------------------------------------
dt = check_t(transmitTime - eph(prn).t_oc); dt = check_t(transmitTime - eph(prn).t_oc);
%--- Calculate clock correction --------------------------------------- %--- Calculate clock correction ---------------------------------------
satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ... eph(prn).a_f0 - ...
eph(prn).T_GD; eph(prn).T_GD;
time = transmitTime - satClkCorr(satNr); time = transmitTime - satClkCorr(satNr);
%% Find satellite's position ---------------------------------------------- %% Find satellite's position ----------------------------------------------
%Restore semi-major axis %Restore semi-major axis
a = eph(prn).sqrtA * eph(prn).sqrtA; a = eph(prn).sqrtA * eph(prn).sqrtA;
%Time correction %Time correction
tk = check_t(time - eph(prn).t_oe); tk = check_t(time - eph(prn).t_oe);
%Initial mean motion %Initial mean motion
n0 = sqrt(GM / a^3); n0 = sqrt(GM / a^3);
%Mean motion %Mean motion
n = n0 + eph(prn).deltan; n = n0 + eph(prn).deltan;
%Mean anomaly %Mean anomaly
M = eph(prn).M_0 + n * tk; M = eph(prn).M_0 + n * tk;
%Reduce mean anomaly to between 0 and 360 deg %Reduce mean anomaly to between 0 and 360 deg
M = rem(M + 2*gpsPi, 2*gpsPi); M = rem(M + 2*gpsPi, 2*gpsPi);
%Initial guess of eccentric anomaly %Initial guess of eccentric anomaly
E = M; E = M;
%--- Iteratively compute eccentric anomaly ---------------------------- %--- Iteratively compute eccentric anomaly ----------------------------
for ii = 1:10 for ii = 1:10
E_old = E; E_old = E;
E = M + eph(prn).e * sin(E); E = M + eph(prn).e * sin(E);
dE = rem(E - E_old, 2*gpsPi); dE = rem(E - E_old, 2*gpsPi);
if abs(dE) < 1.e-12 if abs(dE) < 1.e-12
% Necessary precision is reached, exit from the loop % Necessary precision is reached, exit from the loop
break; break;
end end
end end
%Reduce eccentric anomaly to between 0 and 360 deg %Reduce eccentric anomaly to between 0 and 360 deg
E = rem(E + 2*gpsPi, 2*gpsPi); E = rem(E + 2*gpsPi, 2*gpsPi);
%Compute relativistic correction term %Compute relativistic correction term
dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E); dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E);
%Calculate the true anomaly %Calculate the true anomaly
nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e); nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e);
%Compute angle phi %Compute angle phi
phi = nu + eph(prn).omega; phi = nu + eph(prn).omega;
%Reduce phi to between 0 and 360 deg %Reduce phi to between 0 and 360 deg
phi = rem(phi, 2*gpsPi); phi = rem(phi, 2*gpsPi);
%Correct argument of latitude %Correct argument of latitude
u = phi + ... u = phi + ...
eph(prn).C_uc * cos(2*phi) + ... eph(prn).C_uc * cos(2*phi) + ...
eph(prn).C_us * sin(2*phi); eph(prn).C_us * sin(2*phi);
%Correct radius %Correct radius
r = a * (1 - eph(prn).e*cos(E)) + ... r = a * (1 - eph(prn).e*cos(E)) + ...
eph(prn).C_rc * cos(2*phi) + ... eph(prn).C_rc * cos(2*phi) + ...
eph(prn).C_rs * sin(2*phi); eph(prn).C_rs * sin(2*phi);
%Correct inclination %Correct inclination
i = eph(prn).i_0 + eph(prn).iDot * tk + ... i = eph(prn).i_0 + eph(prn).iDot * tk + ...
eph(prn).C_ic * cos(2*phi) + ... eph(prn).C_ic * cos(2*phi) + ...
eph(prn).C_is * sin(2*phi); eph(prn).C_is * sin(2*phi);
%Compute the angle between the ascending node and the Greenwich meridian %Compute the angle between the ascending node and the Greenwich meridian
Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ... Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ...
Omegae_dot * eph(prn).t_oe; Omegae_dot * eph(prn).t_oe;
%Reduce to between 0 and 360 deg %Reduce to between 0 and 360 deg
Omega = rem(Omega + 2*gpsPi, 2*gpsPi); Omega = rem(Omega + 2*gpsPi, 2*gpsPi);
%--- Compute satellite coordinates ------------------------------------ %--- Compute satellite coordinates ------------------------------------
satPositions(1, satNr) = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega); satPositions(1, satNr) = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega);
satPositions(2, satNr) = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega); satPositions(2, satNr) = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega);
satPositions(3, satNr) = sin(u)*r * sin(i); satPositions(3, satNr) = sin(u)*r * sin(i);
%% Include relativistic correction in clock correction -------------------- %% Include relativistic correction in clock correction --------------------
satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ... eph(prn).a_f0 - ...
eph(prn).T_GD + dtr; eph(prn).T_GD + dtr;
end % for satNr = 1 : numOfSatellites end % for satNr = 1 : numOfSatellites

218
src/utils/matlab/libs/geoFunctions/togeod.m
View File

@ -1,109 +1,109 @@
function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z)
% TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, % TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude,
% height given Cartesian coordinates X,Y,Z, and reference ellipsoid % height given Cartesian coordinates X,Y,Z, and reference ellipsoid
% values semi-major axis (a) and the inverse of flattening (finv). % values semi-major axis (a) and the inverse of flattening (finv).
% %
% [dphi, dlambda, h] = togeod(a, finv, X, Y, Z); % [dphi, dlambda, h] = togeod(a, finv, X, Y, Z);
% %
% The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc) % The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
% The output units of angular quantities will be in decimal degrees % The output units of angular quantities will be in decimal degrees
% (15.5 degrees not 15 deg 30 min). The output units of h will be the % (15.5 degrees not 15 deg 30 min). The output units of h will be the
% same as the units of X,Y,Z,a. % same as the units of X,Y,Z,a.
% %
% Inputs: % Inputs:
% a - semi-major axis of the reference ellipsoid % a - semi-major axis of the reference ellipsoid
% finv - inverse of flattening of the reference ellipsoid % finv - inverse of flattening of the reference ellipsoid
% X,Y,Z - Cartesian coordinates % X,Y,Z - Cartesian coordinates
% %
% Outputs: % Outputs:
% dphi - latitude % dphi - latitude
% dlambda - longitude % dlambda - longitude
% h - height above reference ellipsoid % h - height above reference ellipsoid
% Copyright (C) 1987 C. Goad, Columbus, Ohio % Copyright (C) 1987 C. Goad, Columbus, Ohio
% Reprinted with permission of author, 1996 % Reprinted with permission of author, 1996
% Fortran code translated into MATLAB % Fortran code translated into MATLAB
% Kai Borre 03-30-96 % Kai Borre 03-30-96
%========================================================================== %==========================================================================
h = 0; h = 0;
tolsq = 1.e-10; tolsq = 1.e-10;
maxit = 10; maxit = 10;
% compute radians-to-degree factor % compute radians-to-degree factor
rtd = 180/pi; rtd = 180/pi;
% compute square of eccentricity % compute square of eccentricity
if finv < 1.e-20 if finv < 1.e-20
esq = 0; esq = 0;
else else
esq = (2 - 1/finv) / finv; esq = (2 - 1/finv) / finv;
end end
oneesq = 1 - esq; oneesq = 1 - esq;
% first guess % first guess
% P is distance from spin axis % P is distance from spin axis
P = sqrt(X^2+Y^2); P = sqrt(X^2+Y^2);
% direct calculation of longitude % direct calculation of longitude
if P > 1.e-20 if P > 1.e-20
dlambda = atan2(Y,X) * rtd; dlambda = atan2(Y,X) * rtd;
else else
dlambda = 0; dlambda = 0;
end end
if (dlambda < 0) if (dlambda < 0)
dlambda = dlambda + 360; dlambda = dlambda + 360;
end end
% r is distance from origin (0,0,0) % r is distance from origin (0,0,0)
r = sqrt(P^2 + Z^2); r = sqrt(P^2 + Z^2);
if r > 1.e-20 if r > 1.e-20
sinphi = Z/r; sinphi = Z/r;
else else
sinphi = 0; sinphi = 0;
end end
dphi = asin(sinphi); dphi = asin(sinphi);
% initial value of height = distance from origin minus % initial value of height = distance from origin minus
% approximate distance from origin to surface of ellipsoid % approximate distance from origin to surface of ellipsoid
if r < 1.e-20 if r < 1.e-20
h = 0; h = 0;
return return
end end
h = r - a*(1-sinphi*sinphi/finv); h = r - a*(1-sinphi*sinphi/finv);
% iterate % iterate
for i = 1:maxit for i = 1:maxit
sinphi = sin(dphi); sinphi = sin(dphi);
cosphi = cos(dphi); cosphi = cos(dphi);
% compute radius of curvature in prime vertical direction % compute radius of curvature in prime vertical direction
N_phi = a/sqrt(1-esq*sinphi*sinphi); N_phi = a/sqrt(1-esq*sinphi*sinphi);
% compute residuals in P and Z % compute residuals in P and Z
dP = P - (N_phi + h) * cosphi; dP = P - (N_phi + h) * cosphi;
dZ = Z - (N_phi*oneesq + h) * sinphi; dZ = Z - (N_phi*oneesq + h) * sinphi;
% update height and latitude % update height and latitude
h = h + (sinphi*dZ + cosphi*dP); h = h + (sinphi*dZ + cosphi*dP);
dphi = dphi + (cosphi*dZ - sinphi*dP)/(N_phi + h); dphi = dphi + (cosphi*dZ - sinphi*dP)/(N_phi + h);
% test for convergence % test for convergence
if (dP*dP + dZ*dZ < tolsq) if (dP*dP + dZ*dZ < tolsq)
break; break;
end end
% Not Converged--Warn user % Not Converged--Warn user
if i == maxit if i == maxit
fprintf([' Problem in TOGEOD, did not converge in %2.0f',... fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
' iterations\n'], i); ' iterations\n'], i);
end end
end % for i = 1:maxit end % for i = 1:maxit
dphi = dphi * rtd; dphi = dphi * rtd;
%%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%% %%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%%

108
src/utils/matlab/libs/geoFunctions/topocent.m
View File

@ -1,54 +1,54 @@
function [Az, El, D] = topocent(X, dx) function [Az, El, D] = topocent(X, dx)
% TOPOCENT Transformation of vector dx into topocentric coordinate % TOPOCENT Transformation of vector dx into topocentric coordinate
% system with origin at X. % system with origin at X.
% Both parameters are 3 by 1 vectors. % Both parameters are 3 by 1 vectors.
% %
% [Az, El, D] = topocent(X, dx); % [Az, El, D] = topocent(X, dx);
% %
% Inputs: % Inputs:
% X - vector origin corrdinates (in ECEF system [X; Y; Z;]) % X - vector origin corrdinates (in ECEF system [X; Y; Z;])
% dx - vector ([dX; dY; dZ;]). % dx - vector ([dX; dY; dZ;]).
% %
% Outputs: % Outputs:
% D - vector length. Units like units of the input % D - vector length. Units like units of the input
% Az - azimuth from north positive clockwise, degrees % Az - azimuth from north positive clockwise, degrees
% El - elevation angle, degrees % El - elevation angle, degrees
% Kai Borre 11-24-96 % Kai Borre 11-24-96
% Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
%========================================================================== %==========================================================================
dtr = pi/180; dtr = pi/180;
[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3)); [phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
cl = cos(lambda * dtr); cl = cos(lambda * dtr);
sl = sin(lambda * dtr); sl = sin(lambda * dtr);
cb = cos(phi * dtr); cb = cos(phi * dtr);
sb = sin(phi * dtr); sb = sin(phi * dtr);
F = [-sl -sb*cl cb*cl; F = [-sl -sb*cl cb*cl;
cl -sb*sl cb*sl; cl -sb*sl cb*sl;
0 cb sb]; 0 cb sb];
local_vector = F' * dx; local_vector = F' * dx;
E = local_vector(1); E = local_vector(1);
N = local_vector(2); N = local_vector(2);
U = local_vector(3); U = local_vector(3);
hor_dis = sqrt(E^2 + N^2); hor_dis = sqrt(E^2 + N^2);
if hor_dis < 1.e-20 if hor_dis < 1.e-20
Az = 0; Az = 0;
El = 90; El = 90;
else else
Az = atan2(E, N)/dtr; Az = atan2(E, N)/dtr;
El = atan2(U, hor_dis)/dtr; El = atan2(U, hor_dis)/dtr;
end end
if Az < 0 if Az < 0
Az = Az + 360; Az = Az + 360;
end end
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2); D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%% %%%%%%%%% end topocent.m %%%%%%%%%

190
src/utils/matlab/libs/geoFunctions/tropo.m
View File

@ -1,95 +1,95 @@
function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum) function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum)
% TROPO Calculation of tropospheric correction. % TROPO Calculation of tropospheric correction.
% The range correction ddr in m is to be subtracted from % The range correction ddr in m is to be subtracted from
% pseudo-ranges and carrier phases % pseudo-ranges and carrier phases
% %
% ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); % ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum);
% %
% Inputs: % Inputs:
% sinel - sin of elevation angle of satellite % sinel - sin of elevation angle of satellite
% hsta - height of station in km % hsta - height of station in km
% p - atmospheric pressure in mb at height hp % p - atmospheric pressure in mb at height hp
% tkel - surface temperature in degrees Kelvin at height htkel % tkel - surface temperature in degrees Kelvin at height htkel
% hum - humidity in % at height hhum % hum - humidity in % at height hhum
% hp - height of pressure measurement in km % hp - height of pressure measurement in km
% htkel - height of temperature measurement in km % htkel - height of temperature measurement in km
% hhum - height of humidity measurement in km % hhum - height of humidity measurement in km
% %
% Outputs: % Outputs:
% ddr - range correction (meters) % ddr - range correction (meters)
% %
% Reference % Reference
% Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric % Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric
% Refraction Correction Model. Paper presented at the % Refraction Correction Model. Paper presented at the
% American Geophysical Union Annual Fall Meeting, San % American Geophysical Union Annual Fall Meeting, San
% Francisco, December 12-17 % Francisco, December 12-17
% A Matlab reimplementation of a C code from driver. % A Matlab reimplementation of a C code from driver.
% Kai Borre 06-28-95 % Kai Borre 06-28-95
%========================================================================== %==========================================================================
a_e = 6378.137; % semi-major axis of earth ellipsoid a_e = 6378.137; % semi-major axis of earth ellipsoid
b0 = 7.839257e-5; b0 = 7.839257e-5;
tlapse = -6.5; tlapse = -6.5;
tkhum = tkel + tlapse*(hhum-htkel); tkhum = tkel + tlapse*(hhum-htkel);
atkel = 7.5*(tkhum-273.15) / (237.3+tkhum-273.15); atkel = 7.5*(tkhum-273.15) / (237.3+tkhum-273.15);
e0 = 0.0611 * hum * 10^atkel; e0 = 0.0611 * hum * 10^atkel;
tksea = tkel - tlapse*htkel; tksea = tkel - tlapse*htkel;
em = -978.77 / (2.8704e6*tlapse*1.0e-5); em = -978.77 / (2.8704e6*tlapse*1.0e-5);
tkelh = tksea + tlapse*hhum; tkelh = tksea + tlapse*hhum;
e0sea = e0 * (tksea/tkelh)^(4*em); e0sea = e0 * (tksea/tkelh)^(4*em);
tkelp = tksea + tlapse*hp; tkelp = tksea + tlapse*hp;
psea = p * (tksea/tkelp)^em; psea = p * (tksea/tkelp)^em;
if sinel < 0 if sinel < 0
sinel = 0; sinel = 0;
end end
tropo = 0; tropo = 0;
done = 'FALSE'; done = 'FALSE';
refsea = 77.624e-6 / tksea; refsea = 77.624e-6 / tksea;
htop = 1.1385e-5 / refsea; htop = 1.1385e-5 / refsea;
refsea = refsea * psea; refsea = refsea * psea;
ref = refsea * ((htop-hsta)/htop)^4; ref = refsea * ((htop-hsta)/htop)^4;
while 1 while 1
rtop = (a_e+htop)^2 - (a_e+hsta)^2*(1-sinel^2); rtop = (a_e+htop)^2 - (a_e+hsta)^2*(1-sinel^2);
% check to see if geometry is crazy % check to see if geometry is crazy
if rtop < 0 if rtop < 0
rtop = 0; rtop = 0;
end end
rtop = sqrt(rtop) - (a_e+hsta)*sinel; rtop = sqrt(rtop) - (a_e+hsta)*sinel;
a = -sinel/(htop-hsta); a = -sinel/(htop-hsta);
b = -b0*(1-sinel^2) / (htop-hsta); b = -b0*(1-sinel^2) / (htop-hsta);
rn = zeros(8,1); rn = zeros(8,1);
for i = 1:8 for i = 1:8
rn(i) = rtop^(i+1); rn(i) = rtop^(i+1);
end end
alpha = [2*a, 2*a^2+4*b/3, a*(a^2+3*b),... alpha = [2*a, 2*a^2+4*b/3, a*(a^2+3*b),...
a^4/5+2.4*a^2*b+1.2*b^2, 2*a*b*(a^2+3*b)/3,... a^4/5+2.4*a^2*b+1.2*b^2, 2*a*b*(a^2+3*b)/3,...
b^2*(6*a^2+4*b)*1.428571e-1, 0, 0]; b^2*(6*a^2+4*b)*1.428571e-1, 0, 0];
if b^2 > 1.0e-35 if b^2 > 1.0e-35
alpha(7) = a*b^3/2; alpha(7) = a*b^3/2;
alpha(8) = b^4/9; alpha(8) = b^4/9;
end end
dr = rtop; dr = rtop;
dr = dr + alpha*rn; dr = dr + alpha*rn;
tropo = tropo + dr*ref*1000; tropo = tropo + dr*ref*1000;
if done == 'TRUE ' if done == 'TRUE '
ddr = tropo; ddr = tropo;
break; break;
end end
done = 'TRUE '; done = 'TRUE ';
refsea = (371900.0e-6/tksea-12.92e-6)/tksea; refsea = (371900.0e-6/tksea-12.92e-6)/tksea;
htop = 1.1385e-5 * (1255/tksea+0.05)/refsea; htop = 1.1385e-5 * (1255/tksea+0.05)/refsea;
ref = refsea * e0sea * ((htop-hsta)/htop)^4; ref = refsea * e0sea * ((htop-hsta)/htop)^4;
end; end;
%%%%%%%%% end tropo.m %%%%%%%%%%%%%%%%%%% %%%%%%%%% end tropo.m %%%%%%%%%%%%%%%%%%%

332
src/utils/matlab/libs/plotNavigation.m
View File

@ -1,166 +1,166 @@
% Function plots variations of coordinates over time and a 3D position % Function plots variations of coordinates over time and a 3D position
% plot. It plots receiver coordinates in UTM system or coordinate offsets if % plot. It plots receiver coordinates in UTM system or coordinate offsets if
% the true UTM receiver coordinates are provided. % the true UTM receiver coordinates are provided.
% %
% plotNavigation(navSolutions, settings) % plotNavigation(navSolutions, settings)
% %
% Inputs: % Inputs:
% navSolutions - Results from navigation solution function. It % navSolutions - Results from navigation solution function. It
% contains measured pseudoranges and receiver % contains measured pseudoranges and receiver
% coordinates. % coordinates.
% settings - Receiver settings. The true receiver coordinates % settings - Receiver settings. The true receiver coordinates
% are contained in this structure. % are contained in this structure.
% plot_skyplot - If ==1 then use satellite coordinates to plot the % plot_skyplot - If ==1 then use satellite coordinates to plot the
% the satellite positions % the satellite positions
% Darius Plausinaitis % Darius Plausinaitis
% Modified by Javier Arribas, 2011. jarribas(at)cttc.es % Modified by Javier Arribas, 2011. jarribas(at)cttc.es
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) % Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
% %
% GNSS-SDR is a software defined Global Navigation % GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver % Satellite Systems receiver
% %
% This file is part of GNSS-SDR. % This file is part of GNSS-SDR.
% %
% GNSS-SDR is free software: you can redistribute it and/or modify % GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by % it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or % the Free Software Foundation, either version 3 of the License, or
% at your option) any later version. % at your option) any later version.
% %
% GNSS-SDR is distributed in the hope that it will be useful, % GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of % but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details. % GNU General Public License for more details.
% %
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>. % along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% %
% ------------------------------------------------------------------------- % -------------------------------------------------------------------------
% %
function plotNavigation(navSolutions, settings,plot_skyplot) function plotNavigation(navSolutions, settings,plot_skyplot)
%% Plot results in the necessary data exists ============================== %% Plot results in the necessary data exists ==============================
if (~isempty(navSolutions)) if (~isempty(navSolutions))
%% If reference position is not provided, then set reference position %% If reference position is not provided, then set reference position
%% to the average postion %% to the average postion
if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ... if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ...
|| isnan(settings.truePosition.U) || isnan(settings.truePosition.U)
%=== Compute mean values ========================================== %=== Compute mean values ==========================================
% Remove NaN-s or the output of the function MEAN will be NaN. % Remove NaN-s or the output of the function MEAN will be NaN.
refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E))); refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E)));
refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N))); refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N)));
refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U))); refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U)));
%Also convert geodetic coordinates to deg:min:sec vector format %Also convert geodetic coordinates to deg:min:sec vector format
meanLongitude = dms2mat(deg2dms(... meanLongitude = dms2mat(deg2dms(...
mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5); mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5);
meanLatitude = dms2mat(deg2dms(... meanLatitude = dms2mat(deg2dms(...
mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5); mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5);
LatLong_str=[num2str(meanLatitude(1)), '??', ... LatLong_str=[num2str(meanLatitude(1)), '??', ...
num2str(meanLatitude(2)), '''', ... num2str(meanLatitude(2)), '''', ...
num2str(meanLatitude(3)), '''''', ... num2str(meanLatitude(3)), '''''', ...
',', ... ',', ...
num2str(meanLongitude(1)), '??', ... num2str(meanLongitude(1)), '??', ...
num2str(meanLongitude(2)), '''', ... num2str(meanLongitude(2)), '''', ...
num2str(meanLongitude(3)), ''''''] num2str(meanLongitude(3)), '''''']
refPointLgText = ['Mean Position\newline Lat: ', ... refPointLgText = ['Mean Position\newline Lat: ', ...
num2str(meanLatitude(1)), '{\circ}', ... num2str(meanLatitude(1)), '{\circ}', ...
num2str(meanLatitude(2)), '{\prime}', ... num2str(meanLatitude(2)), '{\prime}', ...
num2str(meanLatitude(3)), '{\prime}{\prime}', ... num2str(meanLatitude(3)), '{\prime}{\prime}', ...
'\newline Lng: ', ... '\newline Lng: ', ...
num2str(meanLongitude(1)), '{\circ}', ... num2str(meanLongitude(1)), '{\circ}', ...
num2str(meanLongitude(2)), '{\prime}', ... num2str(meanLongitude(2)), '{\prime}', ...
num2str(meanLongitude(3)), '{\prime}{\prime}', ... num2str(meanLongitude(3)), '{\prime}{\prime}', ...
'\newline Hgt: ', ... '\newline Hgt: ', ...
num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')]; num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')];
else else
% compute the mean error for static receiver % compute the mean error for static receiver
mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E))); mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E)));
mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N))); mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N)));
mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U))); mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U)));
refCoord.E = settings.truePosition.E; refCoord.E = settings.truePosition.E;
refCoord.N = settings.truePosition.N; refCoord.N = settings.truePosition.N;
refCoord.U = settings.truePosition.U; 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); 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]']; refPointLgText = ['Reference Position, Mean 3D error = ' num2str(error_meters) ' [m]'];
end end
figureNumber = 300; figureNumber = 300;
% The 300 is chosen for more convenient handling of the open % The 300 is chosen for more convenient handling of the open
% figure windows, when many figures are closed and reopened. Figures % figure windows, when many figures are closed and reopened. Figures
% drawn or opened by the user, will not be "overwritten" by this % drawn or opened by the user, will not be "overwritten" by this
% function if the auto numbering is not used. % function if the auto numbering is not used.
%=== Select (or create) and clear the figure ========================== %=== Select (or create) and clear the figure ==========================
figure(figureNumber); figure(figureNumber);
clf (figureNumber); clf (figureNumber);
set (figureNumber, 'Name', 'Navigation solutions'); set (figureNumber, 'Name', 'Navigation solutions');
%--- Draw axes -------------------------------------------------------- %--- Draw axes --------------------------------------------------------
handles(1, 1) = subplot(4, 2, 1 : 4); handles(1, 1) = subplot(4, 2, 1 : 4);
handles(3, 1) = subplot(4, 2, [5, 7]); handles(3, 1) = subplot(4, 2, [5, 7]);
handles(3, 2) = subplot(4, 2, [6, 8]); handles(3, 2) = subplot(4, 2, [6, 8]);
%% Plot all figures ======================================================= %% Plot all figures =======================================================
%--- Coordinate differences in UTM system ----------------------------- %--- Coordinate differences in UTM system -----------------------------
plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ... plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ...
(navSolutions.N - refCoord.N)',... (navSolutions.N - refCoord.N)',...
(navSolutions.U - refCoord.U)']); (navSolutions.U - refCoord.U)']);
title (handles(1, 1), 'Coordinates variations in UTM system'); title (handles(1, 1), 'Coordinates variations in UTM system');
legend(handles(1, 1), 'E', 'N', 'U'); legend(handles(1, 1), 'E', 'N', 'U');
xlabel(handles(1, 1), ['Measurement period: ', ... xlabel(handles(1, 1), ['Measurement period: ', ...
num2str(settings.navSolPeriod), 'ms']); num2str(settings.navSolPeriod), 'ms']);
ylabel(handles(1, 1), 'Variations (m)'); ylabel(handles(1, 1), 'Variations (m)');
grid (handles(1, 1)); grid (handles(1, 1));
axis (handles(1, 1), 'tight'); axis (handles(1, 1), 'tight');
%--- Position plot in UTM system -------------------------------------- %--- Position plot in UTM system --------------------------------------
plot3 (handles(3, 1), navSolutions.E - refCoord.E, ... plot3 (handles(3, 1), navSolutions.E - refCoord.E, ...
navSolutions.N - refCoord.N, ... navSolutions.N - refCoord.N, ...
navSolutions.U - refCoord.U, '+'); navSolutions.U - refCoord.U, '+');
hold (handles(3, 1), 'on'); hold (handles(3, 1), 'on');
%Plot the reference point %Plot the reference point
plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10); plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10);
hold (handles(3, 1), 'off'); hold (handles(3, 1), 'off');
view (handles(3, 1), 0, 90); view (handles(3, 1), 0, 90);
axis (handles(3, 1), 'equal'); axis (handles(3, 1), 'equal');
grid (handles(3, 1), 'minor'); grid (handles(3, 1), 'minor');
legend(handles(3, 1), 'Measurements', refPointLgText); legend(handles(3, 1), 'Measurements', refPointLgText);
title (handles(3, 1), 'Positions in UTM system (3D plot)'); title (handles(3, 1), 'Positions in UTM system (3D plot)');
xlabel(handles(3, 1), 'East (m)'); xlabel(handles(3, 1), 'East (m)');
ylabel(handles(3, 1), 'North (m)'); ylabel(handles(3, 1), 'North (m)');
zlabel(handles(3, 1), 'Upping (m)'); zlabel(handles(3, 1), 'Upping (m)');
if (plot_skyplot==1) if (plot_skyplot==1)
%--- Satellite sky plot ----------------------------------------------- %--- Satellite sky plot -----------------------------------------------
skyPlot(handles(3, 2), ... skyPlot(handles(3, 2), ...
navSolutions.channel.az, ... navSolutions.channel.az, ...
navSolutions.channel.el, ... navSolutions.channel.el, ...
navSolutions.channel.PRN(:, 1)); navSolutions.channel.PRN(:, 1));
title (handles(3, 2), ['Sky plot (mean PDOP: ', ... title (handles(3, 2), ['Sky plot (mean PDOP: ', ...
num2str(mean(navSolutions.DOP(2,:))), ')']); num2str(mean(navSolutions.DOP(2,:))), ')']);
end end
else else
disp('plotNavigation: No navigation data to plot.'); disp('plotNavigation: No navigation data to plot.');
end % if (~isempty(navSolutions)) end % if (~isempty(navSolutions))

374
src/utils/matlab/libs/plotTracking.m
View File

@ -1,187 +1,187 @@
function plotTracking(channelList, trackResults, settings) function plotTracking(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list. % This function plots the tracking results for the given channel list.
% %
% plotTracking(channelList, trackResults, settings) % plotTracking(channelList, trackResults, settings)
% %
% Inputs: % Inputs:
% channelList - list of channels to be plotted. % channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function. % trackResults - tracking results from the tracking function.
% settings - receiver settings. % settings - receiver settings.
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
% %
% Copyright (C) Darius Plausinaitis % Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis % Written by Darius Plausinaitis
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
%This program is free software; you can redistribute it and/or %This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License %modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2 %as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version. %of the License, or (at your option) any later version.
% %
%This program is distributed in the hope that it will be useful, %This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of %but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the %MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
%GNU General Public License for more details. %GNU General Public License for more details.
% %
%You should have received a copy of the GNU General Public License %You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software %along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, %Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA. %USA.
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers % Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels); channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ============================================== %=== For all listed channels ==============================================
for channelNr = channelList for channelNr = channelList
%% Select (or create) and clear the figure ================================ %% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open % The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened. % figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by % Figures drawn or opened by the user, will not be "overwritten" by
% this function. % this function.
figure(channelNr +200); figure(channelNr +200);
clf(channelNr +200); clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ... ' (PRN ', ...
num2str(trackResults(channelNr).PRN(end-1)), ... num2str(trackResults(channelNr).PRN(end-1)), ...
') results']); ') results']);
%% Draw axes ============================================================== %% Draw axes ==============================================================
% Row 1 % Row 1
handles(1, 1) = subplot(4, 3, 1); handles(1, 1) = subplot(4, 3, 1);
handles(1, 2) = subplot(4, 3, [2 3]); handles(1, 2) = subplot(4, 3, [2 3]);
% Row 2 % Row 2
handles(2, 1) = subplot(4, 3, 4); handles(2, 1) = subplot(4, 3, 4);
handles(2, 2) = subplot(4, 3, [5 6]); handles(2, 2) = subplot(4, 3, [5 6]);
% Row 3 % Row 3
handles(3, 1) = subplot(4, 3, 7); handles(3, 1) = subplot(4, 3, 7);
handles(3, 2) = subplot(4, 3, 8); handles(3, 2) = subplot(4, 3, 8);
handles(3, 3) = subplot(4, 3, 9); handles(3, 3) = subplot(4, 3, 9);
% Row 4 % Row 4
handles(4, 1) = subplot(4, 3, 10); handles(4, 1) = subplot(4, 3, 10);
handles(4, 2) = subplot(4, 3, 11); handles(4, 2) = subplot(4, 3, 11);
handles(4, 3) = subplot(4, 3, 12); handles(4, 3) = subplot(4, 3, 12);
%% Plot all figures ======================================================= %% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess)/1000; timeAxisInSeconds = (1:settings.msToProcess)/1000;
%----- Discrete-Time Scatter Plot --------------------------------- %----- Discrete-Time Scatter Plot ---------------------------------
plot(handles(1, 1), trackResults(channelNr).I_P,... plot(handles(1, 1), trackResults(channelNr).I_P,...
trackResults(channelNr).Q_P, ... trackResults(channelNr).Q_P, ...
'.'); '.');
grid (handles(1, 1)); grid (handles(1, 1));
axis (handles(1, 1), 'equal'); axis (handles(1, 1), 'equal');
title (handles(1, 1), 'Discrete-Time Scatter Plot'); title (handles(1, 1), 'Discrete-Time Scatter Plot');
xlabel(handles(1, 1), 'I prompt'); xlabel(handles(1, 1), 'I prompt');
ylabel(handles(1, 1), 'Q prompt'); ylabel(handles(1, 1), 'Q prompt');
%----- Nav bits --------------------------------------------------- %----- Nav bits ---------------------------------------------------
plot (handles(1, 2), timeAxisInSeconds, ... plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).I_P); trackResults(channelNr).I_P);
grid (handles(1, 2)); grid (handles(1, 2));
title (handles(1, 2), 'Bits of the navigation message'); title (handles(1, 2), 'Bits of the navigation message');
xlabel(handles(1, 2), 'Time (s)'); xlabel(handles(1, 2), 'Time (s)');
axis (handles(1, 2), 'tight'); axis (handles(1, 2), 'tight');
%----- PLL discriminator unfiltered-------------------------------- %----- PLL discriminator unfiltered--------------------------------
plot (handles(2, 1), timeAxisInSeconds, ... plot (handles(2, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscr, 'r'); trackResults(channelNr).pllDiscr, 'r');
grid (handles(2, 1)); grid (handles(2, 1));
axis (handles(2, 1), 'tight'); axis (handles(2, 1), 'tight');
xlabel(handles(2, 1), 'Time (s)'); xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Amplitude'); ylabel(handles(2, 1), 'Amplitude');
title (handles(2, 1), 'Raw PLL discriminator'); title (handles(2, 1), 'Raw PLL discriminator');
%----- Correlation ------------------------------------------------ %----- Correlation ------------------------------------------------
plot(handles(2, 2), timeAxisInSeconds, ... plot(handles(2, 2), timeAxisInSeconds, ...
[sqrt(trackResults(channelNr).I_E.^2 + ... [sqrt(trackResults(channelNr).I_E.^2 + ...
trackResults(channelNr).Q_E.^2)', ... trackResults(channelNr).Q_E.^2)', ...
sqrt(trackResults(channelNr).I_P.^2 + ... sqrt(trackResults(channelNr).I_P.^2 + ...
trackResults(channelNr).Q_P.^2)', ... trackResults(channelNr).Q_P.^2)', ...
sqrt(trackResults(channelNr).I_L.^2 + ... sqrt(trackResults(channelNr).I_L.^2 + ...
trackResults(channelNr).Q_L.^2)'], ... trackResults(channelNr).Q_L.^2)'], ...
'-*'); '-*');
grid (handles(2, 2)); grid (handles(2, 2));
title (handles(2, 2), 'Correlation results'); title (handles(2, 2), 'Correlation results');
xlabel(handles(2, 2), 'Time (s)'); xlabel(handles(2, 2), 'Time (s)');
axis (handles(2, 2), 'tight'); axis (handles(2, 2), 'tight');
hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ...
'$\sqrt{I_{P}^2 + Q_{P}^2}$', ... '$\sqrt{I_{P}^2 + Q_{P}^2}$', ...
'$\sqrt{I_{L}^2 + Q_{L}^2}$'); '$\sqrt{I_{L}^2 + Q_{L}^2}$');
%set interpreter from tex to latex. This will draw \sqrt correctly %set interpreter from tex to latex. This will draw \sqrt correctly
set(hLegend, 'Interpreter', 'Latex'); set(hLegend, 'Interpreter', 'Latex');
%----- PLL discriminator filtered---------------------------------- %----- PLL discriminator filtered----------------------------------
plot (handles(3, 1), timeAxisInSeconds, ... plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess), 'b'); trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess), 'b');
grid (handles(3, 1)); grid (handles(3, 1));
axis (handles(3, 1), 'tight'); axis (handles(3, 1), 'tight');
xlabel(handles(3, 1), 'Time (s)'); xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude'); ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Filtered PLL discriminator'); title (handles(3, 1), 'Filtered PLL discriminator');
%----- DLL discriminator unfiltered-------------------------------- %----- DLL discriminator unfiltered--------------------------------
plot (handles(3, 2), timeAxisInSeconds, ... plot (handles(3, 2), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscr, 'r'); trackResults(channelNr).dllDiscr, 'r');
grid (handles(3, 2)); grid (handles(3, 2));
axis (handles(3, 2), 'tight'); axis (handles(3, 2), 'tight');
xlabel(handles(3, 2), 'Time (s)'); xlabel(handles(3, 2), 'Time (s)');
ylabel(handles(3, 2), 'Amplitude'); ylabel(handles(3, 2), 'Amplitude');
title (handles(3, 2), 'Raw DLL discriminator'); title (handles(3, 2), 'Raw DLL discriminator');
%----- DLL discriminator filtered---------------------------------- %----- DLL discriminator filtered----------------------------------
plot (handles(3, 3), timeAxisInSeconds, ... plot (handles(3, 3), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscrFilt, 'b'); trackResults(channelNr).dllDiscrFilt, 'b');
grid (handles(3, 3)); grid (handles(3, 3));
axis (handles(3, 3), 'tight'); axis (handles(3, 3), 'tight');
xlabel(handles(3, 3), 'Time (s)'); xlabel(handles(3, 3), 'Time (s)');
ylabel(handles(3, 3), 'Amplitude'); ylabel(handles(3, 3), 'Amplitude');
title (handles(3, 3), 'Filtered DLL discriminator'); title (handles(3, 3), 'Filtered DLL discriminator');
%----- CNo for signal---------------------------------- %----- CNo for signal----------------------------------
plot (handles(4, 1), timeAxisInSeconds, ... plot (handles(4, 1), timeAxisInSeconds, ...
trackResults(channelNr).CNo(1:settings.msToProcess), 'b'); trackResults(channelNr).CNo(1:settings.msToProcess), 'b');
grid (handles(4, 1)); grid (handles(4, 1));
axis (handles(4, 1), 'tight'); axis (handles(4, 1), 'tight');
xlabel(handles(4, 1), 'Time (s)'); xlabel(handles(4, 1), 'Time (s)');
ylabel(handles(4, 1), 'CNo (dB-Hz)'); ylabel(handles(4, 1), 'CNo (dB-Hz)');
title (handles(4, 1), 'Carrier to Noise Ratio'); title (handles(4, 1), 'Carrier to Noise Ratio');
%----- Carrier Frequency -------------------------------- %----- Carrier Frequency --------------------------------
plot (handles(4, 2), timeAxisInSeconds(2:end), ... plot (handles(4, 2), timeAxisInSeconds(2:end), ...
trackResults(channelNr).carrFreq(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]); trackResults(channelNr).carrFreq(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(4, 2)); grid (handles(4, 2));
axis (handles(4, 2)); axis (handles(4, 2));
xlabel(handles(4, 2), 'Time (s)'); xlabel(handles(4, 2), 'Time (s)');
ylabel(handles(4, 2), 'Freq (hz)'); ylabel(handles(4, 2), 'Freq (hz)');
title (handles(4, 2), 'Carrier Freq'); title (handles(4, 2), 'Carrier Freq');
%----- Code Frequency---------------------------------- %----- Code Frequency----------------------------------
%--- Skip sample 0 to help with results display %--- Skip sample 0 to help with results display
plot (handles(4, 3), timeAxisInSeconds(2:end), ... plot (handles(4, 3), timeAxisInSeconds(2:end), ...
trackResults(channelNr).codeFreq(2:settings.msToProcess), 'Color',[0.2 0.3 0.49]); trackResults(channelNr).codeFreq(2:settings.msToProcess), 'Color',[0.2 0.3 0.49]);
grid (handles(4, 3)); grid (handles(4, 3));
axis (handles(4, 3), 'tight'); axis (handles(4, 3), 'tight');
xlabel(handles(4, 3), 'Time (s)'); xlabel(handles(4, 3), 'Time (s)');
ylabel(handles(4, 3), 'Freq (Hz)'); ylabel(handles(4, 3), 'Freq (Hz)');
title (handles(4, 3), 'Code Freq'); title (handles(4, 3), 'Code Freq');
end % for channelNr = channelList end % for channelNr = channelList

324
src/utils/matlab/libs/plotVEMLTracking.m
View File

@ -1,162 +1,162 @@
function plotVEMLTracking(channelList, trackResults, settings) function plotVEMLTracking(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list. % This function plots the tracking results for the given channel list.
% %
% plotTracking(channelList, trackResults, settings) % plotTracking(channelList, trackResults, settings)
% %
% Inputs: % Inputs:
% channelList - list of channels to be plotted. % channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function. % trackResults - tracking results from the tracking function.
% settings - receiver settings. % settings - receiver settings.
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
% %
% Copyright (C) Darius Plausinaitis % Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis % Written by Darius Plausinaitis
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
%This program is free software; you can redistribute it and/or %This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License %modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2 %as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version. %of the License, or (at your option) any later version.
% %
%This program is distributed in the hope that it will be useful, %This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of %but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the %MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
%GNU General Public License for more details. %GNU General Public License for more details.
% %
%You should have received a copy of the GNU General Public License %You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software %along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, %Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA. %USA.
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers % Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels); channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ============================================== %=== For all listed channels ==============================================
for channelNr = channelList for channelNr = channelList
%% Select (or create) and clear the figure ================================ %% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open % The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened. % figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by % Figures drawn or opened by the user, will not be "overwritten" by
% this function. % this function.
figure(channelNr +200); figure(channelNr +200);
clf(channelNr +200); clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ... ' (PRN ', ...
num2str(trackResults(channelNr).PRN(end-1)), ... num2str(trackResults(channelNr).PRN(end-1)), ...
') results']); ') results']);
%% Draw axes ============================================================== %% Draw axes ==============================================================
% Row 1 % Row 1
handles(1, 1) = subplot(3, 3, 1); handles(1, 1) = subplot(3, 3, 1);
handles(1, 2) = subplot(3, 3, [2 3]); handles(1, 2) = subplot(3, 3, [2 3]);
% Row 2 % Row 2
handles(2, 1) = subplot(3, 3, 4); handles(2, 1) = subplot(3, 3, 4);
handles(2, 2) = subplot(3, 3, [5 6]); handles(2, 2) = subplot(3, 3, [5 6]);
% Row 3 % Row 3
handles(3, 1) = subplot(3, 3, 7); handles(3, 1) = subplot(3, 3, 7);
handles(3, 2) = subplot(3, 3, 8); handles(3, 2) = subplot(3, 3, 8);
handles(3, 3) = subplot(3, 3, 9); handles(3, 3) = subplot(3, 3, 9);
%% Plot all figures ======================================================= %% Plot all figures =======================================================
timeAxisInSeconds = (1:4:settings.msToProcess)/1000; timeAxisInSeconds = (1:4:settings.msToProcess)/1000;
%----- Discrete-Time Scatter Plot --------------------------------- %----- Discrete-Time Scatter Plot ---------------------------------
plot(handles(1, 1), trackResults(channelNr).data_I,... plot(handles(1, 1), trackResults(channelNr).data_I,...
trackResults(channelNr).data_Q, ... trackResults(channelNr).data_Q, ...
'.'); '.');
grid (handles(1, 1)); grid (handles(1, 1));
axis (handles(1, 1), 'equal'); axis (handles(1, 1), 'equal');
title (handles(1, 1), 'Discrete-Time Scatter Plot'); title (handles(1, 1), 'Discrete-Time Scatter Plot');
xlabel(handles(1, 1), 'I prompt'); xlabel(handles(1, 1), 'I prompt');
ylabel(handles(1, 1), 'Q prompt'); ylabel(handles(1, 1), 'Q prompt');
%----- Nav bits --------------------------------------------------- %----- Nav bits ---------------------------------------------------
t = (1:length(trackResults(channelNr).data_I)); t = (1:length(trackResults(channelNr).data_I));
plot (handles(1, 2), t, ... plot (handles(1, 2), t, ...
trackResults(channelNr).data_I); trackResults(channelNr).data_I);
grid (handles(1, 2)); grid (handles(1, 2));
title (handles(1, 2), 'Bits of the navigation message'); title (handles(1, 2), 'Bits of the navigation message');
xlabel(handles(1, 2), 'Time (s)'); xlabel(handles(1, 2), 'Time (s)');
axis (handles(1, 2), 'tight'); axis (handles(1, 2), 'tight');
%----- PLL discriminator unfiltered-------------------------------- %----- PLL discriminator unfiltered--------------------------------
t = (1:length(trackResults(channelNr).pllDiscr)); t = (1:length(trackResults(channelNr).pllDiscr));
plot (handles(2, 1), t, ... plot (handles(2, 1), t, ...
trackResults(channelNr).pllDiscr, 'r'); trackResults(channelNr).pllDiscr, 'r');
grid (handles(2, 1)); grid (handles(2, 1));
axis (handles(2, 1), 'tight'); axis (handles(2, 1), 'tight');
xlabel(handles(2, 1), 'Time (s)'); xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Amplitude'); ylabel(handles(2, 1), 'Amplitude');
title (handles(2, 1), 'Raw PLL discriminator'); title (handles(2, 1), 'Raw PLL discriminator');
%----- Correlation ------------------------------------------------ %----- Correlation ------------------------------------------------
t = (1:length(trackResults(channelNr).I_VE)); t = (1:length(trackResults(channelNr).I_VE));
plot(handles(2, 2), t, ... plot(handles(2, 2), t, ...
[sqrt(trackResults(channelNr).I_VE.^2 + ... [sqrt(trackResults(channelNr).I_VE.^2 + ...
trackResults(channelNr).Q_VE.^2)', ... trackResults(channelNr).Q_VE.^2)', ...
sqrt(trackResults(channelNr).I_E.^2 + ... sqrt(trackResults(channelNr).I_E.^2 + ...
trackResults(channelNr).Q_E.^2)', ... trackResults(channelNr).Q_E.^2)', ...
sqrt(trackResults(channelNr).I_P.^2 + ... sqrt(trackResults(channelNr).I_P.^2 + ...
trackResults(channelNr).Q_P.^2)', ... trackResults(channelNr).Q_P.^2)', ...
sqrt(trackResults(channelNr).I_L.^2 + ... sqrt(trackResults(channelNr).I_L.^2 + ...
trackResults(channelNr).Q_L.^2)', ... trackResults(channelNr).Q_L.^2)', ...
sqrt(trackResults(channelNr).I_VL.^2 + ... sqrt(trackResults(channelNr).I_VL.^2 + ...
trackResults(channelNr).Q_VL.^2)'], ... trackResults(channelNr).Q_VL.^2)'], ...
'-*'); '-*');
grid (handles(2, 2)); grid (handles(2, 2));
title (handles(2, 2), 'Correlation results'); title (handles(2, 2), 'Correlation results');
xlabel(handles(2, 2), 'Time (s)'); xlabel(handles(2, 2), 'Time (s)');
axis (handles(2, 2), 'tight'); axis (handles(2, 2), 'tight');
hLegend = legend(handles(2, 2), '$\sqrt{I_{VE}^2 + Q_{VE}^2}$', ... hLegend = legend(handles(2, 2), '$\sqrt{I_{VE}^2 + Q_{VE}^2}$', ...
'$\sqrt{I_{E}^2 + Q_{E}^2}$', ... '$\sqrt{I_{E}^2 + Q_{E}^2}$', ...
'$\sqrt{I_{P}^2 + Q_{P}^2}$', ... '$\sqrt{I_{P}^2 + Q_{P}^2}$', ...
'$\sqrt{I_{L}^2 + Q_{L}^2}$', ... '$\sqrt{I_{L}^2 + Q_{L}^2}$', ...
'$\sqrt{I_{VL}^2 + Q_{VL}^2}$'); '$\sqrt{I_{VL}^2 + Q_{VL}^2}$');
%set interpreter from tex to latex. This will draw \sqrt correctly %set interpreter from tex to latex. This will draw \sqrt correctly
set(hLegend, 'Interpreter', 'Latex'); set(hLegend, 'Interpreter', 'Latex');
%----- PLL discriminator filtered---------------------------------- %----- PLL discriminator filtered----------------------------------
t = (1:length(trackResults(channelNr).pllDiscrFilt)); t = (1:length(trackResults(channelNr).pllDiscrFilt));
plot (handles(3, 1), t, ... plot (handles(3, 1), t, ...
trackResults(channelNr).pllDiscrFilt, 'b'); trackResults(channelNr).pllDiscrFilt, 'b');
grid (handles(3, 1)); grid (handles(3, 1));
axis (handles(3, 1), 'tight'); axis (handles(3, 1), 'tight');
xlabel(handles(3, 1), 'Time (s)'); xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude'); ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Filtered PLL discriminator'); title (handles(3, 1), 'Filtered PLL discriminator');
%----- DLL discriminator unfiltered-------------------------------- %----- DLL discriminator unfiltered--------------------------------
t = (1:length(trackResults(channelNr).dllDiscr)); t = (1:length(trackResults(channelNr).dllDiscr));
plot (handles(3, 2), t, ... plot (handles(3, 2), t, ...
trackResults(channelNr).dllDiscr, 'r'); trackResults(channelNr).dllDiscr, 'r');
grid (handles(3, 2)); grid (handles(3, 2));
axis (handles(3, 2), 'tight'); axis (handles(3, 2), 'tight');
xlabel(handles(3, 2), 'Time (s)'); xlabel(handles(3, 2), 'Time (s)');
ylabel(handles(3, 2), 'Amplitude'); ylabel(handles(3, 2), 'Amplitude');
title (handles(3, 2), 'Raw DLL discriminator'); title (handles(3, 2), 'Raw DLL discriminator');
%----- DLL discriminator filtered---------------------------------- %----- DLL discriminator filtered----------------------------------
t = (1:length(trackResults(channelNr).dllDiscrFilt)); t = (1:length(trackResults(channelNr).dllDiscrFilt));
plot (handles(3, 3), t, ... plot (handles(3, 3), t, ...
trackResults(channelNr).dllDiscrFilt, 'b'); trackResults(channelNr).dllDiscrFilt, 'b');
grid (handles(3, 3)); grid (handles(3, 3));
axis (handles(3, 3), 'tight'); axis (handles(3, 3), 'tight');
xlabel(handles(3, 3), 'Time (s)'); xlabel(handles(3, 3), 'Time (s)');
ylabel(handles(3, 3), 'Amplitude'); ylabel(handles(3, 3), 'Amplitude');
title (handles(3, 3), 'Filtered DLL discriminator'); title (handles(3, 3), 'Filtered DLL discriminator');
end % for channelNr = channelList end % for channelNr = channelList

300
src/utils/matlab/plotTrackingE5a.m
View File

@ -1,150 +1,150 @@
function plotTracking(channelList, trackResults, settings) function plotTracking(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list. % This function plots the tracking results for the given channel list.
% %
% plotTracking(channelList, trackResults, settings) % plotTracking(channelList, trackResults, settings)
% %
% Inputs: % Inputs:
% channelList - list of channels to be plotted. % channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function. % trackResults - tracking results from the tracking function.
% settings - receiver settings. % settings - receiver settings.
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
% %
% Copyright (C) Darius Plausinaitis % Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis % Written by Darius Plausinaitis
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% This program is free software; you can redistribute it and/or % This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License % modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2 % as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version. % of the License, or (at your option) any later version.
% %
% This program is distributed in the hope that it will be useful, % This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of % but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details. % GNU General Public License for more details.
% %
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software % along with this program; if not, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, % Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
% USA. % USA.
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers % Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels); channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ============================================== %=== For all listed channels ==============================================
for channelNr = channelList for channelNr = channelList
%% Select (or create) and clear the figure ================================ %% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open % The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened. % figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by % Figures drawn or opened by the user, will not be "overwritten" by
% this function. % this function.
figure(channelNr +200); figure(channelNr +200);
clf(channelNr +200); clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ... ' (PRN ', ...
num2str(trackResults(channelNr).PRN), ... num2str(trackResults(channelNr).PRN), ...
') results']); ') results']);
%% Draw axes ============================================================== %% Draw axes ==============================================================
% Row 1 % Row 1
handles(1, 1) = subplot(3, 3, 1); handles(1, 1) = subplot(3, 3, 1);
handles(1, 2) = subplot(3, 3, [2 3]); handles(1, 2) = subplot(3, 3, [2 3]);
% Row 2 % Row 2
handles(2, 1) = subplot(3, 3, 4); handles(2, 1) = subplot(3, 3, 4);
handles(2, 2) = subplot(3, 3, [5 6]); handles(2, 2) = subplot(3, 3, [5 6]);
% Row 3 % Row 3
handles(3, 1) = subplot(3, 3, 7); handles(3, 1) = subplot(3, 3, 7);
handles(3, 2) = subplot(3, 3, 8); handles(3, 2) = subplot(3, 3, 8);
handles(3, 3) = subplot(3, 3, 9); handles(3, 3) = subplot(3, 3, 9);
%% Plot all figures ======================================================= %% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess-1)/1000; timeAxisInSeconds = (1:settings.msToProcess-1)/1000;
%----- Discrete-Time Scatter Plot --------------------------------- %----- Discrete-Time Scatter Plot ---------------------------------
plot(handles(1, 1), trackResults(channelNr).I_PN,... plot(handles(1, 1), trackResults(channelNr).I_PN,...
trackResults(channelNr).Q_PN, ... trackResults(channelNr).Q_PN, ...
'.'); '.');
grid (handles(1, 1)); grid (handles(1, 1));
axis (handles(1, 1), 'equal'); axis (handles(1, 1), 'equal');
title (handles(1, 1), 'Discrete-Time Scatter Plot'); title (handles(1, 1), 'Discrete-Time Scatter Plot');
xlabel(handles(1, 1), 'I prompt'); xlabel(handles(1, 1), 'I prompt');
ylabel(handles(1, 1), 'Q prompt'); ylabel(handles(1, 1), 'Q prompt');
%----- Nav bits --------------------------------------------------- %----- Nav bits ---------------------------------------------------
plot (handles(1, 2), timeAxisInSeconds, ... plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).I_PN(1:settings.msToProcess-1)); trackResults(channelNr).I_PN(1:settings.msToProcess-1));
grid (handles(1, 2)); grid (handles(1, 2));
title (handles(1, 2), 'Bits of the navigation message'); title (handles(1, 2), 'Bits of the navigation message');
xlabel(handles(1, 2), 'Time (s)'); xlabel(handles(1, 2), 'Time (s)');
axis (handles(1, 2), 'tight'); axis (handles(1, 2), 'tight');
%----- PLL discriminator unfiltered-------------------------------- %----- PLL discriminator unfiltered--------------------------------
plot (handles(2, 1), timeAxisInSeconds, ... plot (handles(2, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscr(1:settings.msToProcess-1), 'r'); trackResults(channelNr).pllDiscr(1:settings.msToProcess-1), 'r');
grid (handles(2, 1)); grid (handles(2, 1));
axis (handles(2, 1), 'tight'); axis (handles(2, 1), 'tight');
xlabel(handles(2, 1), 'Time (s)'); xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Amplitude'); ylabel(handles(2, 1), 'Amplitude');
title (handles(2, 1), 'Raw PLL discriminator'); title (handles(2, 1), 'Raw PLL discriminator');
%----- Correlation ------------------------------------------------ %----- Correlation ------------------------------------------------
plot(handles(2, 2), timeAxisInSeconds, ... plot(handles(2, 2), timeAxisInSeconds, ...
[sqrt(trackResults(channelNr).I_E(1:settings.msToProcess-1).^2 + ... [sqrt(trackResults(channelNr).I_E(1:settings.msToProcess-1).^2 + ...
trackResults(channelNr).Q_E(1:settings.msToProcess-1).^2)', ... trackResults(channelNr).Q_E(1:settings.msToProcess-1).^2)', ...
sqrt(trackResults(channelNr).I_P(1:settings.msToProcess-1).^2 + ... sqrt(trackResults(channelNr).I_P(1:settings.msToProcess-1).^2 + ...
trackResults(channelNr).Q_P(1:settings.msToProcess-1).^2)', ... trackResults(channelNr).Q_P(1:settings.msToProcess-1).^2)', ...
sqrt(trackResults(channelNr).I_L(1:settings.msToProcess-1).^2 + ... sqrt(trackResults(channelNr).I_L(1:settings.msToProcess-1).^2 + ...
trackResults(channelNr).Q_L(1:settings.msToProcess-1).^2)'], ... trackResults(channelNr).Q_L(1:settings.msToProcess-1).^2)'], ...
'-*'); '-*');
grid (handles(2, 2)); grid (handles(2, 2));
title (handles(2, 2), 'Correlation results'); title (handles(2, 2), 'Correlation results');
xlabel(handles(2, 2), 'Time (s)'); xlabel(handles(2, 2), 'Time (s)');
axis (handles(2, 2), 'tight'); axis (handles(2, 2), 'tight');
hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ...
'$\sqrt{I_{P}^2 + Q_{P}^2}$', ... '$\sqrt{I_{P}^2 + Q_{P}^2}$', ...
'$\sqrt{I_{L}^2 + Q_{L}^2}$'); '$\sqrt{I_{L}^2 + Q_{L}^2}$');
%set interpreter from tex to latex. This will draw \sqrt correctly %set interpreter from tex to latex. This will draw \sqrt correctly
set(hLegend, 'Interpreter', 'Latex'); set(hLegend, 'Interpreter', 'Latex');
%----- PLL discriminator filtered---------------------------------- %----- PLL discriminator filtered----------------------------------
plot (handles(3, 1), timeAxisInSeconds, ... plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess-1), 'b'); trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess-1), 'b');
grid (handles(3, 1)); grid (handles(3, 1));
axis (handles(3, 1), 'tight'); axis (handles(3, 1), 'tight');
xlabel(handles(3, 1), 'Time (s)'); xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude'); ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Filtered PLL discriminator'); title (handles(3, 1), 'Filtered PLL discriminator');
%----- DLL discriminator unfiltered-------------------------------- %----- DLL discriminator unfiltered--------------------------------
plot (handles(3, 2), timeAxisInSeconds, ... plot (handles(3, 2), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscr(1:settings.msToProcess-1), 'r'); trackResults(channelNr).dllDiscr(1:settings.msToProcess-1), 'r');
grid (handles(3, 2)); grid (handles(3, 2));
axis (handles(3, 2), 'tight'); axis (handles(3, 2), 'tight');
xlabel(handles(3, 2), 'Time (s)'); xlabel(handles(3, 2), 'Time (s)');
ylabel(handles(3, 2), 'Amplitude'); ylabel(handles(3, 2), 'Amplitude');
title (handles(3, 2), 'Raw DLL discriminator'); title (handles(3, 2), 'Raw DLL discriminator');
%----- DLL discriminator filtered---------------------------------- %----- DLL discriminator filtered----------------------------------
plot (handles(3, 3), timeAxisInSeconds, ... plot (handles(3, 3), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscrFilt(1:settings.msToProcess-1), 'b'); trackResults(channelNr).dllDiscrFilt(1:settings.msToProcess-1), 'b');
grid (handles(3, 3)); grid (handles(3, 3));
axis (handles(3, 3), 'tight'); axis (handles(3, 3), 'tight');
xlabel(handles(3, 3), 'Time (s)'); xlabel(handles(3, 3), 'Time (s)');
ylabel(handles(3, 3), 'Amplitude'); ylabel(handles(3, 3), 'Amplitude');
title (handles(3, 3), 'Filtered DLL discriminator'); title (handles(3, 3), 'Filtered DLL discriminator');
end % for channelNr = channelList end % for channelNr = channelList