From aa1e98f943d52cd66bad123b94ead00c2f59a15b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 30 Mar 2018 12:04:14 +0200 Subject: [PATCH 1/2] Get rid of ^M character --- src/utils/matlab/dll_pll_veml_plot_sample.m | 166 ++++---- .../galileo_e1_dll_pll_veml_plot_sample.m | 158 ++++---- .../matlab/glonass_ca_dll_pll_plot_sample.m | 146 +++---- .../matlab/gps_l1_ca_dll_pll_plot_sample.m | 150 +++---- .../matlab/gps_l1_ca_telemetry_plot_sample.m | 78 ++-- src/utils/matlab/libs/geoFunctions/cart2geo.m | 116 +++--- src/utils/matlab/libs/geoFunctions/cart2utm.m | 344 ++++++++-------- src/utils/matlab/libs/geoFunctions/check_t.m | 50 +-- src/utils/matlab/libs/geoFunctions/clksin.m | 72 ++-- src/utils/matlab/libs/geoFunctions/clsin.m | 46 +-- src/utils/matlab/libs/geoFunctions/deg2dms.m | 86 ++-- src/utils/matlab/libs/geoFunctions/dms2deg.m | 24 +- src/utils/matlab/libs/geoFunctions/dms2mat.m | 208 +++++----- src/utils/matlab/libs/geoFunctions/e_r_corr.m | 62 +-- .../matlab/libs/geoFunctions/findUtmZone.m | 138 +++---- src/utils/matlab/libs/geoFunctions/geo2cart.m | 90 ++--- .../matlab/libs/geoFunctions/leastSquarePos.m | 222 +++++------ src/utils/matlab/libs/geoFunctions/mat2dms.m | 248 ++++++------ src/utils/matlab/libs/geoFunctions/roundn.m | 92 ++--- src/utils/matlab/libs/geoFunctions/satpos.m | 276 ++++++------- src/utils/matlab/libs/geoFunctions/togeod.m | 218 +++++----- src/utils/matlab/libs/geoFunctions/topocent.m | 108 ++--- src/utils/matlab/libs/geoFunctions/tropo.m | 190 ++++----- src/utils/matlab/libs/plotNavigation.m | 332 ++++++++-------- src/utils/matlab/libs/plotTracking.m | 374 +++++++++--------- src/utils/matlab/libs/plotVEMLTracking.m | 324 +++++++-------- src/utils/matlab/plotTrackingE5a.m | 300 +++++++------- 27 files changed, 2309 insertions(+), 2309 deletions(-) diff --git a/src/utils/matlab/dll_pll_veml_plot_sample.m b/src/utils/matlab/dll_pll_veml_plot_sample.m index 273de3861..cd79955e3 100644 --- a/src/utils/matlab/dll_pll_veml_plot_sample.m +++ b/src/utils/matlab/dll_pll_veml_plot_sample.m @@ -1,83 +1,83 @@ -% Reads GNSS-SDR Tracking dump binary file using the provided -% function and plots some internal variables -% Javier Arribas, 2011. jarribas(at)cttc.es -% Antonio Ramos, 2018. antonio.ramos(at)cttc.es -% ------------------------------------------------------------------------- -% -% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) -% -% GNSS-SDR is a software defined Global Navigation -% Satellite Systems receiver -% -% This file is part of GNSS-SDR. -% -% 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 -% the Free Software Foundation, either version 3 of the License, or -% at your option) any later version. -% -% GNSS-SDR is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License -% along with GNSS-SDR. If not, see . -% -% ------------------------------------------------------------------------- -% - -close all; -clear all; - -if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') - addpath('./libs') -end - -samplingFreq = 5000000; %[Hz] -coherent_integration_time_ms = 20; %[ms] -channels = 5; % Number of channels -first_channel = 0; % Number of the first channel - -path = '/dump_dir/'; %% CHANGE THIS PATH - -for N=1:1:channels - 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); -end - -% GNSS-SDR format conversion to MATLAB GPS receiver - -for N=1:1:channels - trackResults(N).status = 'T'; %fake track - trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; - trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; - trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; - trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; - trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; - trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; - - trackResults(N).I_P = 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_E = GNSS_tracking(N).E.'; - trackResults(N).I_L = GNSS_tracking(N).L.'; - trackResults(N).I_VL = GNSS_tracking(N).VL.'; - 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_L = zeros(1,length(GNSS_tracking(N).L)); - trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); - trackResults(N).data_I = GNSS_tracking(N).prompt_I.'; - trackResults(N).data_Q = GNSS_tracking(N).prompt_Q.'; - trackResults(N).PRN = GNSS_tracking(N).PRN.'; - trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; - - % Use original MATLAB tracking plot function - settings.numberOfChannels = channels; - settings.msToProcess = length(GNSS_tracking(N).E) * coherent_integration_time_ms; - plotVEMLTracking(N, trackResults, settings) -end - - - +% Reads GNSS-SDR Tracking dump binary file using the provided +% function and plots some internal variables +% Javier Arribas, 2011. jarribas(at)cttc.es +% Antonio Ramos, 2018. antonio.ramos(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% 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 +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') + addpath('./libs') +end + +samplingFreq = 5000000; %[Hz] +coherent_integration_time_ms = 20; %[ms] +channels = 5; % Number of channels +first_channel = 0; % Number of the first channel + +path = '/dump_dir/'; %% CHANGE THIS PATH + +for N=1:1:channels + 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); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = 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_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).I_VL = GNSS_tracking(N).VL.'; + 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_L = zeros(1,length(GNSS_tracking(N).L)); + trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); + trackResults(N).data_I = GNSS_tracking(N).prompt_I.'; + trackResults(N).data_Q = GNSS_tracking(N).prompt_Q.'; + trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E) * coherent_integration_time_ms; + plotVEMLTracking(N, trackResults, settings) +end + + + diff --git a/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m b/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m index fb04ccd71..240c279f7 100644 --- a/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m +++ b/src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample.m @@ -1,79 +1,79 @@ -% Reads GNSS-SDR Tracking dump binary file using the provided -% function and plots some internal variables -% Javier Arribas, 2011. jarribas(at)cttc.es -% ------------------------------------------------------------------------- -% -% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) -% -% GNSS-SDR is a software defined Global Navigation -% Satellite Systems receiver -% -% This file is part of GNSS-SDR. -% -% 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 -% the Free Software Foundation, either version 3 of the License, or -% at your option) any later version. -% -% GNSS-SDR is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License -% along with GNSS-SDR. If not, see . -% -% ------------------------------------------------------------------------- -% - -close all; -clear all; - -if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') - addpath('./libs') -end - -samplingFreq = 5000000; %[Hz] -channels = 7; % Number of channels -first_channel = 0; % Number of the first channel - -path = '/Users/carlesfernandez/git/cttc/build/'; %% CHANGE THIS PATH - -for N=1:1:channels - 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); -end - -% GNSS-SDR format conversion to MATLAB GPS receiver - -for N=1:1:channels - trackResults(N).status = 'T'; %fake track - trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; - trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; - trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; - trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; - trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; - trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; - - trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; - trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; - - trackResults(N).I_VE = GNSS_tracking(N).VE.'; - trackResults(N).I_E = GNSS_tracking(N).E.'; - trackResults(N).I_L = GNSS_tracking(N).L.'; - trackResults(N).I_VL = GNSS_tracking(N).VL.'; - 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_L = zeros(1,length(GNSS_tracking(N).L)); - trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); - trackResults(N).PRN = GNSS_tracking(N).PRN.'; - trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; - - % Use original MATLAB tracking plot function - settings.numberOfChannels = channels; - settings.msToProcess = length(GNSS_tracking(N).E)*4; - plotVEMLTracking(N, trackResults, settings) -end - - - +% Reads GNSS-SDR Tracking dump binary file using the provided +% function and plots some internal variables +% Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% 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 +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') + addpath('./libs') +end + +samplingFreq = 5000000; %[Hz] +channels = 7; % Number of channels +first_channel = 0; % Number of the first channel + +path = '/Users/carlesfernandez/git/cttc/build/'; %% CHANGE THIS PATH + +for N=1:1:channels + 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); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; + trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; + + trackResults(N).I_VE = GNSS_tracking(N).VE.'; + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).I_VL = GNSS_tracking(N).VL.'; + 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_L = zeros(1,length(GNSS_tracking(N).L)); + trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); + trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E)*4; + plotVEMLTracking(N, trackResults, settings) +end + + + diff --git a/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m b/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m index d44799f6f..fc342eb26 100644 --- a/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m +++ b/src/utils/matlab/glonass_ca_dll_pll_plot_sample.m @@ -1,73 +1,73 @@ -% Reads GNSS-SDR Tracking dump binary file using the provided -% function and plots some internal variables -% Damian Miralles, 2017. dmiralles2009(at)gmail.com -% ------------------------------------------------------------------------- -% -% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) -% -% GNSS-SDR is a software defined Global Navigation -% Satellite Systems receiver -% -% This file is part of GNSS-SDR. -% -% 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 -% the Free Software Foundation, either version 3 of the License, or -% at your option) any later version. -% -% GNSS-SDR is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License -% along with GNSS-SDR. If not, see . -% -% ------------------------------------------------------------------------- -% - -close all; -clear all; - -if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') - addpath('./libs') -end - - -samplingFreq = 6625000; %[Hz] -channels = 5; -first_channel = 0; - -path = '/archive/'; %% CHANGE THIS PATH - -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 - GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); -end - -% GNSS-SDR format conversion to MATLAB GPS receiver - -for N=1:1:channels - trackResults(N).status = 'T'; %fake track - trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; - trackResults(N).carrFreq = GNSS_tracking(N).carrier_freq_hz.'; - trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; - trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; - trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; - trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; - - trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; - trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; - - trackResults(N).I_E = GNSS_tracking(N).E.'; - trackResults(N).I_L = GNSS_tracking(N).L.'; - trackResults(N).Q_E = 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).PRN = ones(1,length(GNSS_tracking(N).E)); - - % Use original MATLAB tracking plot function - settings.numberOfChannels = channels; - settings.msToProcess = length(GNSS_tracking(N).E); - plotTracking(N, trackResults, settings) -end +% Reads GNSS-SDR Tracking dump binary file using the provided +% function and plots some internal variables +% Damian Miralles, 2017. dmiralles2009(at)gmail.com +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% 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 +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') + addpath('./libs') +end + + +samplingFreq = 6625000; %[Hz] +channels = 5; +first_channel = 0; + +path = '/archive/'; %% CHANGE THIS PATH + +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 + GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_freq_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; + trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; + + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).Q_E = 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).PRN = ones(1,length(GNSS_tracking(N).E)); + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E); + plotTracking(N, trackResults, settings) +end diff --git a/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m b/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m index 7dd7aa3d6..093799f7d 100644 --- a/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m +++ b/src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m @@ -1,75 +1,75 @@ -% Reads GNSS-SDR Tracking dump binary file using the provided -% function and plots some internal variables -% Javier Arribas, 2011. jarribas(at)cttc.es -% ------------------------------------------------------------------------- -% -% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) -% -% GNSS-SDR is a software defined Global Navigation -% Satellite Systems receiver -% -% This file is part of GNSS-SDR. -% -% 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 -% the Free Software Foundation, either version 3 of the License, or -% at your option) any later version. -% -% GNSS-SDR is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License -% along with GNSS-SDR. If not, see . -% -% ------------------------------------------------------------------------- -% - -close all; -clear all; - -if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') - addpath('./libs') -end - - -samplingFreq = 6625000; %[Hz] -channels = 5; -first_channel = 0; - -path = '/archive/'; %% CHANGE THIS PATH - -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 - GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); -end - -% GNSS-SDR format conversion to MATLAB GPS receiver - -for N=1:1:channels - trackResults(N).status = 'T'; %fake track - trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; - trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; - trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; - trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; - trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; - trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; - - trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; - trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; - - trackResults(N).I_E = GNSS_tracking(N).E.'; - trackResults(N).I_L = GNSS_tracking(N).L.'; - trackResults(N).Q_E = 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).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; - - % Use original MATLAB tracking plot function - settings.numberOfChannels = channels; - settings.msToProcess = length(GNSS_tracking(N).E); - plotTracking(N, trackResults, settings) -end - - +% Reads GNSS-SDR Tracking dump binary file using the provided +% function and plots some internal variables +% Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% 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 +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') + addpath('./libs') +end + + +samplingFreq = 6625000; %[Hz] +channels = 5; +first_channel = 0; + +path = '/archive/'; %% CHANGE THIS PATH + +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 + GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).prompt_I.'; + trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.'; + + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).Q_E = 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).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E); + plotTracking(N, trackResults, settings) +end + + diff --git a/src/utils/matlab/gps_l1_ca_telemetry_plot_sample.m b/src/utils/matlab/gps_l1_ca_telemetry_plot_sample.m index f1b839419..9e0e27125 100644 --- a/src/utils/matlab/gps_l1_ca_telemetry_plot_sample.m +++ b/src/utils/matlab/gps_l1_ca_telemetry_plot_sample.m @@ -1,39 +1,39 @@ -% Reads GNSS-SDR Tracking dump binary file using the provided -% function and plots some internal variables -% Javier Arribas, 2011. jarribas(at)cttc.es -% ------------------------------------------------------------------------- -% -% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) -% -% GNSS-SDR is a software defined Global Navigation -% Satellite Systems receiver -% -% This file is part of GNSS-SDR. -% -% 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 -% the Free Software Foundation, either version 3 of the License, or -% at your option) any later version. -% -% GNSS-SDR is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License -% along with GNSS-SDR. If not, see . -% -% ------------------------------------------------------------------------- -% - -%close all; -%clear all; -samplingFreq = 64e6/16; %[Hz] -channels=4; -path='/home/javier/workspace/gnss-sdr-ref/trunk/install/'; -clear PRN_absolute_sample_start; -for N=1:1:channels - telemetry_log_path=[path 'telemetry' num2str(N-1) '.dat']; - GNSS_telemetry(N)= gps_l1_ca_read_telemetry_dump(telemetry_log_path); -end - +% Reads GNSS-SDR Tracking dump binary file using the provided +% function and plots some internal variables +% Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% 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 +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +%close all; +%clear all; +samplingFreq = 64e6/16; %[Hz] +channels=4; +path='/home/javier/workspace/gnss-sdr-ref/trunk/install/'; +clear PRN_absolute_sample_start; +for N=1:1:channels + telemetry_log_path=[path 'telemetry' num2str(N-1) '.dat']; + GNSS_telemetry(N)= gps_l1_ca_read_telemetry_dump(telemetry_log_path); +end + diff --git a/src/utils/matlab/libs/geoFunctions/cart2geo.m b/src/utils/matlab/libs/geoFunctions/cart2geo.m index 45947151f..324a226c1 100644 --- a/src/utils/matlab/libs/geoFunctions/cart2geo.m +++ b/src/utils/matlab/libs/geoFunctions/cart2geo.m @@ -1,58 +1,58 @@ -function [phi, lambda, h] = cart2geo(X, Y, Z, i) -% CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical -% coordinates (phi, lambda, h) on a selected reference ellipsoid. -% -% [phi, lambda, h] = cart2geo(X, Y, Z, i); -% -% Choices i of Reference Ellipsoid for Geographical Coordinates -% 1. International Ellipsoid 1924 -% 2. International Ellipsoid 1967 -% 3. World Geodetic System 1972 -% 4. Geodetic Reference System 1980 -% 5. World Geodetic System 1984 - -% Kai Borre 10-13-98 -% Copyright (c) by Kai Borre -% Revision: 1.0 Date: 1998/10/23 -%========================================================================== - -a = [6378388 6378160 6378135 6378137 6378137]; -f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563]; - -lambda = atan2(Y,X); -ex2 = (2-f(i))*f(i)/((1-f(i))^2); -c = a(i)*sqrt(1+ex2); -phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i)))); - -h = 0.1; oldh = 0; -iterations = 0; -while abs(h-oldh) > 1.e-12 - oldh = h; - 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))))); - h = sqrt(X^2+Y^2)/cos(phi)-N; - - iterations = iterations + 1; - if iterations > 100 - fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh); - break; - end -end - -phi = phi*180/pi; -% b = zeros(1,3); -% b(1,1) = fix(phi); -% b(2,1) = fix(rem(phi,b(1,1))*60); -% b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600; - -lambda = lambda*180/pi; -% l = zeros(1,3); -% l(1,1) = fix(lambda); -% l(2,1) = fix(rem(lambda,l(1,1))*60); -% 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 lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3)) -%fprintf('\n h =%14.3f\n',h) - -%%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%% +function [phi, lambda, h] = cart2geo(X, Y, Z, i) +% CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical +% coordinates (phi, lambda, h) on a selected reference ellipsoid. +% +% [phi, lambda, h] = cart2geo(X, Y, Z, i); +% +% Choices i of Reference Ellipsoid for Geographical Coordinates +% 1. International Ellipsoid 1924 +% 2. International Ellipsoid 1967 +% 3. World Geodetic System 1972 +% 4. Geodetic Reference System 1980 +% 5. World Geodetic System 1984 + +% Kai Borre 10-13-98 +% Copyright (c) by Kai Borre +% Revision: 1.0 Date: 1998/10/23 +%========================================================================== + +a = [6378388 6378160 6378135 6378137 6378137]; +f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563]; + +lambda = atan2(Y,X); +ex2 = (2-f(i))*f(i)/((1-f(i))^2); +c = a(i)*sqrt(1+ex2); +phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i)))); + +h = 0.1; oldh = 0; +iterations = 0; +while abs(h-oldh) > 1.e-12 + oldh = h; + 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))))); + h = sqrt(X^2+Y^2)/cos(phi)-N; + + iterations = iterations + 1; + if iterations > 100 + fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh); + break; + end +end + +phi = phi*180/pi; +% b = zeros(1,3); +% b(1,1) = fix(phi); +% b(2,1) = fix(rem(phi,b(1,1))*60); +% b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600; + +lambda = lambda*180/pi; +% l = zeros(1,3); +% l(1,1) = fix(lambda); +% l(2,1) = fix(rem(lambda,l(1,1))*60); +% 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 lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3)) +%fprintf('\n h =%14.3f\n',h) + +%%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/cart2utm.m b/src/utils/matlab/libs/geoFunctions/cart2utm.m index 8617bf541..48da278ea 100644 --- a/src/utils/matlab/libs/geoFunctions/cart2utm.m +++ b/src/utils/matlab/libs/geoFunctions/cart2utm.m @@ -1,173 +1,173 @@ -function [E, N, U] = cart2utm(X, Y, Z, zone) -% CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. -% -% [E, N, U] = cart2utm(X, Y, Z, zone); -% -% Inputs: -% X,Y,Z - Cartesian coordinates. Coordinates are referenced -% with respect to the International Terrestrial Reference -% Frame 1996 (ITRF96) -% zone - UTM zone of the given position -% -% Outputs: -% E, N, U - UTM coordinates (Easting, Northing, Uping) - -% Kai Borre -11-1994 -% Copyright (c) by Kai Borre - -% This implementation is based upon -% O. Andersson & K. Poder (1981) Koordinattransformationer -% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren -% Vol. 30: 552--571 and Vol. 31: 76 -% -% An excellent, general reference (KW) is -% R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der -% h\"oheren Geod\"asie und Kartographie. -% Erster Band, Springer Verlag - -% Explanation of variables used: -% f flattening of ellipsoid -% a semi major axis in m -% m0 1 - scale at central meridian; for UTM 0.0004 -% Q_n normalized meridian quadrant -% E0 Easting of central meridian -% L0 Longitude of central meridian -% bg constants for ellipsoidal geogr. to spherical geogr. -% gb constants for spherical geogr. to ellipsoidal geogr. -% gtu constants for ellipsoidal N, E to spherical N, E -% utg constants for spherical N, E to ellipoidal N, E -% tolutm tolerance for utm, 1.2E-10*meridian quadrant -% tolgeo tolerance for geographical, 0.00040 second of arc - -% B, L refer to latitude and longitude. Southern latitude is negative -% International ellipsoid of 1924, valid for ED50 - -a = 6378388; -f = 1/297; -ex2 = (2-f)*f / ((1-f)^2); -c = a * sqrt(1+ex2); -vec = [X; Y; Z-4.5]; -alpha = .756e-6; -R = [ 1 -alpha 0; - alpha 1 0; - 0 0 1]; -trans = [89.5; 93.8; 127.6]; -scale = 0.9999988; -v = scale*R*vec + trans; % coordinate vector in ED50 -L = atan2(v(2), v(1)); -N1 = 6395000; % preliminary value -B = atan2(v(3)/((1-f)^2*N1), norm(v(1:2))/N1); % preliminary value -U = 0.1; oldU = 0; - -iterations = 0; -while abs(U-oldU) > 1.e-4 - oldU = U; - N1 = c/sqrt(1+ex2*(cos(B))^2); - B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) ); - U = norm(v(1:2))/cos(B)-N1; - - iterations = iterations + 1; - if iterations > 100 - fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU); - break; - end -end - -% Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) -m0 = 0.0004; -n = f / (2-f); -m = n^2 * (1/4 + n*n/64); -w = (a*(-n-m0+m*(1-m0))) / (1+n); -Q_n = a + w; - -% Easting and longitude of central meridian -E0 = 500000; -L0 = (zone-30)*6 - 3; - -% Check tolerance for reverse transformation -tolutm = pi/2 * 1.2e-10 * Q_n; -tolgeo = 0.000040; - -% Coefficients of trigonometric series - -% ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) -% 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[3] = n^3*(-26/15 + n*34/21); -% bg[4] = n^4*1237/630; - -% spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) -% 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[3] = n^3*(56/15 + n*(-136/35)); -% gb[4] = n^4*4279/630; - -% spherical to ellipsoidal N, E, KW p. 196, (69) -% 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[3] = n^3*(61/240 + n*(-103/140)); -% gtu[4] = n^4*49561/161280; - -% ellipsoidal to spherical N, E, KW p. 194, (65) -% 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[3] = n^3*(-17/480 + n*37/840); -% utg[4] = n^4*(-4397/161280); - -% With f = 1/297 we get - -bg = [-3.37077907e-3; - 4.73444769e-6; - -8.29914570e-9; - 1.58785330e-11]; - -gb = [ 3.37077588e-3; - 6.62769080e-6; - 1.78718601e-8; - 5.49266312e-11]; - -gtu = [ 8.41275991e-4; - 7.67306686e-7; - 1.21291230e-9; - 2.48508228e-12]; - -utg = [-8.41276339e-4; - -5.95619298e-8; - -1.69485209e-10; - -2.20473896e-13]; - -% Ellipsoidal latitude, longitude to spherical latitude, longitude -neg_geo = 'FALSE'; - -if B < 0 - neg_geo = 'TRUE '; -end - -Bg_r = abs(B); -[res_clensin] = clsin(bg, 4, 2*Bg_r); -Bg_r = Bg_r + res_clensin; -L0 = L0*pi / 180; -Lg_r = L - L0; - -% Spherical latitude, longitude to complementary spherical latitude -% i.e. spherical N, E -cos_BN = cos(Bg_r); -Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN); -Ep = atanh(sin(Lg_r) * cos_BN); - -%Spherical normalized N, E to ellipsoidal N, E -Np = 2 * Np; -Ep = 2 * Ep; -[dN, dE] = clksin(gtu, 4, Np, Ep); -Np = Np/2; -Ep = Ep/2; -Np = Np + dN; -Ep = Ep + dE; -N = Q_n * Np; -E = Q_n*Ep + E0; - -if neg_geo == 'TRUE ' - N = -N + 20000000; -end; - +function [E, N, U] = cart2utm(X, Y, Z, zone) +% CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. +% +% [E, N, U] = cart2utm(X, Y, Z, zone); +% +% Inputs: +% X,Y,Z - Cartesian coordinates. Coordinates are referenced +% with respect to the International Terrestrial Reference +% Frame 1996 (ITRF96) +% zone - UTM zone of the given position +% +% Outputs: +% E, N, U - UTM coordinates (Easting, Northing, Uping) + +% Kai Borre -11-1994 +% Copyright (c) by Kai Borre + +% This implementation is based upon +% O. Andersson & K. Poder (1981) Koordinattransformationer +% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren +% Vol. 30: 552--571 and Vol. 31: 76 +% +% An excellent, general reference (KW) is +% R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der +% h\"oheren Geod\"asie und Kartographie. +% Erster Band, Springer Verlag + +% Explanation of variables used: +% f flattening of ellipsoid +% a semi major axis in m +% m0 1 - scale at central meridian; for UTM 0.0004 +% Q_n normalized meridian quadrant +% E0 Easting of central meridian +% L0 Longitude of central meridian +% bg constants for ellipsoidal geogr. to spherical geogr. +% gb constants for spherical geogr. to ellipsoidal geogr. +% gtu constants for ellipsoidal N, E to spherical N, E +% utg constants for spherical N, E to ellipoidal N, E +% tolutm tolerance for utm, 1.2E-10*meridian quadrant +% tolgeo tolerance for geographical, 0.00040 second of arc + +% B, L refer to latitude and longitude. Southern latitude is negative +% International ellipsoid of 1924, valid for ED50 + +a = 6378388; +f = 1/297; +ex2 = (2-f)*f / ((1-f)^2); +c = a * sqrt(1+ex2); +vec = [X; Y; Z-4.5]; +alpha = .756e-6; +R = [ 1 -alpha 0; + alpha 1 0; + 0 0 1]; +trans = [89.5; 93.8; 127.6]; +scale = 0.9999988; +v = scale*R*vec + trans; % coordinate vector in ED50 +L = atan2(v(2), v(1)); +N1 = 6395000; % preliminary value +B = atan2(v(3)/((1-f)^2*N1), norm(v(1:2))/N1); % preliminary value +U = 0.1; oldU = 0; + +iterations = 0; +while abs(U-oldU) > 1.e-4 + oldU = U; + N1 = c/sqrt(1+ex2*(cos(B))^2); + B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) ); + U = norm(v(1:2))/cos(B)-N1; + + iterations = iterations + 1; + if iterations > 100 + fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU); + break; + end +end + +% Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) +m0 = 0.0004; +n = f / (2-f); +m = n^2 * (1/4 + n*n/64); +w = (a*(-n-m0+m*(1-m0))) / (1+n); +Q_n = a + w; + +% Easting and longitude of central meridian +E0 = 500000; +L0 = (zone-30)*6 - 3; + +% Check tolerance for reverse transformation +tolutm = pi/2 * 1.2e-10 * Q_n; +tolgeo = 0.000040; + +% Coefficients of trigonometric series + +% ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) +% 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[3] = n^3*(-26/15 + n*34/21); +% bg[4] = n^4*1237/630; + +% spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) +% 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[3] = n^3*(56/15 + n*(-136/35)); +% gb[4] = n^4*4279/630; + +% spherical to ellipsoidal N, E, KW p. 196, (69) +% 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[3] = n^3*(61/240 + n*(-103/140)); +% gtu[4] = n^4*49561/161280; + +% ellipsoidal to spherical N, E, KW p. 194, (65) +% 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[3] = n^3*(-17/480 + n*37/840); +% utg[4] = n^4*(-4397/161280); + +% With f = 1/297 we get + +bg = [-3.37077907e-3; + 4.73444769e-6; + -8.29914570e-9; + 1.58785330e-11]; + +gb = [ 3.37077588e-3; + 6.62769080e-6; + 1.78718601e-8; + 5.49266312e-11]; + +gtu = [ 8.41275991e-4; + 7.67306686e-7; + 1.21291230e-9; + 2.48508228e-12]; + +utg = [-8.41276339e-4; + -5.95619298e-8; + -1.69485209e-10; + -2.20473896e-13]; + +% Ellipsoidal latitude, longitude to spherical latitude, longitude +neg_geo = 'FALSE'; + +if B < 0 + neg_geo = 'TRUE '; +end + +Bg_r = abs(B); +[res_clensin] = clsin(bg, 4, 2*Bg_r); +Bg_r = Bg_r + res_clensin; +L0 = L0*pi / 180; +Lg_r = L - L0; + +% Spherical latitude, longitude to complementary spherical latitude +% i.e. spherical N, E +cos_BN = cos(Bg_r); +Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN); +Ep = atanh(sin(Lg_r) * cos_BN); + +%Spherical normalized N, E to ellipsoidal N, E +Np = 2 * Np; +Ep = 2 * Ep; +[dN, dE] = clksin(gtu, 4, Np, Ep); +Np = Np/2; +Ep = Ep/2; +Np = Np + dN; +Ep = Ep + dE; +N = Q_n * Np; +E = Q_n*Ep + E0; + +if neg_geo == 'TRUE ' + N = -N + 20000000; +end; + %%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%% \ No newline at end of file diff --git a/src/utils/matlab/libs/geoFunctions/check_t.m b/src/utils/matlab/libs/geoFunctions/check_t.m index 5c3cb6148..1b3ed323c 100644 --- a/src/utils/matlab/libs/geoFunctions/check_t.m +++ b/src/utils/matlab/libs/geoFunctions/check_t.m @@ -1,26 +1,26 @@ -function corrTime = check_t(time) -% CHECK_T accounting for beginning or end of week crossover. -% -% corrTime = check_t(time); -% -% Inputs: -% time - time in seconds -% -% Outputs: -% corrTime - corrected time (seconds) - -% Kai Borre 04-01-96 -% Copyright (c) by Kai Borre -%========================================================================== - -half_week = 302400; % seconds - -corrTime = time; - -if time > half_week - corrTime = time - 2*half_week; -elseif time < -half_week - corrTime = time + 2*half_week; +function corrTime = check_t(time) +% CHECK_T accounting for beginning or end of week crossover. +% +% corrTime = check_t(time); +% +% Inputs: +% time - time in seconds +% +% Outputs: +% corrTime - corrected time (seconds) + +% Kai Borre 04-01-96 +% Copyright (c) by Kai Borre +%========================================================================== + +half_week = 302400; % seconds + +corrTime = time; + +if time > half_week + corrTime = time - 2*half_week; +elseif time < -half_week + corrTime = time + 2*half_week; end - -%%%%%%% end check_t.m %%%%%%%%%%%%%%%%% + +%%%%%%% end check_t.m %%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/clksin.m b/src/utils/matlab/libs/geoFunctions/clksin.m index 99a16331e..f6f600c19 100644 --- a/src/utils/matlab/libs/geoFunctions/clksin.m +++ b/src/utils/matlab/libs/geoFunctions/clksin.m @@ -1,36 +1,36 @@ -function [re, im] = clksin(ar, degree, arg_real, arg_imag) -% Clenshaw summation of sinus with complex argument -% [re, im] = clksin(ar, degree, arg_real, arg_imag); - -% Written by Kai Borre -% December 20, 1995 -% -% See also WGS2UTM or CART2UTM -% -%========================================================================== - -sin_arg_r = sin(arg_real); -cos_arg_r = cos(arg_real); -sinh_arg_i = sinh(arg_imag); -cosh_arg_i = cosh(arg_imag); - -r = 2 * cos_arg_r * cosh_arg_i; -i =-2 * sin_arg_r * sinh_arg_i; - -hr1 = 0; hr = 0; hi1 = 0; hi = 0; - -for t = degree : -1 : 1 - hr2 = hr1; - hr1 = hr; - hi2 = hi1; - hi1 = hi; - z = ar(t) + r*hr1 - i*hi - hr2; - hi = i*hr1 + r*hi1 - hi2; - hr = z; -end - -r = sin_arg_r * cosh_arg_i; -i = cos_arg_r * sinh_arg_i; - -re = r*hr - i*hi; -im = r*hi + i*hr; +function [re, im] = clksin(ar, degree, arg_real, arg_imag) +% Clenshaw summation of sinus with complex argument +% [re, im] = clksin(ar, degree, arg_real, arg_imag); + +% Written by Kai Borre +% December 20, 1995 +% +% See also WGS2UTM or CART2UTM +% +%========================================================================== + +sin_arg_r = sin(arg_real); +cos_arg_r = cos(arg_real); +sinh_arg_i = sinh(arg_imag); +cosh_arg_i = cosh(arg_imag); + +r = 2 * cos_arg_r * cosh_arg_i; +i =-2 * sin_arg_r * sinh_arg_i; + +hr1 = 0; hr = 0; hi1 = 0; hi = 0; + +for t = degree : -1 : 1 + hr2 = hr1; + hr1 = hr; + hi2 = hi1; + hi1 = hi; + z = ar(t) + r*hr1 - i*hi - hr2; + hi = i*hr1 + r*hi1 - hi2; + hr = z; +end + +r = sin_arg_r * cosh_arg_i; +i = cos_arg_r * sinh_arg_i; + +re = r*hr - i*hi; +im = r*hi + i*hr; diff --git a/src/utils/matlab/libs/geoFunctions/clsin.m b/src/utils/matlab/libs/geoFunctions/clsin.m index 15b6d5b7b..46cf32524 100644 --- a/src/utils/matlab/libs/geoFunctions/clsin.m +++ b/src/utils/matlab/libs/geoFunctions/clsin.m @@ -1,24 +1,24 @@ -function result = clsin(ar, degree, argument) -% Clenshaw summation of sinus of argument. -% -% result = clsin(ar, degree, argument); - -% Written by Kai Borre -% December 20, 1995 -% -% See also WGS2UTM or CART2UTM -%========================================================================== - -cos_arg = 2 * cos(argument); -hr1 = 0; -hr = 0; - -for t = degree : -1 : 1 - hr2 = hr1; - hr1 = hr; - hr = ar(t) + cos_arg*hr1 - hr2; -end - +function result = clsin(ar, degree, argument) +% Clenshaw summation of sinus of argument. +% +% result = clsin(ar, degree, argument); + +% Written by Kai Borre +% December 20, 1995 +% +% See also WGS2UTM or CART2UTM +%========================================================================== + +cos_arg = 2 * cos(argument); +hr1 = 0; +hr = 0; + +for t = degree : -1 : 1 + hr2 = hr1; + hr1 = hr; + hr = ar(t) + cos_arg*hr1 - hr2; +end + result = hr * sin(argument); - -%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/deg2dms.m b/src/utils/matlab/libs/geoFunctions/deg2dms.m index c7fc195df..1f925894d 100644 --- a/src/utils/matlab/libs/geoFunctions/deg2dms.m +++ b/src/utils/matlab/libs/geoFunctions/deg2dms.m @@ -1,43 +1,43 @@ -function dmsOutput = deg2dms(deg) -% DEG2DMS Conversion of degrees to degrees, minutes, and seconds. -% The output format (dms format) is: (degrees*100 + minutes + seconds/100) - -% Written by Kai Borre -% February 7, 2001 -% Updated by Darius Plausinaitis - -%%% Save the sign for later processing -neg_arg = false; -if deg < 0 - % Only positive numbers should be used while spliting into deg/min/sec - deg = -deg; - neg_arg = true; -end - -%%% Split degrees minutes and seconds -int_deg = floor(deg); -decimal = deg - int_deg; -min_part = decimal*60; -min = floor(min_part); -sec_part = min_part - floor(min_part); -sec = sec_part*60; - -%%% Check for overflow -if sec == 60 - min = min + 1; - sec = 0; -end -if min == 60 - int_deg = int_deg + 1; - min = 0; -end - -%%% Construct the output -dmsOutput = int_deg * 100 + min + sec/100; - -%%% Correct the sign -if neg_arg == true - dmsOutput = -dmsOutput; -end - -%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%% +function dmsOutput = deg2dms(deg) +% DEG2DMS Conversion of degrees to degrees, minutes, and seconds. +% The output format (dms format) is: (degrees*100 + minutes + seconds/100) + +% Written by Kai Borre +% February 7, 2001 +% Updated by Darius Plausinaitis + +%%% Save the sign for later processing +neg_arg = false; +if deg < 0 + % Only positive numbers should be used while spliting into deg/min/sec + deg = -deg; + neg_arg = true; +end + +%%% Split degrees minutes and seconds +int_deg = floor(deg); +decimal = deg - int_deg; +min_part = decimal*60; +min = floor(min_part); +sec_part = min_part - floor(min_part); +sec = sec_part*60; + +%%% Check for overflow +if sec == 60 + min = min + 1; + sec = 0; +end +if min == 60 + int_deg = int_deg + 1; + min = 0; +end + +%%% Construct the output +dmsOutput = int_deg * 100 + min + sec/100; + +%%% Correct the sign +if neg_arg == true + dmsOutput = -dmsOutput; +end + +%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/dms2deg.m b/src/utils/matlab/libs/geoFunctions/dms2deg.m index 1b4420dd0..a8e99f9a6 100644 --- a/src/utils/matlab/libs/geoFunctions/dms2deg.m +++ b/src/utils/matlab/libs/geoFunctions/dms2deg.m @@ -1,12 +1,12 @@ - -function deg = dms2deg(dms) -% DMS2DEG Conversion of degrees, minutes, and seconds to degrees. - -% Written by Javier Arribas 2011 -% December 7, 2011 - -%if (dms(1)>=0) -deg=dms(1)+dms(2)/60+dms(3)/3600; -%else -%deg=dms(1)-dms(2)/60-dms(3)/3600; -%end + +function deg = dms2deg(dms) +% DMS2DEG Conversion of degrees, minutes, and seconds to degrees. + +% Written by Javier Arribas 2011 +% December 7, 2011 + +%if (dms(1)>=0) +deg=dms(1)+dms(2)/60+dms(3)/3600; +%else +%deg=dms(1)-dms(2)/60-dms(3)/3600; +%end diff --git a/src/utils/matlab/libs/geoFunctions/dms2mat.m b/src/utils/matlab/libs/geoFunctions/dms2mat.m index a7d5b7023..a82b60ccc 100644 --- a/src/utils/matlab/libs/geoFunctions/dms2mat.m +++ b/src/utils/matlab/libs/geoFunctions/dms2mat.m @@ -1,104 +1,104 @@ -function [dout,mout,sout] = dms2mat(dms,n) - -% DMS2MAT Converts a dms vector format to a [deg min sec] matrix -% -% [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. -% This allows compressed dms data to be expanded to a d,m,s triple, -% for easier reporting and viewing of the data. -% -% [d,m,s] = DMS2MAT(dms,n) uses n digits in the accuracy of the -% seconds calculation. n = -2 uses accuracy in the hundredths position, -% n = 0 uses accuracy in the units position. Default is n = -5. -% For further discussion of the input n, see ROUNDN. -% -% 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. -% -% See also MAT2DMS - -% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. -% Written by: E. Byrns, E. Brown -% Revision: 1.10 $Date: 2002/03/20 21:25:06 - - -if nargin == 0 - error('Incorrect number of arguments') -elseif nargin == 1 - n = -5; -end - -% Test for empty arguments - -if isempty(dms); dout = []; mout = []; sout = []; return; end - -% Test for complex arguments - -if ~isreal(dms) - warning('Imaginary parts of complex ANGLE argument ignored') - dms = real(dms); -end - -% Don't let seconds be rounded beyond the tens place. -% If you did, then 55 seconds rounds to 100, which is not good. - -if n == 2; n = 1; end - -% Construct a sign vector which has +1 when dms >= 0 and -1 when dms < 0. - -signvec = sign(dms); -signvec = signvec + (signvec == 0); % Ensure +1 when dms = 0 - -% Decompress the dms data vector - -dms = abs(dms); -d = fix(dms/100); % Degrees -m = fix(dms) - abs(100*d); % Minutes -[s,msg] = roundn(100*rem(dms,1),n); % Seconds: Truncate to roundoff error -if ~isempty(msg); error(msg); end - -% Adjust for 60 seconds or 60 minutes. -% Test for seconds > 60 to allow for round-off from roundn, -% Test for minutes > 60 as a ripple effect from seconds > 60 - - -indx = find(s >= 60); -if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = s(indx) - 60; end -indx = find(m >= 60); -if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx) - 60; end - -% Data consistency checks - -if any(m > 59) | any (m < 0) - error('Minutes must be >= 0 and <= 59') - -elseif any(s >= 60) | any( s < 0) - error('Seconds must be >= 0 and < 60') -end - -% Determine where to store the sign of the angle. It should be -% associated with the largest nonzero component of d:m:s. - -dsign = signvec .* (d~=0); -msign = signvec .* (d==0 & m~=0); -ssign = signvec .* (d==0 & m==0 & s~=0); - -% 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 -% 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. - -d = ((dsign==0) + dsign).*fix(d); % Apply signs to the degrees -m = ((msign==0) + msign).*fix(m); % Apply signs to minutes -s = ((ssign==0) + ssign).*s; % Apply signs to seconds - -% Set the output arguments - -if nargout <= 1 - dout = [d m s]; -elseif nargout == 3 - dout = d; mout = m; sout = s; -else - error('Invalid number of output arguments') -end - +function [dout,mout,sout] = dms2mat(dms,n) + +% DMS2MAT Converts a dms vector format to a [deg min sec] matrix +% +% [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. +% This allows compressed dms data to be expanded to a d,m,s triple, +% for easier reporting and viewing of the data. +% +% [d,m,s] = DMS2MAT(dms,n) uses n digits in the accuracy of the +% seconds calculation. n = -2 uses accuracy in the hundredths position, +% n = 0 uses accuracy in the units position. Default is n = -5. +% For further discussion of the input n, see ROUNDN. +% +% 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. +% +% See also MAT2DMS + +% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. +% Written by: E. Byrns, E. Brown +% Revision: 1.10 $Date: 2002/03/20 21:25:06 + + +if nargin == 0 + error('Incorrect number of arguments') +elseif nargin == 1 + n = -5; +end + +% Test for empty arguments + +if isempty(dms); dout = []; mout = []; sout = []; return; end + +% Test for complex arguments + +if ~isreal(dms) + warning('Imaginary parts of complex ANGLE argument ignored') + dms = real(dms); +end + +% Don't let seconds be rounded beyond the tens place. +% If you did, then 55 seconds rounds to 100, which is not good. + +if n == 2; n = 1; end + +% Construct a sign vector which has +1 when dms >= 0 and -1 when dms < 0. + +signvec = sign(dms); +signvec = signvec + (signvec == 0); % Ensure +1 when dms = 0 + +% Decompress the dms data vector + +dms = abs(dms); +d = fix(dms/100); % Degrees +m = fix(dms) - abs(100*d); % Minutes +[s,msg] = roundn(100*rem(dms,1),n); % Seconds: Truncate to roundoff error +if ~isempty(msg); error(msg); end + +% Adjust for 60 seconds or 60 minutes. +% Test for seconds > 60 to allow for round-off from roundn, +% Test for minutes > 60 as a ripple effect from seconds > 60 + + +indx = find(s >= 60); +if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = s(indx) - 60; end +indx = find(m >= 60); +if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx) - 60; end + +% Data consistency checks + +if any(m > 59) | any (m < 0) + error('Minutes must be >= 0 and <= 59') + +elseif any(s >= 60) | any( s < 0) + error('Seconds must be >= 0 and < 60') +end + +% Determine where to store the sign of the angle. It should be +% associated with the largest nonzero component of d:m:s. + +dsign = signvec .* (d~=0); +msign = signvec .* (d==0 & m~=0); +ssign = signvec .* (d==0 & m==0 & s~=0); + +% 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 +% 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. + +d = ((dsign==0) + dsign).*fix(d); % Apply signs to the degrees +m = ((msign==0) + msign).*fix(m); % Apply signs to minutes +s = ((ssign==0) + ssign).*s; % Apply signs to seconds + +% Set the output arguments + +if nargout <= 1 + dout = [d m s]; +elseif nargout == 3 + dout = d; mout = m; sout = s; +else + error('Invalid number of output arguments') +end + diff --git a/src/utils/matlab/libs/geoFunctions/e_r_corr.m b/src/utils/matlab/libs/geoFunctions/e_r_corr.m index 4d93d9f2f..b668a714a 100644 --- a/src/utils/matlab/libs/geoFunctions/e_r_corr.m +++ b/src/utils/matlab/libs/geoFunctions/e_r_corr.m @@ -1,31 +1,31 @@ -function X_sat_rot = e_r_corr(traveltime, X_sat) -% E_R_CORR Returns rotated satellite ECEF coordinates due to Earth -% rotation during signal travel time -% -% X_sat_rot = e_r_corr(traveltime, X_sat); -% -% Inputs: -% travelTime - signal travel time -% X_sat - satellite's ECEF coordinates -% -% Outputs: -% X_sat_rot - rotated satellite's coordinates (ECEF) - -% Written by Kai Borre -% Copyright (c) by Kai Borre -%========================================================================== - -Omegae_dot = 7.292115147e-5; % rad/sec - -%--- Find rotation angle -------------------------------------------------- -omegatau = Omegae_dot * traveltime; - -%--- Make a rotation matrix ----------------------------------------------- -R3 = [ cos(omegatau) sin(omegatau) 0; - -sin(omegatau) cos(omegatau) 0; - 0 0 1]; - -%--- Do the rotation ------------------------------------------------------ -X_sat_rot = R3 * X_sat; - -%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%% +function X_sat_rot = e_r_corr(traveltime, X_sat) +% E_R_CORR Returns rotated satellite ECEF coordinates due to Earth +% rotation during signal travel time +% +% X_sat_rot = e_r_corr(traveltime, X_sat); +% +% Inputs: +% travelTime - signal travel time +% X_sat - satellite's ECEF coordinates +% +% Outputs: +% X_sat_rot - rotated satellite's coordinates (ECEF) + +% Written by Kai Borre +% Copyright (c) by Kai Borre +%========================================================================== + +Omegae_dot = 7.292115147e-5; % rad/sec + +%--- Find rotation angle -------------------------------------------------- +omegatau = Omegae_dot * traveltime; + +%--- Make a rotation matrix ----------------------------------------------- +R3 = [ cos(omegatau) sin(omegatau) 0; + -sin(omegatau) cos(omegatau) 0; + 0 0 1]; + +%--- Do the rotation ------------------------------------------------------ +X_sat_rot = R3 * X_sat; + +%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/findUtmZone.m b/src/utils/matlab/libs/geoFunctions/findUtmZone.m index 1294c7837..e5717d635 100644 --- a/src/utils/matlab/libs/geoFunctions/findUtmZone.m +++ b/src/utils/matlab/libs/geoFunctions/findUtmZone.m @@ -1,69 +1,69 @@ -function utmZone = findUtmZone(latitude, longitude) -% Function finds the UTM zone number for given longitude and latitude. -% 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 -% 84 (84 degree North). -% -% utmZone = findUtmZone(latitude, longitude); -% -% Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not -% 15 deg 30 min). - -%-------------------------------------------------------------------------- -% SoftGNSS v3.0 -% -% Copyright (C) Darius Plausinaitis -% Written by Darius Plausinaitis -%-------------------------------------------------------------------------- -%This program is free software; you can redistribute it and/or -%modify it under the terms of the GNU General Public License -%as published by the Free Software Foundation; either version 2 -%of the License, or (at your option) any later version. -% -%This program is distributed in the hope that it will be useful, -%but WITHOUT ANY WARRANTY; without even the implied warranty of -%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%GNU General Public License for more details. -% -%You should have received a copy of the GNU General Public License -%along with this program; if not, write to the Free Software -%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, -%USA. -%========================================================================== - -%% Check value bounds ===================================================== - -if ((longitude > 180) || (longitude < -180)) - error('Longitude value exceeds limits (-180:180).'); -end - -if ((latitude > 84) || (latitude < -80)) - error('Latitude value exceeds limits (-80:84).'); -end - -%% Find zone ============================================================== - -% Start at 180 deg west = -180 deg - -utmZone = fix((180 + longitude)/ 6) + 1; - -%% Correct zone numbers for particular areas ============================== - -if (latitude > 72) - % Corrections for zones 31 33 35 37 - if ((longitude >= 0) && (longitude < 9)) - utmZone = 31; - elseif ((longitude >= 9) && (longitude < 21)) - utmZone = 33; - elseif ((longitude >= 21) && (longitude < 33)) - utmZone = 35; - elseif ((longitude >= 33) && (longitude < 42)) - utmZone = 37; - end - -elseif ((latitude >= 56) && (latitude < 64)) - % Correction for zone 32 - if ((longitude >= 3) && (longitude < 12)) - utmZone = 32; - end -end +function utmZone = findUtmZone(latitude, longitude) +% Function finds the UTM zone number for given longitude and latitude. +% 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 +% 84 (84 degree North). +% +% utmZone = findUtmZone(latitude, longitude); +% +% Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not +% 15 deg 30 min). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%========================================================================== + +%% Check value bounds ===================================================== + +if ((longitude > 180) || (longitude < -180)) + error('Longitude value exceeds limits (-180:180).'); +end + +if ((latitude > 84) || (latitude < -80)) + error('Latitude value exceeds limits (-80:84).'); +end + +%% Find zone ============================================================== + +% Start at 180 deg west = -180 deg + +utmZone = fix((180 + longitude)/ 6) + 1; + +%% Correct zone numbers for particular areas ============================== + +if (latitude > 72) + % Corrections for zones 31 33 35 37 + if ((longitude >= 0) && (longitude < 9)) + utmZone = 31; + elseif ((longitude >= 9) && (longitude < 21)) + utmZone = 33; + elseif ((longitude >= 21) && (longitude < 33)) + utmZone = 35; + elseif ((longitude >= 33) && (longitude < 42)) + utmZone = 37; + end + +elseif ((latitude >= 56) && (latitude < 64)) + % Correction for zone 32 + if ((longitude >= 3) && (longitude < 12)) + utmZone = 32; + end +end diff --git a/src/utils/matlab/libs/geoFunctions/geo2cart.m b/src/utils/matlab/libs/geoFunctions/geo2cart.m index 3297e966a..90cd6a10f 100644 --- a/src/utils/matlab/libs/geoFunctions/geo2cart.m +++ b/src/utils/matlab/libs/geoFunctions/geo2cart.m @@ -1,46 +1,46 @@ -function [X, Y, Z] = geo2cart(phi, lambda, h, i) -% GEO2CART Conversion of geographical coordinates (phi, lambda, h) to -% Cartesian coordinates (X, Y, Z). -% -% [X, Y, Z] = geo2cart(phi, lambda, h, i); -% -% Format for phi and lambda: [degrees minutes seconds]. -% h, X, Y, and Z are in meters. -% -% Choices i of Reference Ellipsoid -% 1. International Ellipsoid 1924 -% 2. International Ellipsoid 1967 -% 3. World Geodetic System 1972 -% 4. Geodetic Reference System 1980 -% 5. World Geodetic System 1984 -% -% Inputs: -% phi - geocentric latitude (format [degrees minutes seconds]) -% lambda - geocentric longitude (format [degrees minutes seconds]) -% h - height -% i - reference ellipsoid type -% -% Outputs: -% X, Y, Z - Cartesian coordinates (meters) - -% Kai Borre 10-13-98 -% Copyright (c) by Kai Borre -%========================================================================== - -b = phi(1) + phi(2)/60 + phi(3)/3600; -b = b*pi / 180; -l = lambda(1) + lambda(2)/60 + lambda(3)/3600; -l = l*pi / 180; - -a = [6378388 6378160 6378135 6378137 6378137]; -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); -c = a(i) * sqrt(1+ex2); -N = c / sqrt(1 + ex2*cos(b)^2); - -X = (N+h) * cos(b) * cos(l); -Y = (N+h) * cos(b) * sin(l); +function [X, Y, Z] = geo2cart(phi, lambda, h, i) +% GEO2CART Conversion of geographical coordinates (phi, lambda, h) to +% Cartesian coordinates (X, Y, Z). +% +% [X, Y, Z] = geo2cart(phi, lambda, h, i); +% +% Format for phi and lambda: [degrees minutes seconds]. +% h, X, Y, and Z are in meters. +% +% Choices i of Reference Ellipsoid +% 1. International Ellipsoid 1924 +% 2. International Ellipsoid 1967 +% 3. World Geodetic System 1972 +% 4. Geodetic Reference System 1980 +% 5. World Geodetic System 1984 +% +% Inputs: +% phi - geocentric latitude (format [degrees minutes seconds]) +% lambda - geocentric longitude (format [degrees minutes seconds]) +% h - height +% i - reference ellipsoid type +% +% Outputs: +% X, Y, Z - Cartesian coordinates (meters) + +% Kai Borre 10-13-98 +% Copyright (c) by Kai Borre +%========================================================================== + +b = phi(1) + phi(2)/60 + phi(3)/3600; +b = b*pi / 180; +l = lambda(1) + lambda(2)/60 + lambda(3)/3600; +l = l*pi / 180; + +a = [6378388 6378160 6378135 6378137 6378137]; +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); +c = a(i) * sqrt(1+ex2); +N = c / sqrt(1 + ex2*cos(b)^2); + +X = (N+h) * cos(b) * cos(l); +Y = (N+h) * cos(b) * sin(l); Z = ((1-f(i))^2*N + h) * sin(b); - -%%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/leastSquarePos.m b/src/utils/matlab/libs/geoFunctions/leastSquarePos.m index 6f5c46f4f..dee7e2180 100644 --- a/src/utils/matlab/libs/geoFunctions/leastSquarePos.m +++ b/src/utils/matlab/libs/geoFunctions/leastSquarePos.m @@ -1,111 +1,111 @@ -function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) -% Function calculates the Least Square Solution. -% -% [pos, el, az, dop] = leastSquarePos(satpos, obs, settings); -% -% Inputs: -% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - -% one column per satellite) -% obs - Observations - the pseudorange measurements to each -% satellite: -% (e.g. [20000000 21000000 .... .... .... .... ....]) -% settings - receiver settings -% -% Outputs: -% pos - receiver position and receiver clock error -% (in ECEF system: [X, Y, Z, dt]) -% el - Satellites elevation angles (degrees) -% az - Satellites azimuth angles (degrees) -% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) - -%-------------------------------------------------------------------------- -% SoftGNSS v3.0 -%-------------------------------------------------------------------------- -%Based on Kai Borre -%Copyright (c) by Kai Borre -%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen -%========================================================================== - -%=== Initialization ======================================================= -nmbOfIterations = 7; - -dtr = pi/180; -pos = zeros(4, 1); -X = satpos; -nmbOfSatellites = size(satpos, 2); - -A = zeros(nmbOfSatellites, 4); -omc = zeros(nmbOfSatellites, 1); -az = zeros(1, nmbOfSatellites); -el = az; - -%=== Iteratively find receiver position =================================== -for iter = 1:nmbOfIterations - - for i = 1:nmbOfSatellites - if iter == 1 - %--- Initialize variables at the first iteration -------------- - Rot_X = X(:, i); - trop = 2; - else - %--- Update equations ----------------------------------------- - rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... - (X(3, i) - pos(3))^2; - traveltime = sqrt(rho2) / settings.c ; - - %--- Correct satellite position (do to earth rotation) -------- - Rot_X = e_r_corr(traveltime, X(:, i)); - - %--- Find the elevation angel of the satellite ---------------- - [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); - - if (settings.useTropCorr == 1) - %--- Calculate tropospheric correction -------------------- - trop = tropo(sin(el(i) * dtr), ... - 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); - else - % Do not calculate or apply the tropospheric corrections - trop = 0; - end - end % if iter == 1 ... ... else - - %--- Apply the corrections ---------------------------------------- - omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); - - %--- Construct the A matrix --------------------------------------- - A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... - (-(Rot_X(2) - pos(2))) / obs(i) ... - (-(Rot_X(3) - pos(3))) / obs(i) ... - 1 ]; - end % for i = 1:nmbOfSatellites - - % These lines allow the code to exit gracefully in case of any errors - if rank(A) ~= 4 - pos = zeros(1, 4); - return - end - - %--- Find position update --------------------------------------------- - x = A \ omc; - - %--- Apply position update -------------------------------------------- - pos = pos + x; - -end % for iter = 1:nmbOfIterations - -pos = pos'; - -%=== Calculate Dilution Of Precision ====================================== -if nargout == 4 - %--- Initialize output ------------------------------------------------ - dop = zeros(1, 5); - - %--- Calculate DOP ---------------------------------------------------- - Q = inv(A'*A); - - dop(1) = sqrt(trace(Q)); % GDOP - dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP - dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP - dop(4) = sqrt(Q(3,3)); % VDOP - dop(5) = sqrt(Q(4,4)); % TDOP -end +function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) +% Function calculates the Least Square Solution. +% +% [pos, el, az, dop] = leastSquarePos(satpos, obs, settings); +% +% Inputs: +% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - +% one column per satellite) +% obs - Observations - the pseudorange measurements to each +% satellite: +% (e.g. [20000000 21000000 .... .... .... .... ....]) +% settings - receiver settings +% +% Outputs: +% pos - receiver position and receiver clock error +% (in ECEF system: [X, Y, Z, dt]) +% el - Satellites elevation angles (degrees) +% az - Satellites azimuth angles (degrees) +% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +%-------------------------------------------------------------------------- +%Based on Kai Borre +%Copyright (c) by Kai Borre +%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +%========================================================================== + +%=== Initialization ======================================================= +nmbOfIterations = 7; + +dtr = pi/180; +pos = zeros(4, 1); +X = satpos; +nmbOfSatellites = size(satpos, 2); + +A = zeros(nmbOfSatellites, 4); +omc = zeros(nmbOfSatellites, 1); +az = zeros(1, nmbOfSatellites); +el = az; + +%=== Iteratively find receiver position =================================== +for iter = 1:nmbOfIterations + + for i = 1:nmbOfSatellites + if iter == 1 + %--- Initialize variables at the first iteration -------------- + Rot_X = X(:, i); + trop = 2; + else + %--- Update equations ----------------------------------------- + rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2; + traveltime = sqrt(rho2) / settings.c ; + + %--- Correct satellite position (do to earth rotation) -------- + Rot_X = e_r_corr(traveltime, X(:, i)); + + %--- Find the elevation angel of the satellite ---------------- + [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); + + if (settings.useTropCorr == 1) + %--- Calculate tropospheric correction -------------------- + trop = tropo(sin(el(i) * dtr), ... + 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + else + % Do not calculate or apply the tropospheric corrections + trop = 0; + end + end % if iter == 1 ... ... else + + %--- Apply the corrections ---------------------------------------- + omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); + + %--- Construct the A matrix --------------------------------------- + A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... + (-(Rot_X(2) - pos(2))) / obs(i) ... + (-(Rot_X(3) - pos(3))) / obs(i) ... + 1 ]; + end % for i = 1:nmbOfSatellites + + % These lines allow the code to exit gracefully in case of any errors + if rank(A) ~= 4 + pos = zeros(1, 4); + return + end + + %--- Find position update --------------------------------------------- + x = A \ omc; + + %--- Apply position update -------------------------------------------- + pos = pos + x; + +end % for iter = 1:nmbOfIterations + +pos = pos'; + +%=== Calculate Dilution Of Precision ====================================== +if nargout == 4 + %--- Initialize output ------------------------------------------------ + dop = zeros(1, 5); + + %--- Calculate DOP ---------------------------------------------------- + Q = inv(A'*A); + + dop(1) = sqrt(trace(Q)); % GDOP + dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP + dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP + dop(4) = sqrt(Q(3,3)); % VDOP + dop(5) = sqrt(Q(4,4)); % TDOP +end diff --git a/src/utils/matlab/libs/geoFunctions/mat2dms.m b/src/utils/matlab/libs/geoFunctions/mat2dms.m index 1c3787ab6..fad0eca81 100644 --- a/src/utils/matlab/libs/geoFunctions/mat2dms.m +++ b/src/utils/matlab/libs/geoFunctions/mat2dms.m @@ -1,124 +1,124 @@ -function dmsvec = mat2dms(d,m,s,n) -% MAT2DMS Converts a [deg min sec] matrix to vector format -% -% 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. -% 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. -% The inputs d, m and s must be of equal size. Minutes and -% second must be between 0 and 60. -% -% dms = MAT2DMS(mat) assumes and input matrix of [d m s]. This is -% useful only for single column vectors for d, m and s. -% -% dms = MAT2DMS(d,m) and dms = MAT2DMS([d m]) assume that seconds -% are zero, s = 0. -% -% dms = MAT2DMS(d,m,s,n) uses n as the accuracy of the seconds -% calculation. n = -2 uses accuracy in the hundredths position, -% n = 0 uses accuracy in the units position. Default is n = -5. -% For further discussion of the input n, see ROUNDN. -% -% See also DMS2MAT - -% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. -% Written by: E. Byrns, E. Brown -% Revision: 1.10 Date: 2002/03/20 21:25:51 - - -if nargin == 0 - error('Incorrect number of arguments') - -elseif nargin==1 - if size(d,2)== 3 - s = d(:,3); m = d(:,2); d = d(:,1); - elseif size(d,2)== 2 - m = d(:,2); d = d(:,1); s = zeros(size(d)); - elseif size(d,2) == 0 - d = []; m = []; s = []; - else - error('Single input matrices must be n-by-2 or n-by-3.'); - end - n = -5; - -elseif nargin == 2 - s = zeros(size(d)); - n = -5; - -elseif nargin == 3 - n = -5; -end - -% Test for empty arguments - -if isempty(d) & isempty(m) & isempty(s); dmsvec = []; return; end - -% Don't let seconds be rounded beyond the tens place. -% If you did, then 55 seconds rounds to 100, which is not good. - -if n == 2; n = 1; end - -% Complex argument tests - -if any([~isreal(d) ~isreal(m) ~isreal(s)]) - warning('Imaginary parts of complex ANGLE argument ignored') - d = real(d); m = real(m); s = real(s); -end - -% Dimension and value tests - -if ~isequal(size(d),size(m),size(s)) - error('Inconsistent dimensions for input arguments') -elseif any(rem(d(~isnan(d)),1) ~= 0 | rem(m(~isnan(m)),1) ~= 0) - error('Degrees and minutes must be integers') -end - -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 - % 60 minutes, but the error message -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 -end - -% 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) ) - error('Multiple negative entries in a DMS specification') -elseif any((s<0 & (m~=0 | d~= 0)) | (m<0 & d~=0)) - error('Incorrect negative DMS specification') -end - -% Construct a sign vector which has +1 when -% 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 - -negvec = (d<0) | (m<0) | (s<0); -signvec = ~negvec - negvec; - -% Convert to all positive numbers. Allows for easier -% adjusting at 60 seconds and 60 minutes - -d = abs(d); m = abs(m); s = abs(s); - -% Truncate seconds to a specified accuracy to eliminate round-off errors - -[s,msg] = roundn(s,n); -if ~isempty(msg); error(msg); end - -% 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. -% This round-off effect has happened though. - -indx = find(s >= 60); -if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = 0; end - -% 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), -% thus, the test below includes the greater than condition. - -indx = find(m >= 60); -if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx)-60; end - -% Construct the dms vector format - -dmsvec = signvec .* (100*d + m + s/100); +function dmsvec = mat2dms(d,m,s,n) +% MAT2DMS Converts a [deg min sec] matrix to vector format +% +% 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. +% 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. +% The inputs d, m and s must be of equal size. Minutes and +% second must be between 0 and 60. +% +% dms = MAT2DMS(mat) assumes and input matrix of [d m s]. This is +% useful only for single column vectors for d, m and s. +% +% dms = MAT2DMS(d,m) and dms = MAT2DMS([d m]) assume that seconds +% are zero, s = 0. +% +% dms = MAT2DMS(d,m,s,n) uses n as the accuracy of the seconds +% calculation. n = -2 uses accuracy in the hundredths position, +% n = 0 uses accuracy in the units position. Default is n = -5. +% For further discussion of the input n, see ROUNDN. +% +% See also DMS2MAT + +% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. +% Written by: E. Byrns, E. Brown +% Revision: 1.10 Date: 2002/03/20 21:25:51 + + +if nargin == 0 + error('Incorrect number of arguments') + +elseif nargin==1 + if size(d,2)== 3 + s = d(:,3); m = d(:,2); d = d(:,1); + elseif size(d,2)== 2 + m = d(:,2); d = d(:,1); s = zeros(size(d)); + elseif size(d,2) == 0 + d = []; m = []; s = []; + else + error('Single input matrices must be n-by-2 or n-by-3.'); + end + n = -5; + +elseif nargin == 2 + s = zeros(size(d)); + n = -5; + +elseif nargin == 3 + n = -5; +end + +% Test for empty arguments + +if isempty(d) & isempty(m) & isempty(s); dmsvec = []; return; end + +% Don't let seconds be rounded beyond the tens place. +% If you did, then 55 seconds rounds to 100, which is not good. + +if n == 2; n = 1; end + +% Complex argument tests + +if any([~isreal(d) ~isreal(m) ~isreal(s)]) + warning('Imaginary parts of complex ANGLE argument ignored') + d = real(d); m = real(m); s = real(s); +end + +% Dimension and value tests + +if ~isequal(size(d),size(m),size(s)) + error('Inconsistent dimensions for input arguments') +elseif any(rem(d(~isnan(d)),1) ~= 0 | rem(m(~isnan(m)),1) ~= 0) + error('Degrees and minutes must be integers') +end + +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 + % 60 minutes, but the error message +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 +end + +% 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) ) + error('Multiple negative entries in a DMS specification') +elseif any((s<0 & (m~=0 | d~= 0)) | (m<0 & d~=0)) + error('Incorrect negative DMS specification') +end + +% Construct a sign vector which has +1 when +% 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 + +negvec = (d<0) | (m<0) | (s<0); +signvec = ~negvec - negvec; + +% Convert to all positive numbers. Allows for easier +% adjusting at 60 seconds and 60 minutes + +d = abs(d); m = abs(m); s = abs(s); + +% Truncate seconds to a specified accuracy to eliminate round-off errors + +[s,msg] = roundn(s,n); +if ~isempty(msg); error(msg); end + +% 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. +% This round-off effect has happened though. + +indx = find(s >= 60); +if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = 0; end + +% 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), +% thus, the test below includes the greater than condition. + +indx = find(m >= 60); +if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx)-60; end + +% Construct the dms vector format + +dmsvec = signvec .* (100*d + m + s/100); diff --git a/src/utils/matlab/libs/geoFunctions/roundn.m b/src/utils/matlab/libs/geoFunctions/roundn.m index ef9c53f4c..ca2eb8998 100644 --- a/src/utils/matlab/libs/geoFunctions/roundn.m +++ b/src/utils/matlab/libs/geoFunctions/roundn.m @@ -1,46 +1,46 @@ -function [x,msg] = roundn(x,n) - -% ROUNDN Rounds input data at specified power of 10 -% -% 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 -% of tens position. For example, n = -2 rounds the input data to -% the 10E-2 (hundredths) position. -% -% [y,msg] = ROUNDN(...) returns the text of any error condition -% encountered in the output variable msg. -% -% See also ROUND - -% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. -% Written by: E. Byrns, E. Brown -% Revision: 1.9 Date: 2002/03/20 21:26:19 - -msg = []; % Initialize output - -if nargin == 0 - error('Incorrect number of arguments') -elseif nargin == 1 - n = -2; -end - -% Test for scalar n - -if max(size(n)) ~= 1 - msg = 'Scalar accuracy required'; - if nargout < 2; error(msg); end - return -elseif ~isreal(n) - warning('Imaginary part of complex N argument ignored') - n = real(n); -end - -% Compute the exponential factors for rounding at specified -% power of 10. Ensure that n is an integer. - -factors = 10 ^ (fix(-n)); - -% Set the significant digits for the input data - -x = round(x * factors) / factors; +function [x,msg] = roundn(x,n) + +% ROUNDN Rounds input data at specified power of 10 +% +% 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 +% of tens position. For example, n = -2 rounds the input data to +% the 10E-2 (hundredths) position. +% +% [y,msg] = ROUNDN(...) returns the text of any error condition +% encountered in the output variable msg. +% +% See also ROUND + +% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. +% Written by: E. Byrns, E. Brown +% Revision: 1.9 Date: 2002/03/20 21:26:19 + +msg = []; % Initialize output + +if nargin == 0 + error('Incorrect number of arguments') +elseif nargin == 1 + n = -2; +end + +% Test for scalar n + +if max(size(n)) ~= 1 + msg = 'Scalar accuracy required'; + if nargout < 2; error(msg); end + return +elseif ~isreal(n) + warning('Imaginary part of complex N argument ignored') + n = real(n); +end + +% Compute the exponential factors for rounding at specified +% power of 10. Ensure that n is an integer. + +factors = 10 ^ (fix(-n)); + +% Set the significant digits for the input data + +x = round(x * factors) / factors; diff --git a/src/utils/matlab/libs/geoFunctions/satpos.m b/src/utils/matlab/libs/geoFunctions/satpos.m index 11f4f101e..e359a5df1 100644 --- a/src/utils/matlab/libs/geoFunctions/satpos.m +++ b/src/utils/matlab/libs/geoFunctions/satpos.m @@ -1,138 +1,138 @@ -function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ... - eph, settings) -% SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for -% given ephemeris EPH. Coordinates are computed for each satellite in the -% list PRNLIST. -%[ satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); -% -% Inputs: -% transmitTime - transmission time -% prnList - list of PRN-s to be processed -% eph - ephemerides of satellites -% settings - receiver settings -% -% Outputs: -% satPositions - position of satellites (in ECEF system [X; Y; Z;]) -% satClkCorr - correction of satellite clocks - -%-------------------------------------------------------------------------- -% SoftGNSS v3.0 -%-------------------------------------------------------------------------- -% Based on Kai Borre 04-09-96 -% Copyright (c) by Kai Borre -% Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen - -%% Initialize constants =================================================== -numOfSatellites = size(prnList, 2); - -% GPS constatns - -gpsPi = 3.1415926535898; % Pi used in the GPS coordinate -% system - -%--- Constants for satellite position calculation ------------------------- -Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s] -GM = 3.986005e14; % Universal gravitational constant times -% the mass of the Earth, [m^3/s^2] -F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)] - -%% Initialize results ===================================================== -satClkCorr = zeros(1, numOfSatellites); -satPositions = zeros(3, numOfSatellites); - -%% Process each satellite ================================================= - -for satNr = 1 : numOfSatellites - - prn = prnList(satNr); - - %% Find initial satellite clock correction -------------------------------- - - %--- Find time difference --------------------------------------------- - dt = check_t(transmitTime - eph(prn).t_oc); - - %--- Calculate clock correction --------------------------------------- - satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... - eph(prn).a_f0 - ... - eph(prn).T_GD; - - time = transmitTime - satClkCorr(satNr); - - %% Find satellite's position ---------------------------------------------- - - %Restore semi-major axis - a = eph(prn).sqrtA * eph(prn).sqrtA; - - %Time correction - tk = check_t(time - eph(prn).t_oe); - - %Initial mean motion - n0 = sqrt(GM / a^3); - %Mean motion - n = n0 + eph(prn).deltan; - - %Mean anomaly - M = eph(prn).M_0 + n * tk; - %Reduce mean anomaly to between 0 and 360 deg - M = rem(M + 2*gpsPi, 2*gpsPi); - - %Initial guess of eccentric anomaly - E = M; - - %--- Iteratively compute eccentric anomaly ---------------------------- - for ii = 1:10 - E_old = E; - E = M + eph(prn).e * sin(E); - dE = rem(E - E_old, 2*gpsPi); - - if abs(dE) < 1.e-12 - % Necessary precision is reached, exit from the loop - break; - end - end - - %Reduce eccentric anomaly to between 0 and 360 deg - E = rem(E + 2*gpsPi, 2*gpsPi); - - %Compute relativistic correction term - dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E); - - %Calculate the true anomaly - nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e); - - %Compute angle phi - phi = nu + eph(prn).omega; - %Reduce phi to between 0 and 360 deg - phi = rem(phi, 2*gpsPi); - - %Correct argument of latitude - u = phi + ... - eph(prn).C_uc * cos(2*phi) + ... - eph(prn).C_us * sin(2*phi); - %Correct radius - r = a * (1 - eph(prn).e*cos(E)) + ... - eph(prn).C_rc * cos(2*phi) + ... - eph(prn).C_rs * sin(2*phi); - %Correct inclination - i = eph(prn).i_0 + eph(prn).iDot * tk + ... - eph(prn).C_ic * cos(2*phi) + ... - eph(prn).C_is * sin(2*phi); - - %Compute the angle between the ascending node and the Greenwich meridian - Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ... - Omegae_dot * eph(prn).t_oe; - %Reduce to between 0 and 360 deg - Omega = rem(Omega + 2*gpsPi, 2*gpsPi); - - %--- Compute satellite coordinates ------------------------------------ - 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(3, satNr) = sin(u)*r * sin(i); - - - %% Include relativistic correction in clock correction -------------------- - satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... - eph(prn).a_f0 - ... - eph(prn).T_GD + dtr; - -end % for satNr = 1 : numOfSatellites +function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ... + eph, settings) +% SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for +% given ephemeris EPH. Coordinates are computed for each satellite in the +% list PRNLIST. +%[ satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); +% +% Inputs: +% transmitTime - transmission time +% prnList - list of PRN-s to be processed +% eph - ephemerides of satellites +% settings - receiver settings +% +% Outputs: +% satPositions - position of satellites (in ECEF system [X; Y; Z;]) +% satClkCorr - correction of satellite clocks + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +%-------------------------------------------------------------------------- +% Based on Kai Borre 04-09-96 +% Copyright (c) by Kai Borre +% Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen + +%% Initialize constants =================================================== +numOfSatellites = size(prnList, 2); + +% GPS constatns + +gpsPi = 3.1415926535898; % Pi used in the GPS coordinate +% system + +%--- Constants for satellite position calculation ------------------------- +Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s] +GM = 3.986005e14; % Universal gravitational constant times +% the mass of the Earth, [m^3/s^2] +F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)] + +%% Initialize results ===================================================== +satClkCorr = zeros(1, numOfSatellites); +satPositions = zeros(3, numOfSatellites); + +%% Process each satellite ================================================= + +for satNr = 1 : numOfSatellites + + prn = prnList(satNr); + + %% Find initial satellite clock correction -------------------------------- + + %--- Find time difference --------------------------------------------- + dt = check_t(transmitTime - eph(prn).t_oc); + + %--- Calculate clock correction --------------------------------------- + satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... + eph(prn).a_f0 - ... + eph(prn).T_GD; + + time = transmitTime - satClkCorr(satNr); + + %% Find satellite's position ---------------------------------------------- + + %Restore semi-major axis + a = eph(prn).sqrtA * eph(prn).sqrtA; + + %Time correction + tk = check_t(time - eph(prn).t_oe); + + %Initial mean motion + n0 = sqrt(GM / a^3); + %Mean motion + n = n0 + eph(prn).deltan; + + %Mean anomaly + M = eph(prn).M_0 + n * tk; + %Reduce mean anomaly to between 0 and 360 deg + M = rem(M + 2*gpsPi, 2*gpsPi); + + %Initial guess of eccentric anomaly + E = M; + + %--- Iteratively compute eccentric anomaly ---------------------------- + for ii = 1:10 + E_old = E; + E = M + eph(prn).e * sin(E); + dE = rem(E - E_old, 2*gpsPi); + + if abs(dE) < 1.e-12 + % Necessary precision is reached, exit from the loop + break; + end + end + + %Reduce eccentric anomaly to between 0 and 360 deg + E = rem(E + 2*gpsPi, 2*gpsPi); + + %Compute relativistic correction term + dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E); + + %Calculate the true anomaly + nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e); + + %Compute angle phi + phi = nu + eph(prn).omega; + %Reduce phi to between 0 and 360 deg + phi = rem(phi, 2*gpsPi); + + %Correct argument of latitude + u = phi + ... + eph(prn).C_uc * cos(2*phi) + ... + eph(prn).C_us * sin(2*phi); + %Correct radius + r = a * (1 - eph(prn).e*cos(E)) + ... + eph(prn).C_rc * cos(2*phi) + ... + eph(prn).C_rs * sin(2*phi); + %Correct inclination + i = eph(prn).i_0 + eph(prn).iDot * tk + ... + eph(prn).C_ic * cos(2*phi) + ... + eph(prn).C_is * sin(2*phi); + + %Compute the angle between the ascending node and the Greenwich meridian + Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ... + Omegae_dot * eph(prn).t_oe; + %Reduce to between 0 and 360 deg + Omega = rem(Omega + 2*gpsPi, 2*gpsPi); + + %--- Compute satellite coordinates ------------------------------------ + 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(3, satNr) = sin(u)*r * sin(i); + + + %% Include relativistic correction in clock correction -------------------- + satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... + eph(prn).a_f0 - ... + eph(prn).T_GD + dtr; + +end % for satNr = 1 : numOfSatellites diff --git a/src/utils/matlab/libs/geoFunctions/togeod.m b/src/utils/matlab/libs/geoFunctions/togeod.m index 9f7812d41..b9d7bbb28 100644 --- a/src/utils/matlab/libs/geoFunctions/togeod.m +++ b/src/utils/matlab/libs/geoFunctions/togeod.m @@ -1,109 +1,109 @@ -function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) -% TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, -% height given Cartesian coordinates X,Y,Z, and reference ellipsoid -% values semi-major axis (a) and the inverse of flattening (finv). -% -% [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 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 -% same as the units of X,Y,Z,a. -% -% Inputs: -% a - semi-major axis of the reference ellipsoid -% finv - inverse of flattening of the reference ellipsoid -% X,Y,Z - Cartesian coordinates -% -% Outputs: -% dphi - latitude -% dlambda - longitude -% h - height above reference ellipsoid - -% Copyright (C) 1987 C. Goad, Columbus, Ohio -% Reprinted with permission of author, 1996 -% Fortran code translated into MATLAB -% Kai Borre 03-30-96 -%========================================================================== - -h = 0; -tolsq = 1.e-10; -maxit = 10; - -% compute radians-to-degree factor -rtd = 180/pi; - -% compute square of eccentricity -if finv < 1.e-20 - esq = 0; -else - esq = (2 - 1/finv) / finv; -end - -oneesq = 1 - esq; - -% first guess -% P is distance from spin axis -P = sqrt(X^2+Y^2); -% direct calculation of longitude - -if P > 1.e-20 - dlambda = atan2(Y,X) * rtd; -else - dlambda = 0; -end - -if (dlambda < 0) - dlambda = dlambda + 360; -end - -% r is distance from origin (0,0,0) -r = sqrt(P^2 + Z^2); - -if r > 1.e-20 - sinphi = Z/r; -else - sinphi = 0; -end - -dphi = asin(sinphi); - -% initial value of height = distance from origin minus -% approximate distance from origin to surface of ellipsoid -if r < 1.e-20 - h = 0; - return -end - -h = r - a*(1-sinphi*sinphi/finv); - -% iterate -for i = 1:maxit - sinphi = sin(dphi); - cosphi = cos(dphi); - - % compute radius of curvature in prime vertical direction - N_phi = a/sqrt(1-esq*sinphi*sinphi); - - % compute residuals in P and Z - dP = P - (N_phi + h) * cosphi; - dZ = Z - (N_phi*oneesq + h) * sinphi; - - % update height and latitude - h = h + (sinphi*dZ + cosphi*dP); - dphi = dphi + (cosphi*dZ - sinphi*dP)/(N_phi + h); - - % test for convergence - if (dP*dP + dZ*dZ < tolsq) - break; - end - - % Not Converged--Warn user - if i == maxit - fprintf([' Problem in TOGEOD, did not converge in %2.0f',... - ' iterations\n'], i); - end -end % for i = 1:maxit - -dphi = dphi * rtd; -%%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%% +function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) +% TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, +% height given Cartesian coordinates X,Y,Z, and reference ellipsoid +% values semi-major axis (a) and the inverse of flattening (finv). +% +% [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 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 +% same as the units of X,Y,Z,a. +% +% Inputs: +% a - semi-major axis of the reference ellipsoid +% finv - inverse of flattening of the reference ellipsoid +% X,Y,Z - Cartesian coordinates +% +% Outputs: +% dphi - latitude +% dlambda - longitude +% h - height above reference ellipsoid + +% Copyright (C) 1987 C. Goad, Columbus, Ohio +% Reprinted with permission of author, 1996 +% Fortran code translated into MATLAB +% Kai Borre 03-30-96 +%========================================================================== + +h = 0; +tolsq = 1.e-10; +maxit = 10; + +% compute radians-to-degree factor +rtd = 180/pi; + +% compute square of eccentricity +if finv < 1.e-20 + esq = 0; +else + esq = (2 - 1/finv) / finv; +end + +oneesq = 1 - esq; + +% first guess +% P is distance from spin axis +P = sqrt(X^2+Y^2); +% direct calculation of longitude + +if P > 1.e-20 + dlambda = atan2(Y,X) * rtd; +else + dlambda = 0; +end + +if (dlambda < 0) + dlambda = dlambda + 360; +end + +% r is distance from origin (0,0,0) +r = sqrt(P^2 + Z^2); + +if r > 1.e-20 + sinphi = Z/r; +else + sinphi = 0; +end + +dphi = asin(sinphi); + +% initial value of height = distance from origin minus +% approximate distance from origin to surface of ellipsoid +if r < 1.e-20 + h = 0; + return +end + +h = r - a*(1-sinphi*sinphi/finv); + +% iterate +for i = 1:maxit + sinphi = sin(dphi); + cosphi = cos(dphi); + + % compute radius of curvature in prime vertical direction + N_phi = a/sqrt(1-esq*sinphi*sinphi); + + % compute residuals in P and Z + dP = P - (N_phi + h) * cosphi; + dZ = Z - (N_phi*oneesq + h) * sinphi; + + % update height and latitude + h = h + (sinphi*dZ + cosphi*dP); + dphi = dphi + (cosphi*dZ - sinphi*dP)/(N_phi + h); + + % test for convergence + if (dP*dP + dZ*dZ < tolsq) + break; + end + + % Not Converged--Warn user + if i == maxit + fprintf([' Problem in TOGEOD, did not converge in %2.0f',... + ' iterations\n'], i); + end +end % for i = 1:maxit + +dphi = dphi * rtd; +%%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/topocent.m b/src/utils/matlab/libs/geoFunctions/topocent.m index 7ec0aaede..723cb3dfb 100644 --- a/src/utils/matlab/libs/geoFunctions/topocent.m +++ b/src/utils/matlab/libs/geoFunctions/topocent.m @@ -1,54 +1,54 @@ -function [Az, El, D] = topocent(X, dx) -% TOPOCENT Transformation of vector dx into topocentric coordinate -% system with origin at X. -% Both parameters are 3 by 1 vectors. -% -% [Az, El, D] = topocent(X, dx); -% -% Inputs: -% X - vector origin corrdinates (in ECEF system [X; Y; Z;]) -% dx - vector ([dX; dY; dZ;]). -% -% Outputs: -% D - vector length. Units like units of the input -% Az - azimuth from north positive clockwise, degrees -% El - elevation angle, degrees - -% Kai Borre 11-24-96 -% Copyright (c) by Kai Borre -%========================================================================== - -dtr = pi/180; - -[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3)); - -cl = cos(lambda * dtr); -sl = sin(lambda * dtr); -cb = cos(phi * dtr); -sb = sin(phi * dtr); - -F = [-sl -sb*cl cb*cl; - cl -sb*sl cb*sl; - 0 cb sb]; - -local_vector = F' * dx; -E = local_vector(1); -N = local_vector(2); -U = local_vector(3); - -hor_dis = sqrt(E^2 + N^2); - -if hor_dis < 1.e-20 - Az = 0; - El = 90; -else - Az = atan2(E, N)/dtr; - El = atan2(U, hor_dis)/dtr; -end - -if Az < 0 - Az = Az + 360; -end - -D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2); -%%%%%%%%% end topocent.m %%%%%%%%% +function [Az, El, D] = topocent(X, dx) +% TOPOCENT Transformation of vector dx into topocentric coordinate +% system with origin at X. +% Both parameters are 3 by 1 vectors. +% +% [Az, El, D] = topocent(X, dx); +% +% Inputs: +% X - vector origin corrdinates (in ECEF system [X; Y; Z;]) +% dx - vector ([dX; dY; dZ;]). +% +% Outputs: +% D - vector length. Units like units of the input +% Az - azimuth from north positive clockwise, degrees +% El - elevation angle, degrees + +% Kai Borre 11-24-96 +% Copyright (c) by Kai Borre +%========================================================================== + +dtr = pi/180; + +[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3)); + +cl = cos(lambda * dtr); +sl = sin(lambda * dtr); +cb = cos(phi * dtr); +sb = sin(phi * dtr); + +F = [-sl -sb*cl cb*cl; + cl -sb*sl cb*sl; + 0 cb sb]; + +local_vector = F' * dx; +E = local_vector(1); +N = local_vector(2); +U = local_vector(3); + +hor_dis = sqrt(E^2 + N^2); + +if hor_dis < 1.e-20 + Az = 0; + El = 90; +else + Az = atan2(E, N)/dtr; + El = atan2(U, hor_dis)/dtr; +end + +if Az < 0 + Az = Az + 360; +end + +D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2); +%%%%%%%%% end topocent.m %%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/tropo.m b/src/utils/matlab/libs/geoFunctions/tropo.m index f0f08d218..4e1801aac 100644 --- a/src/utils/matlab/libs/geoFunctions/tropo.m +++ b/src/utils/matlab/libs/geoFunctions/tropo.m @@ -1,95 +1,95 @@ -function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum) -% TROPO Calculation of tropospheric correction. -% The range correction ddr in m is to be subtracted from -% pseudo-ranges and carrier phases -% -% ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); -% -% Inputs: -% sinel - sin of elevation angle of satellite -% hsta - height of station in km -% p - atmospheric pressure in mb at height hp -% tkel - surface temperature in degrees Kelvin at height htkel -% hum - humidity in % at height hhum -% hp - height of pressure measurement in km -% htkel - height of temperature measurement in km -% hhum - height of humidity measurement in km -% -% Outputs: -% ddr - range correction (meters) -% -% Reference -% Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric -% Refraction Correction Model. Paper presented at the -% American Geophysical Union Annual Fall Meeting, San -% Francisco, December 12-17 - -% A Matlab reimplementation of a C code from driver. -% Kai Borre 06-28-95 -%========================================================================== - -a_e = 6378.137; % semi-major axis of earth ellipsoid -b0 = 7.839257e-5; -tlapse = -6.5; -tkhum = tkel + tlapse*(hhum-htkel); -atkel = 7.5*(tkhum-273.15) / (237.3+tkhum-273.15); -e0 = 0.0611 * hum * 10^atkel; -tksea = tkel - tlapse*htkel; -em = -978.77 / (2.8704e6*tlapse*1.0e-5); -tkelh = tksea + tlapse*hhum; -e0sea = e0 * (tksea/tkelh)^(4*em); -tkelp = tksea + tlapse*hp; -psea = p * (tksea/tkelp)^em; - -if sinel < 0 - sinel = 0; -end - -tropo = 0; -done = 'FALSE'; -refsea = 77.624e-6 / tksea; -htop = 1.1385e-5 / refsea; -refsea = refsea * psea; -ref = refsea * ((htop-hsta)/htop)^4; - -while 1 - rtop = (a_e+htop)^2 - (a_e+hsta)^2*(1-sinel^2); - - % check to see if geometry is crazy - if rtop < 0 - rtop = 0; - end - - rtop = sqrt(rtop) - (a_e+hsta)*sinel; - a = -sinel/(htop-hsta); - b = -b0*(1-sinel^2) / (htop-hsta); - rn = zeros(8,1); - - for i = 1:8 - rn(i) = rtop^(i+1); - end - - 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,... - b^2*(6*a^2+4*b)*1.428571e-1, 0, 0]; - - if b^2 > 1.0e-35 - alpha(7) = a*b^3/2; - alpha(8) = b^4/9; - end - - dr = rtop; - dr = dr + alpha*rn; - tropo = tropo + dr*ref*1000; - - if done == 'TRUE ' - ddr = tropo; - break; - end - - done = 'TRUE '; - refsea = (371900.0e-6/tksea-12.92e-6)/tksea; - htop = 1.1385e-5 * (1255/tksea+0.05)/refsea; - ref = refsea * e0sea * ((htop-hsta)/htop)^4; -end; -%%%%%%%%% end tropo.m %%%%%%%%%%%%%%%%%%% +function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum) +% TROPO Calculation of tropospheric correction. +% The range correction ddr in m is to be subtracted from +% pseudo-ranges and carrier phases +% +% ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); +% +% Inputs: +% sinel - sin of elevation angle of satellite +% hsta - height of station in km +% p - atmospheric pressure in mb at height hp +% tkel - surface temperature in degrees Kelvin at height htkel +% hum - humidity in % at height hhum +% hp - height of pressure measurement in km +% htkel - height of temperature measurement in km +% hhum - height of humidity measurement in km +% +% Outputs: +% ddr - range correction (meters) +% +% Reference +% Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric +% Refraction Correction Model. Paper presented at the +% American Geophysical Union Annual Fall Meeting, San +% Francisco, December 12-17 + +% A Matlab reimplementation of a C code from driver. +% Kai Borre 06-28-95 +%========================================================================== + +a_e = 6378.137; % semi-major axis of earth ellipsoid +b0 = 7.839257e-5; +tlapse = -6.5; +tkhum = tkel + tlapse*(hhum-htkel); +atkel = 7.5*(tkhum-273.15) / (237.3+tkhum-273.15); +e0 = 0.0611 * hum * 10^atkel; +tksea = tkel - tlapse*htkel; +em = -978.77 / (2.8704e6*tlapse*1.0e-5); +tkelh = tksea + tlapse*hhum; +e0sea = e0 * (tksea/tkelh)^(4*em); +tkelp = tksea + tlapse*hp; +psea = p * (tksea/tkelp)^em; + +if sinel < 0 + sinel = 0; +end + +tropo = 0; +done = 'FALSE'; +refsea = 77.624e-6 / tksea; +htop = 1.1385e-5 / refsea; +refsea = refsea * psea; +ref = refsea * ((htop-hsta)/htop)^4; + +while 1 + rtop = (a_e+htop)^2 - (a_e+hsta)^2*(1-sinel^2); + + % check to see if geometry is crazy + if rtop < 0 + rtop = 0; + end + + rtop = sqrt(rtop) - (a_e+hsta)*sinel; + a = -sinel/(htop-hsta); + b = -b0*(1-sinel^2) / (htop-hsta); + rn = zeros(8,1); + + for i = 1:8 + rn(i) = rtop^(i+1); + end + + 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,... + b^2*(6*a^2+4*b)*1.428571e-1, 0, 0]; + + if b^2 > 1.0e-35 + alpha(7) = a*b^3/2; + alpha(8) = b^4/9; + end + + dr = rtop; + dr = dr + alpha*rn; + tropo = tropo + dr*ref*1000; + + if done == 'TRUE ' + ddr = tropo; + break; + end + + done = 'TRUE '; + refsea = (371900.0e-6/tksea-12.92e-6)/tksea; + htop = 1.1385e-5 * (1255/tksea+0.05)/refsea; + ref = refsea * e0sea * ((htop-hsta)/htop)^4; +end; +%%%%%%%%% end tropo.m %%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/plotNavigation.m b/src/utils/matlab/libs/plotNavigation.m index fd6af14d8..9e27fc4d2 100644 --- a/src/utils/matlab/libs/plotNavigation.m +++ b/src/utils/matlab/libs/plotNavigation.m @@ -1,166 +1,166 @@ -% Function plots variations of coordinates over time and a 3D position -% plot. It plots receiver coordinates in UTM system or coordinate offsets if -% the true UTM receiver coordinates are provided. -% -% plotNavigation(navSolutions, settings) -% -% Inputs: -% navSolutions - Results from navigation solution function. It -% contains measured pseudoranges and receiver -% coordinates. -% settings - Receiver settings. The true receiver coordinates -% are contained in this structure. -% plot_skyplot - If ==1 then use satellite coordinates to plot the -% the satellite positions - -% Darius Plausinaitis -% Modified by Javier Arribas, 2011. jarribas(at)cttc.es -% ------------------------------------------------------------------------- -% -% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) -% -% GNSS-SDR is a software defined Global Navigation -% Satellite Systems receiver -% -% This file is part of GNSS-SDR. -% -% 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 -% the Free Software Foundation, either version 3 of the License, or -% at your option) any later version. -% -% GNSS-SDR is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License -% along with GNSS-SDR. If not, see . -% -% ------------------------------------------------------------------------- -% - -function plotNavigation(navSolutions, settings,plot_skyplot) - - -%% Plot results in the necessary data exists ============================== -if (~isempty(navSolutions)) - - %% If reference position is not provided, then set reference position - %% to the average postion - if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ... - || isnan(settings.truePosition.U) - - %=== Compute mean values ========================================== - % Remove NaN-s or the output of the function MEAN will be NaN. - refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E))); - refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N))); - refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U))); - - %Also convert geodetic coordinates to deg:min:sec vector format - meanLongitude = dms2mat(deg2dms(... - mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5); - meanLatitude = dms2mat(deg2dms(... - mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5); - - LatLong_str=[num2str(meanLatitude(1)), '??', ... - num2str(meanLatitude(2)), '''', ... - num2str(meanLatitude(3)), '''''', ... - ',', ... - num2str(meanLongitude(1)), '??', ... - num2str(meanLongitude(2)), '''', ... - num2str(meanLongitude(3)), ''''''] - - - - refPointLgText = ['Mean Position\newline Lat: ', ... - num2str(meanLatitude(1)), '{\circ}', ... - num2str(meanLatitude(2)), '{\prime}', ... - num2str(meanLatitude(3)), '{\prime}{\prime}', ... - '\newline Lng: ', ... - num2str(meanLongitude(1)), '{\circ}', ... - num2str(meanLongitude(2)), '{\prime}', ... - num2str(meanLongitude(3)), '{\prime}{\prime}', ... - '\newline Hgt: ', ... - num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')]; - - else - % compute the mean error for static receiver - mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E))); - mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N))); - mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U))); - refCoord.E = settings.truePosition.E; - refCoord.N = settings.truePosition.N; - refCoord.U = settings.truePosition.U; - - error_meters=sqrt((mean_position.E-refCoord.E)^2+(mean_position.N-refCoord.N)^2+(mean_position.U-refCoord.U)^2); - - refPointLgText = ['Reference Position, Mean 3D error = ' num2str(error_meters) ' [m]']; - end - - figureNumber = 300; - % The 300 is chosen for more convenient handling of the open - % figure windows, when many figures are closed and reopened. Figures - % drawn or opened by the user, will not be "overwritten" by this - % function if the auto numbering is not used. - - %=== Select (or create) and clear the figure ========================== - figure(figureNumber); - clf (figureNumber); - set (figureNumber, 'Name', 'Navigation solutions'); - - %--- Draw axes -------------------------------------------------------- - handles(1, 1) = subplot(4, 2, 1 : 4); - handles(3, 1) = subplot(4, 2, [5, 7]); - handles(3, 2) = subplot(4, 2, [6, 8]); - - %% Plot all figures ======================================================= - - %--- Coordinate differences in UTM system ----------------------------- - plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ... - (navSolutions.N - refCoord.N)',... - (navSolutions.U - refCoord.U)']); - - title (handles(1, 1), 'Coordinates variations in UTM system'); - legend(handles(1, 1), 'E', 'N', 'U'); - xlabel(handles(1, 1), ['Measurement period: ', ... - num2str(settings.navSolPeriod), 'ms']); - ylabel(handles(1, 1), 'Variations (m)'); - grid (handles(1, 1)); - axis (handles(1, 1), 'tight'); - - %--- Position plot in UTM system -------------------------------------- - plot3 (handles(3, 1), navSolutions.E - refCoord.E, ... - navSolutions.N - refCoord.N, ... - navSolutions.U - refCoord.U, '+'); - hold (handles(3, 1), 'on'); - - %Plot the reference point - plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10); - hold (handles(3, 1), 'off'); - - view (handles(3, 1), 0, 90); - axis (handles(3, 1), 'equal'); - grid (handles(3, 1), 'minor'); - - legend(handles(3, 1), 'Measurements', refPointLgText); - - title (handles(3, 1), 'Positions in UTM system (3D plot)'); - xlabel(handles(3, 1), 'East (m)'); - ylabel(handles(3, 1), 'North (m)'); - zlabel(handles(3, 1), 'Upping (m)'); - - if (plot_skyplot==1) - %--- Satellite sky plot ----------------------------------------------- - skyPlot(handles(3, 2), ... - navSolutions.channel.az, ... - navSolutions.channel.el, ... - navSolutions.channel.PRN(:, 1)); - - title (handles(3, 2), ['Sky plot (mean PDOP: ', ... - num2str(mean(navSolutions.DOP(2,:))), ')']); - end - -else - disp('plotNavigation: No navigation data to plot.'); -end % if (~isempty(navSolutions)) +% Function plots variations of coordinates over time and a 3D position +% plot. It plots receiver coordinates in UTM system or coordinate offsets if +% the true UTM receiver coordinates are provided. +% +% plotNavigation(navSolutions, settings) +% +% Inputs: +% navSolutions - Results from navigation solution function. It +% contains measured pseudoranges and receiver +% coordinates. +% settings - Receiver settings. The true receiver coordinates +% are contained in this structure. +% plot_skyplot - If ==1 then use satellite coordinates to plot the +% the satellite positions + +% Darius Plausinaitis +% Modified by Javier Arribas, 2011. jarribas(at)cttc.es +% ------------------------------------------------------------------------- +% +% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% +% GNSS-SDR is a software defined Global Navigation +% Satellite Systems receiver +% +% This file is part of GNSS-SDR. +% +% 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 +% the Free Software Foundation, either version 3 of the License, or +% at your option) any later version. +% +% GNSS-SDR is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with GNSS-SDR. If not, see . +% +% ------------------------------------------------------------------------- +% + +function plotNavigation(navSolutions, settings,plot_skyplot) + + +%% Plot results in the necessary data exists ============================== +if (~isempty(navSolutions)) + + %% If reference position is not provided, then set reference position + %% to the average postion + if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ... + || isnan(settings.truePosition.U) + + %=== Compute mean values ========================================== + % Remove NaN-s or the output of the function MEAN will be NaN. + refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E))); + refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N))); + refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U))); + + %Also convert geodetic coordinates to deg:min:sec vector format + meanLongitude = dms2mat(deg2dms(... + mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5); + meanLatitude = dms2mat(deg2dms(... + mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5); + + LatLong_str=[num2str(meanLatitude(1)), '??', ... + num2str(meanLatitude(2)), '''', ... + num2str(meanLatitude(3)), '''''', ... + ',', ... + num2str(meanLongitude(1)), '??', ... + num2str(meanLongitude(2)), '''', ... + num2str(meanLongitude(3)), ''''''] + + + + refPointLgText = ['Mean Position\newline Lat: ', ... + num2str(meanLatitude(1)), '{\circ}', ... + num2str(meanLatitude(2)), '{\prime}', ... + num2str(meanLatitude(3)), '{\prime}{\prime}', ... + '\newline Lng: ', ... + num2str(meanLongitude(1)), '{\circ}', ... + num2str(meanLongitude(2)), '{\prime}', ... + num2str(meanLongitude(3)), '{\prime}{\prime}', ... + '\newline Hgt: ', ... + num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')]; + + else + % compute the mean error for static receiver + mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E))); + mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N))); + mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U))); + refCoord.E = settings.truePosition.E; + refCoord.N = settings.truePosition.N; + refCoord.U = settings.truePosition.U; + + error_meters=sqrt((mean_position.E-refCoord.E)^2+(mean_position.N-refCoord.N)^2+(mean_position.U-refCoord.U)^2); + + refPointLgText = ['Reference Position, Mean 3D error = ' num2str(error_meters) ' [m]']; + end + + figureNumber = 300; + % The 300 is chosen for more convenient handling of the open + % figure windows, when many figures are closed and reopened. Figures + % drawn or opened by the user, will not be "overwritten" by this + % function if the auto numbering is not used. + + %=== Select (or create) and clear the figure ========================== + figure(figureNumber); + clf (figureNumber); + set (figureNumber, 'Name', 'Navigation solutions'); + + %--- Draw axes -------------------------------------------------------- + handles(1, 1) = subplot(4, 2, 1 : 4); + handles(3, 1) = subplot(4, 2, [5, 7]); + handles(3, 2) = subplot(4, 2, [6, 8]); + + %% Plot all figures ======================================================= + + %--- Coordinate differences in UTM system ----------------------------- + plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ... + (navSolutions.N - refCoord.N)',... + (navSolutions.U - refCoord.U)']); + + title (handles(1, 1), 'Coordinates variations in UTM system'); + legend(handles(1, 1), 'E', 'N', 'U'); + xlabel(handles(1, 1), ['Measurement period: ', ... + num2str(settings.navSolPeriod), 'ms']); + ylabel(handles(1, 1), 'Variations (m)'); + grid (handles(1, 1)); + axis (handles(1, 1), 'tight'); + + %--- Position plot in UTM system -------------------------------------- + plot3 (handles(3, 1), navSolutions.E - refCoord.E, ... + navSolutions.N - refCoord.N, ... + navSolutions.U - refCoord.U, '+'); + hold (handles(3, 1), 'on'); + + %Plot the reference point + plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10); + hold (handles(3, 1), 'off'); + + view (handles(3, 1), 0, 90); + axis (handles(3, 1), 'equal'); + grid (handles(3, 1), 'minor'); + + legend(handles(3, 1), 'Measurements', refPointLgText); + + title (handles(3, 1), 'Positions in UTM system (3D plot)'); + xlabel(handles(3, 1), 'East (m)'); + ylabel(handles(3, 1), 'North (m)'); + zlabel(handles(3, 1), 'Upping (m)'); + + if (plot_skyplot==1) + %--- Satellite sky plot ----------------------------------------------- + skyPlot(handles(3, 2), ... + navSolutions.channel.az, ... + navSolutions.channel.el, ... + navSolutions.channel.PRN(:, 1)); + + title (handles(3, 2), ['Sky plot (mean PDOP: ', ... + num2str(mean(navSolutions.DOP(2,:))), ')']); + end + +else + disp('plotNavigation: No navigation data to plot.'); +end % if (~isempty(navSolutions)) diff --git a/src/utils/matlab/libs/plotTracking.m b/src/utils/matlab/libs/plotTracking.m index e8d6ede85..b280d82aa 100644 --- a/src/utils/matlab/libs/plotTracking.m +++ b/src/utils/matlab/libs/plotTracking.m @@ -1,187 +1,187 @@ -function plotTracking(channelList, trackResults, settings) -% This function plots the tracking results for the given channel list. -% -% plotTracking(channelList, trackResults, settings) -% -% Inputs: -% channelList - list of channels to be plotted. -% trackResults - tracking results from the tracking function. -% settings - receiver settings. - -%-------------------------------------------------------------------------- -% SoftGNSS v3.0 -% -% Copyright (C) Darius Plausinaitis -% Written by Darius Plausinaitis -%-------------------------------------------------------------------------- -%This program is free software; you can redistribute it and/or -%modify it under the terms of the GNU General Public License -%as published by the Free Software Foundation; either version 2 -%of the License, or (at your option) any later version. -% -%This program is distributed in the hope that it will be useful, -%but WITHOUT ANY WARRANTY; without even the implied warranty of -%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%GNU General Public License for more details. -% -%You should have received a copy of the GNU General Public License -%along with this program; if not, write to the Free Software -%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, -%USA. -%-------------------------------------------------------------------------- - - -% Protection - if the list contains incorrect channel numbers -channelList = intersect(channelList, 1:settings.numberOfChannels); - -%=== For all listed channels ============================================== -for channelNr = channelList - - %% Select (or create) and clear the figure ================================ - % The number 200 is added just for more convenient handling of the open - % figure windows, when many figures are closed and reopened. - % Figures drawn or opened by the user, will not be "overwritten" by - % this function. - - figure(channelNr +200); - clf(channelNr +200); - set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... - ' (PRN ', ... - num2str(trackResults(channelNr).PRN(end-1)), ... - ') results']); - - %% Draw axes ============================================================== - % Row 1 - handles(1, 1) = subplot(4, 3, 1); - handles(1, 2) = subplot(4, 3, [2 3]); - % Row 2 - handles(2, 1) = subplot(4, 3, 4); - handles(2, 2) = subplot(4, 3, [5 6]); - % Row 3 - handles(3, 1) = subplot(4, 3, 7); - handles(3, 2) = subplot(4, 3, 8); - handles(3, 3) = subplot(4, 3, 9); - % Row 4 - handles(4, 1) = subplot(4, 3, 10); - handles(4, 2) = subplot(4, 3, 11); - handles(4, 3) = subplot(4, 3, 12); - - - %% Plot all figures ======================================================= - - timeAxisInSeconds = (1:settings.msToProcess)/1000; - - %----- Discrete-Time Scatter Plot --------------------------------- - plot(handles(1, 1), trackResults(channelNr).I_P,... - trackResults(channelNr).Q_P, ... - '.'); - - grid (handles(1, 1)); - axis (handles(1, 1), 'equal'); - title (handles(1, 1), 'Discrete-Time Scatter Plot'); - xlabel(handles(1, 1), 'I prompt'); - ylabel(handles(1, 1), 'Q prompt'); - - %----- Nav bits --------------------------------------------------- - plot (handles(1, 2), timeAxisInSeconds, ... - trackResults(channelNr).I_P); - - grid (handles(1, 2)); - title (handles(1, 2), 'Bits of the navigation message'); - xlabel(handles(1, 2), 'Time (s)'); - axis (handles(1, 2), 'tight'); - - %----- PLL discriminator unfiltered-------------------------------- - plot (handles(2, 1), timeAxisInSeconds, ... - trackResults(channelNr).pllDiscr, 'r'); - - grid (handles(2, 1)); - axis (handles(2, 1), 'tight'); - xlabel(handles(2, 1), 'Time (s)'); - ylabel(handles(2, 1), 'Amplitude'); - title (handles(2, 1), 'Raw PLL discriminator'); - - %----- Correlation ------------------------------------------------ - plot(handles(2, 2), timeAxisInSeconds, ... - [sqrt(trackResults(channelNr).I_E.^2 + ... - trackResults(channelNr).Q_E.^2)', ... - sqrt(trackResults(channelNr).I_P.^2 + ... - trackResults(channelNr).Q_P.^2)', ... - sqrt(trackResults(channelNr).I_L.^2 + ... - trackResults(channelNr).Q_L.^2)'], ... - '-*'); - - grid (handles(2, 2)); - title (handles(2, 2), 'Correlation results'); - xlabel(handles(2, 2), 'Time (s)'); - axis (handles(2, 2), 'tight'); - - hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... - '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... - '$\sqrt{I_{L}^2 + Q_{L}^2}$'); - - %set interpreter from tex to latex. This will draw \sqrt correctly - set(hLegend, 'Interpreter', 'Latex'); - - %----- PLL discriminator filtered---------------------------------- - plot (handles(3, 1), timeAxisInSeconds, ... - trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess), 'b'); - - grid (handles(3, 1)); - axis (handles(3, 1), 'tight'); - xlabel(handles(3, 1), 'Time (s)'); - ylabel(handles(3, 1), 'Amplitude'); - title (handles(3, 1), 'Filtered PLL discriminator'); - - %----- DLL discriminator unfiltered-------------------------------- - plot (handles(3, 2), timeAxisInSeconds, ... - trackResults(channelNr).dllDiscr, 'r'); - - grid (handles(3, 2)); - axis (handles(3, 2), 'tight'); - xlabel(handles(3, 2), 'Time (s)'); - ylabel(handles(3, 2), 'Amplitude'); - title (handles(3, 2), 'Raw DLL discriminator'); - - %----- DLL discriminator filtered---------------------------------- - plot (handles(3, 3), timeAxisInSeconds, ... - trackResults(channelNr).dllDiscrFilt, 'b'); - - grid (handles(3, 3)); - axis (handles(3, 3), 'tight'); - xlabel(handles(3, 3), 'Time (s)'); - ylabel(handles(3, 3), 'Amplitude'); - title (handles(3, 3), 'Filtered DLL discriminator'); - - %----- CNo for signal---------------------------------- - plot (handles(4, 1), timeAxisInSeconds, ... - trackResults(channelNr).CNo(1:settings.msToProcess), 'b'); - - grid (handles(4, 1)); - axis (handles(4, 1), 'tight'); - xlabel(handles(4, 1), 'Time (s)'); - ylabel(handles(4, 1), 'CNo (dB-Hz)'); - title (handles(4, 1), 'Carrier to Noise Ratio'); - - %----- Carrier Frequency -------------------------------- - plot (handles(4, 2), timeAxisInSeconds(2:end), ... - trackResults(channelNr).carrFreq(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]); - - grid (handles(4, 2)); - axis (handles(4, 2)); - xlabel(handles(4, 2), 'Time (s)'); - ylabel(handles(4, 2), 'Freq (hz)'); - title (handles(4, 2), 'Carrier Freq'); - - %----- Code Frequency---------------------------------- - %--- Skip sample 0 to help with results display - plot (handles(4, 3), timeAxisInSeconds(2:end), ... - trackResults(channelNr).codeFreq(2:settings.msToProcess), 'Color',[0.2 0.3 0.49]); - - grid (handles(4, 3)); - axis (handles(4, 3), 'tight'); - xlabel(handles(4, 3), 'Time (s)'); - ylabel(handles(4, 3), 'Freq (Hz)'); - title (handles(4, 3), 'Code Freq'); - -end % for channelNr = channelList +function plotTracking(channelList, trackResults, settings) +% This function plots the tracking results for the given channel list. +% +% plotTracking(channelList, trackResults, settings) +% +% Inputs: +% channelList - list of channels to be plotted. +% trackResults - tracking results from the tracking function. +% settings - receiver settings. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + + +% Protection - if the list contains incorrect channel numbers +channelList = intersect(channelList, 1:settings.numberOfChannels); + +%=== For all listed channels ============================================== +for channelNr = channelList + + %% Select (or create) and clear the figure ================================ + % The number 200 is added just for more convenient handling of the open + % figure windows, when many figures are closed and reopened. + % Figures drawn or opened by the user, will not be "overwritten" by + % this function. + + figure(channelNr +200); + clf(channelNr +200); + set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... + ' (PRN ', ... + num2str(trackResults(channelNr).PRN(end-1)), ... + ') results']); + + %% Draw axes ============================================================== + % Row 1 + handles(1, 1) = subplot(4, 3, 1); + handles(1, 2) = subplot(4, 3, [2 3]); + % Row 2 + handles(2, 1) = subplot(4, 3, 4); + handles(2, 2) = subplot(4, 3, [5 6]); + % Row 3 + handles(3, 1) = subplot(4, 3, 7); + handles(3, 2) = subplot(4, 3, 8); + handles(3, 3) = subplot(4, 3, 9); + % Row 4 + handles(4, 1) = subplot(4, 3, 10); + handles(4, 2) = subplot(4, 3, 11); + handles(4, 3) = subplot(4, 3, 12); + + + %% Plot all figures ======================================================= + + timeAxisInSeconds = (1:settings.msToProcess)/1000; + + %----- Discrete-Time Scatter Plot --------------------------------- + plot(handles(1, 1), trackResults(channelNr).I_P,... + trackResults(channelNr).Q_P, ... + '.'); + + grid (handles(1, 1)); + axis (handles(1, 1), 'equal'); + title (handles(1, 1), 'Discrete-Time Scatter Plot'); + xlabel(handles(1, 1), 'I prompt'); + ylabel(handles(1, 1), 'Q prompt'); + + %----- Nav bits --------------------------------------------------- + plot (handles(1, 2), timeAxisInSeconds, ... + trackResults(channelNr).I_P); + + grid (handles(1, 2)); + title (handles(1, 2), 'Bits of the navigation message'); + xlabel(handles(1, 2), 'Time (s)'); + axis (handles(1, 2), 'tight'); + + %----- PLL discriminator unfiltered-------------------------------- + plot (handles(2, 1), timeAxisInSeconds, ... + trackResults(channelNr).pllDiscr, 'r'); + + grid (handles(2, 1)); + axis (handles(2, 1), 'tight'); + xlabel(handles(2, 1), 'Time (s)'); + ylabel(handles(2, 1), 'Amplitude'); + title (handles(2, 1), 'Raw PLL discriminator'); + + %----- Correlation ------------------------------------------------ + plot(handles(2, 2), timeAxisInSeconds, ... + [sqrt(trackResults(channelNr).I_E.^2 + ... + trackResults(channelNr).Q_E.^2)', ... + sqrt(trackResults(channelNr).I_P.^2 + ... + trackResults(channelNr).Q_P.^2)', ... + sqrt(trackResults(channelNr).I_L.^2 + ... + trackResults(channelNr).Q_L.^2)'], ... + '-*'); + + grid (handles(2, 2)); + title (handles(2, 2), 'Correlation results'); + xlabel(handles(2, 2), 'Time (s)'); + axis (handles(2, 2), 'tight'); + + hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... + '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... + '$\sqrt{I_{L}^2 + Q_{L}^2}$'); + + %set interpreter from tex to latex. This will draw \sqrt correctly + set(hLegend, 'Interpreter', 'Latex'); + + %----- PLL discriminator filtered---------------------------------- + plot (handles(3, 1), timeAxisInSeconds, ... + trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess), 'b'); + + grid (handles(3, 1)); + axis (handles(3, 1), 'tight'); + xlabel(handles(3, 1), 'Time (s)'); + ylabel(handles(3, 1), 'Amplitude'); + title (handles(3, 1), 'Filtered PLL discriminator'); + + %----- DLL discriminator unfiltered-------------------------------- + plot (handles(3, 2), timeAxisInSeconds, ... + trackResults(channelNr).dllDiscr, 'r'); + + grid (handles(3, 2)); + axis (handles(3, 2), 'tight'); + xlabel(handles(3, 2), 'Time (s)'); + ylabel(handles(3, 2), 'Amplitude'); + title (handles(3, 2), 'Raw DLL discriminator'); + + %----- DLL discriminator filtered---------------------------------- + plot (handles(3, 3), timeAxisInSeconds, ... + trackResults(channelNr).dllDiscrFilt, 'b'); + + grid (handles(3, 3)); + axis (handles(3, 3), 'tight'); + xlabel(handles(3, 3), 'Time (s)'); + ylabel(handles(3, 3), 'Amplitude'); + title (handles(3, 3), 'Filtered DLL discriminator'); + + %----- CNo for signal---------------------------------- + plot (handles(4, 1), timeAxisInSeconds, ... + trackResults(channelNr).CNo(1:settings.msToProcess), 'b'); + + grid (handles(4, 1)); + axis (handles(4, 1), 'tight'); + xlabel(handles(4, 1), 'Time (s)'); + ylabel(handles(4, 1), 'CNo (dB-Hz)'); + title (handles(4, 1), 'Carrier to Noise Ratio'); + + %----- Carrier Frequency -------------------------------- + plot (handles(4, 2), timeAxisInSeconds(2:end), ... + trackResults(channelNr).carrFreq(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]); + + grid (handles(4, 2)); + axis (handles(4, 2)); + xlabel(handles(4, 2), 'Time (s)'); + ylabel(handles(4, 2), 'Freq (hz)'); + title (handles(4, 2), 'Carrier Freq'); + + %----- Code Frequency---------------------------------- + %--- Skip sample 0 to help with results display + plot (handles(4, 3), timeAxisInSeconds(2:end), ... + trackResults(channelNr).codeFreq(2:settings.msToProcess), 'Color',[0.2 0.3 0.49]); + + grid (handles(4, 3)); + axis (handles(4, 3), 'tight'); + xlabel(handles(4, 3), 'Time (s)'); + ylabel(handles(4, 3), 'Freq (Hz)'); + title (handles(4, 3), 'Code Freq'); + +end % for channelNr = channelList diff --git a/src/utils/matlab/libs/plotVEMLTracking.m b/src/utils/matlab/libs/plotVEMLTracking.m index 40cab0bb6..bc4e4e806 100644 --- a/src/utils/matlab/libs/plotVEMLTracking.m +++ b/src/utils/matlab/libs/plotVEMLTracking.m @@ -1,162 +1,162 @@ -function plotVEMLTracking(channelList, trackResults, settings) -% This function plots the tracking results for the given channel list. -% -% plotTracking(channelList, trackResults, settings) -% -% Inputs: -% channelList - list of channels to be plotted. -% trackResults - tracking results from the tracking function. -% settings - receiver settings. - -%-------------------------------------------------------------------------- -% SoftGNSS v3.0 -% -% Copyright (C) Darius Plausinaitis -% Written by Darius Plausinaitis -%-------------------------------------------------------------------------- -%This program is free software; you can redistribute it and/or -%modify it under the terms of the GNU General Public License -%as published by the Free Software Foundation; either version 2 -%of the License, or (at your option) any later version. -% -%This program is distributed in the hope that it will be useful, -%but WITHOUT ANY WARRANTY; without even the implied warranty of -%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%GNU General Public License for more details. -% -%You should have received a copy of the GNU General Public License -%along with this program; if not, write to the Free Software -%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, -%USA. -%-------------------------------------------------------------------------- - -% Protection - if the list contains incorrect channel numbers -channelList = intersect(channelList, 1:settings.numberOfChannels); - -%=== For all listed channels ============================================== -for channelNr = channelList - - %% Select (or create) and clear the figure ================================ - % The number 200 is added just for more convenient handling of the open - % figure windows, when many figures are closed and reopened. - % Figures drawn or opened by the user, will not be "overwritten" by - % this function. - - figure(channelNr +200); - clf(channelNr +200); - set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... - ' (PRN ', ... - num2str(trackResults(channelNr).PRN(end-1)), ... - ') results']); - - %% Draw axes ============================================================== - % Row 1 - handles(1, 1) = subplot(3, 3, 1); - handles(1, 2) = subplot(3, 3, [2 3]); - % Row 2 - handles(2, 1) = subplot(3, 3, 4); - handles(2, 2) = subplot(3, 3, [5 6]); - % Row 3 - handles(3, 1) = subplot(3, 3, 7); - handles(3, 2) = subplot(3, 3, 8); - handles(3, 3) = subplot(3, 3, 9); - - %% Plot all figures ======================================================= - - timeAxisInSeconds = (1:4:settings.msToProcess)/1000; - - %----- Discrete-Time Scatter Plot --------------------------------- - plot(handles(1, 1), trackResults(channelNr).data_I,... - trackResults(channelNr).data_Q, ... - '.'); - - grid (handles(1, 1)); - axis (handles(1, 1), 'equal'); - title (handles(1, 1), 'Discrete-Time Scatter Plot'); - xlabel(handles(1, 1), 'I prompt'); - ylabel(handles(1, 1), 'Q prompt'); - - %----- Nav bits --------------------------------------------------- - t = (1:length(trackResults(channelNr).data_I)); - plot (handles(1, 2), t, ... - trackResults(channelNr).data_I); - - grid (handles(1, 2)); - title (handles(1, 2), 'Bits of the navigation message'); - xlabel(handles(1, 2), 'Time (s)'); - axis (handles(1, 2), 'tight'); - - %----- PLL discriminator unfiltered-------------------------------- - t = (1:length(trackResults(channelNr).pllDiscr)); - plot (handles(2, 1), t, ... - trackResults(channelNr).pllDiscr, 'r'); - - grid (handles(2, 1)); - axis (handles(2, 1), 'tight'); - xlabel(handles(2, 1), 'Time (s)'); - ylabel(handles(2, 1), 'Amplitude'); - title (handles(2, 1), 'Raw PLL discriminator'); - - %----- Correlation ------------------------------------------------ - t = (1:length(trackResults(channelNr).I_VE)); - plot(handles(2, 2), t, ... - [sqrt(trackResults(channelNr).I_VE.^2 + ... - trackResults(channelNr).Q_VE.^2)', ... - sqrt(trackResults(channelNr).I_E.^2 + ... - trackResults(channelNr).Q_E.^2)', ... - sqrt(trackResults(channelNr).I_P.^2 + ... - trackResults(channelNr).Q_P.^2)', ... - sqrt(trackResults(channelNr).I_L.^2 + ... - trackResults(channelNr).Q_L.^2)', ... - sqrt(trackResults(channelNr).I_VL.^2 + ... - trackResults(channelNr).Q_VL.^2)'], ... - '-*'); - - grid (handles(2, 2)); - title (handles(2, 2), 'Correlation results'); - xlabel(handles(2, 2), 'Time (s)'); - axis (handles(2, 2), 'tight'); - - hLegend = legend(handles(2, 2), '$\sqrt{I_{VE}^2 + Q_{VE}^2}$', ... - '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... - '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... - '$\sqrt{I_{L}^2 + Q_{L}^2}$', ... - '$\sqrt{I_{VL}^2 + Q_{VL}^2}$'); - - %set interpreter from tex to latex. This will draw \sqrt correctly - set(hLegend, 'Interpreter', 'Latex'); - - %----- PLL discriminator filtered---------------------------------- - t = (1:length(trackResults(channelNr).pllDiscrFilt)); - plot (handles(3, 1), t, ... - trackResults(channelNr).pllDiscrFilt, 'b'); - - grid (handles(3, 1)); - axis (handles(3, 1), 'tight'); - xlabel(handles(3, 1), 'Time (s)'); - ylabel(handles(3, 1), 'Amplitude'); - title (handles(3, 1), 'Filtered PLL discriminator'); - - %----- DLL discriminator unfiltered-------------------------------- - t = (1:length(trackResults(channelNr).dllDiscr)); - plot (handles(3, 2), t, ... - trackResults(channelNr).dllDiscr, 'r'); - - grid (handles(3, 2)); - axis (handles(3, 2), 'tight'); - xlabel(handles(3, 2), 'Time (s)'); - ylabel(handles(3, 2), 'Amplitude'); - title (handles(3, 2), 'Raw DLL discriminator'); - - %----- DLL discriminator filtered---------------------------------- - t = (1:length(trackResults(channelNr).dllDiscrFilt)); - plot (handles(3, 3), t, ... - trackResults(channelNr).dllDiscrFilt, 'b'); - - grid (handles(3, 3)); - axis (handles(3, 3), 'tight'); - xlabel(handles(3, 3), 'Time (s)'); - ylabel(handles(3, 3), 'Amplitude'); - title (handles(3, 3), 'Filtered DLL discriminator'); - -end % for channelNr = channelList +function plotVEMLTracking(channelList, trackResults, settings) +% This function plots the tracking results for the given channel list. +% +% plotTracking(channelList, trackResults, settings) +% +% Inputs: +% channelList - list of channels to be plotted. +% trackResults - tracking results from the tracking function. +% settings - receiver settings. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% Protection - if the list contains incorrect channel numbers +channelList = intersect(channelList, 1:settings.numberOfChannels); + +%=== For all listed channels ============================================== +for channelNr = channelList + + %% Select (or create) and clear the figure ================================ + % The number 200 is added just for more convenient handling of the open + % figure windows, when many figures are closed and reopened. + % Figures drawn or opened by the user, will not be "overwritten" by + % this function. + + figure(channelNr +200); + clf(channelNr +200); + set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... + ' (PRN ', ... + num2str(trackResults(channelNr).PRN(end-1)), ... + ') results']); + + %% Draw axes ============================================================== + % Row 1 + handles(1, 1) = subplot(3, 3, 1); + handles(1, 2) = subplot(3, 3, [2 3]); + % Row 2 + handles(2, 1) = subplot(3, 3, 4); + handles(2, 2) = subplot(3, 3, [5 6]); + % Row 3 + handles(3, 1) = subplot(3, 3, 7); + handles(3, 2) = subplot(3, 3, 8); + handles(3, 3) = subplot(3, 3, 9); + + %% Plot all figures ======================================================= + + timeAxisInSeconds = (1:4:settings.msToProcess)/1000; + + %----- Discrete-Time Scatter Plot --------------------------------- + plot(handles(1, 1), trackResults(channelNr).data_I,... + trackResults(channelNr).data_Q, ... + '.'); + + grid (handles(1, 1)); + axis (handles(1, 1), 'equal'); + title (handles(1, 1), 'Discrete-Time Scatter Plot'); + xlabel(handles(1, 1), 'I prompt'); + ylabel(handles(1, 1), 'Q prompt'); + + %----- Nav bits --------------------------------------------------- + t = (1:length(trackResults(channelNr).data_I)); + plot (handles(1, 2), t, ... + trackResults(channelNr).data_I); + + grid (handles(1, 2)); + title (handles(1, 2), 'Bits of the navigation message'); + xlabel(handles(1, 2), 'Time (s)'); + axis (handles(1, 2), 'tight'); + + %----- PLL discriminator unfiltered-------------------------------- + t = (1:length(trackResults(channelNr).pllDiscr)); + plot (handles(2, 1), t, ... + trackResults(channelNr).pllDiscr, 'r'); + + grid (handles(2, 1)); + axis (handles(2, 1), 'tight'); + xlabel(handles(2, 1), 'Time (s)'); + ylabel(handles(2, 1), 'Amplitude'); + title (handles(2, 1), 'Raw PLL discriminator'); + + %----- Correlation ------------------------------------------------ + t = (1:length(trackResults(channelNr).I_VE)); + plot(handles(2, 2), t, ... + [sqrt(trackResults(channelNr).I_VE.^2 + ... + trackResults(channelNr).Q_VE.^2)', ... + sqrt(trackResults(channelNr).I_E.^2 + ... + trackResults(channelNr).Q_E.^2)', ... + sqrt(trackResults(channelNr).I_P.^2 + ... + trackResults(channelNr).Q_P.^2)', ... + sqrt(trackResults(channelNr).I_L.^2 + ... + trackResults(channelNr).Q_L.^2)', ... + sqrt(trackResults(channelNr).I_VL.^2 + ... + trackResults(channelNr).Q_VL.^2)'], ... + '-*'); + + grid (handles(2, 2)); + title (handles(2, 2), 'Correlation results'); + xlabel(handles(2, 2), 'Time (s)'); + axis (handles(2, 2), 'tight'); + + hLegend = legend(handles(2, 2), '$\sqrt{I_{VE}^2 + Q_{VE}^2}$', ... + '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... + '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... + '$\sqrt{I_{L}^2 + Q_{L}^2}$', ... + '$\sqrt{I_{VL}^2 + Q_{VL}^2}$'); + + %set interpreter from tex to latex. This will draw \sqrt correctly + set(hLegend, 'Interpreter', 'Latex'); + + %----- PLL discriminator filtered---------------------------------- + t = (1:length(trackResults(channelNr).pllDiscrFilt)); + plot (handles(3, 1), t, ... + trackResults(channelNr).pllDiscrFilt, 'b'); + + grid (handles(3, 1)); + axis (handles(3, 1), 'tight'); + xlabel(handles(3, 1), 'Time (s)'); + ylabel(handles(3, 1), 'Amplitude'); + title (handles(3, 1), 'Filtered PLL discriminator'); + + %----- DLL discriminator unfiltered-------------------------------- + t = (1:length(trackResults(channelNr).dllDiscr)); + plot (handles(3, 2), t, ... + trackResults(channelNr).dllDiscr, 'r'); + + grid (handles(3, 2)); + axis (handles(3, 2), 'tight'); + xlabel(handles(3, 2), 'Time (s)'); + ylabel(handles(3, 2), 'Amplitude'); + title (handles(3, 2), 'Raw DLL discriminator'); + + %----- DLL discriminator filtered---------------------------------- + t = (1:length(trackResults(channelNr).dllDiscrFilt)); + plot (handles(3, 3), t, ... + trackResults(channelNr).dllDiscrFilt, 'b'); + + grid (handles(3, 3)); + axis (handles(3, 3), 'tight'); + xlabel(handles(3, 3), 'Time (s)'); + ylabel(handles(3, 3), 'Amplitude'); + title (handles(3, 3), 'Filtered DLL discriminator'); + +end % for channelNr = channelList diff --git a/src/utils/matlab/plotTrackingE5a.m b/src/utils/matlab/plotTrackingE5a.m index d8966caaf..e63d23162 100644 --- a/src/utils/matlab/plotTrackingE5a.m +++ b/src/utils/matlab/plotTrackingE5a.m @@ -1,150 +1,150 @@ -function plotTracking(channelList, trackResults, settings) -% This function plots the tracking results for the given channel list. -% -% plotTracking(channelList, trackResults, settings) -% -% Inputs: -% channelList - list of channels to be plotted. -% trackResults - tracking results from the tracking function. -% settings - receiver settings. - -%-------------------------------------------------------------------------- -% SoftGNSS v3.0 -% -% Copyright (C) Darius Plausinaitis -% Written by Darius Plausinaitis -%-------------------------------------------------------------------------- -% This program is free software; you can redistribute it and/or -% modify it under the terms of the GNU General Public License -% as published by the Free Software Foundation; either version 2 -% of the License, or (at your option) any later version. -% -% This program is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License -% along with this program; if not, write to the Free Software -% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, -% USA. -%-------------------------------------------------------------------------- - -% Protection - if the list contains incorrect channel numbers -channelList = intersect(channelList, 1:settings.numberOfChannels); - -%=== For all listed channels ============================================== -for channelNr = channelList - - %% Select (or create) and clear the figure ================================ - % The number 200 is added just for more convenient handling of the open - % figure windows, when many figures are closed and reopened. - % Figures drawn or opened by the user, will not be "overwritten" by - % this function. - - figure(channelNr +200); - clf(channelNr +200); - set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... - ' (PRN ', ... - num2str(trackResults(channelNr).PRN), ... - ') results']); - - %% Draw axes ============================================================== - % Row 1 - handles(1, 1) = subplot(3, 3, 1); - handles(1, 2) = subplot(3, 3, [2 3]); - % Row 2 - handles(2, 1) = subplot(3, 3, 4); - handles(2, 2) = subplot(3, 3, [5 6]); - % Row 3 - handles(3, 1) = subplot(3, 3, 7); - handles(3, 2) = subplot(3, 3, 8); - handles(3, 3) = subplot(3, 3, 9); - - %% Plot all figures ======================================================= - - timeAxisInSeconds = (1:settings.msToProcess-1)/1000; - - %----- Discrete-Time Scatter Plot --------------------------------- - plot(handles(1, 1), trackResults(channelNr).I_PN,... - trackResults(channelNr).Q_PN, ... - '.'); - - grid (handles(1, 1)); - axis (handles(1, 1), 'equal'); - title (handles(1, 1), 'Discrete-Time Scatter Plot'); - xlabel(handles(1, 1), 'I prompt'); - ylabel(handles(1, 1), 'Q prompt'); - - %----- Nav bits --------------------------------------------------- - plot (handles(1, 2), timeAxisInSeconds, ... - trackResults(channelNr).I_PN(1:settings.msToProcess-1)); - - grid (handles(1, 2)); - title (handles(1, 2), 'Bits of the navigation message'); - xlabel(handles(1, 2), 'Time (s)'); - axis (handles(1, 2), 'tight'); - - %----- PLL discriminator unfiltered-------------------------------- - plot (handles(2, 1), timeAxisInSeconds, ... - trackResults(channelNr).pllDiscr(1:settings.msToProcess-1), 'r'); - - grid (handles(2, 1)); - axis (handles(2, 1), 'tight'); - xlabel(handles(2, 1), 'Time (s)'); - ylabel(handles(2, 1), 'Amplitude'); - title (handles(2, 1), 'Raw PLL discriminator'); - - %----- Correlation ------------------------------------------------ - plot(handles(2, 2), timeAxisInSeconds, ... - [sqrt(trackResults(channelNr).I_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 + ... - trackResults(channelNr).Q_P(1:settings.msToProcess-1).^2)', ... - sqrt(trackResults(channelNr).I_L(1:settings.msToProcess-1).^2 + ... - trackResults(channelNr).Q_L(1:settings.msToProcess-1).^2)'], ... - '-*'); - - grid (handles(2, 2)); - title (handles(2, 2), 'Correlation results'); - xlabel(handles(2, 2), 'Time (s)'); - axis (handles(2, 2), 'tight'); - - hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... - '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... - '$\sqrt{I_{L}^2 + Q_{L}^2}$'); - - %set interpreter from tex to latex. This will draw \sqrt correctly - set(hLegend, 'Interpreter', 'Latex'); - - %----- PLL discriminator filtered---------------------------------- - plot (handles(3, 1), timeAxisInSeconds, ... - trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess-1), 'b'); - - grid (handles(3, 1)); - axis (handles(3, 1), 'tight'); - xlabel(handles(3, 1), 'Time (s)'); - ylabel(handles(3, 1), 'Amplitude'); - title (handles(3, 1), 'Filtered PLL discriminator'); - - %----- DLL discriminator unfiltered-------------------------------- - plot (handles(3, 2), timeAxisInSeconds, ... - trackResults(channelNr).dllDiscr(1:settings.msToProcess-1), 'r'); - - grid (handles(3, 2)); - axis (handles(3, 2), 'tight'); - xlabel(handles(3, 2), 'Time (s)'); - ylabel(handles(3, 2), 'Amplitude'); - title (handles(3, 2), 'Raw DLL discriminator'); - - %----- DLL discriminator filtered---------------------------------- - plot (handles(3, 3), timeAxisInSeconds, ... - trackResults(channelNr).dllDiscrFilt(1:settings.msToProcess-1), 'b'); - - grid (handles(3, 3)); - axis (handles(3, 3), 'tight'); - xlabel(handles(3, 3), 'Time (s)'); - ylabel(handles(3, 3), 'Amplitude'); - title (handles(3, 3), 'Filtered DLL discriminator'); - -end % for channelNr = channelList +function plotTracking(channelList, trackResults, settings) +% This function plots the tracking results for the given channel list. +% +% plotTracking(channelList, trackResults, settings) +% +% Inputs: +% channelList - list of channels to be plotted. +% trackResults - tracking results from the tracking function. +% settings - receiver settings. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +% This program is free software; you can redistribute it and/or +% modify it under the terms of the GNU General Public License +% as published by the Free Software Foundation; either version 2 +% of the License, or (at your option) any later version. +% +% This program is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with this program; if not, write to the Free Software +% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +% USA. +%-------------------------------------------------------------------------- + +% Protection - if the list contains incorrect channel numbers +channelList = intersect(channelList, 1:settings.numberOfChannels); + +%=== For all listed channels ============================================== +for channelNr = channelList + + %% Select (or create) and clear the figure ================================ + % The number 200 is added just for more convenient handling of the open + % figure windows, when many figures are closed and reopened. + % Figures drawn or opened by the user, will not be "overwritten" by + % this function. + + figure(channelNr +200); + clf(channelNr +200); + set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... + ' (PRN ', ... + num2str(trackResults(channelNr).PRN), ... + ') results']); + + %% Draw axes ============================================================== + % Row 1 + handles(1, 1) = subplot(3, 3, 1); + handles(1, 2) = subplot(3, 3, [2 3]); + % Row 2 + handles(2, 1) = subplot(3, 3, 4); + handles(2, 2) = subplot(3, 3, [5 6]); + % Row 3 + handles(3, 1) = subplot(3, 3, 7); + handles(3, 2) = subplot(3, 3, 8); + handles(3, 3) = subplot(3, 3, 9); + + %% Plot all figures ======================================================= + + timeAxisInSeconds = (1:settings.msToProcess-1)/1000; + + %----- Discrete-Time Scatter Plot --------------------------------- + plot(handles(1, 1), trackResults(channelNr).I_PN,... + trackResults(channelNr).Q_PN, ... + '.'); + + grid (handles(1, 1)); + axis (handles(1, 1), 'equal'); + title (handles(1, 1), 'Discrete-Time Scatter Plot'); + xlabel(handles(1, 1), 'I prompt'); + ylabel(handles(1, 1), 'Q prompt'); + + %----- Nav bits --------------------------------------------------- + plot (handles(1, 2), timeAxisInSeconds, ... + trackResults(channelNr).I_PN(1:settings.msToProcess-1)); + + grid (handles(1, 2)); + title (handles(1, 2), 'Bits of the navigation message'); + xlabel(handles(1, 2), 'Time (s)'); + axis (handles(1, 2), 'tight'); + + %----- PLL discriminator unfiltered-------------------------------- + plot (handles(2, 1), timeAxisInSeconds, ... + trackResults(channelNr).pllDiscr(1:settings.msToProcess-1), 'r'); + + grid (handles(2, 1)); + axis (handles(2, 1), 'tight'); + xlabel(handles(2, 1), 'Time (s)'); + ylabel(handles(2, 1), 'Amplitude'); + title (handles(2, 1), 'Raw PLL discriminator'); + + %----- Correlation ------------------------------------------------ + plot(handles(2, 2), timeAxisInSeconds, ... + [sqrt(trackResults(channelNr).I_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 + ... + trackResults(channelNr).Q_P(1:settings.msToProcess-1).^2)', ... + sqrt(trackResults(channelNr).I_L(1:settings.msToProcess-1).^2 + ... + trackResults(channelNr).Q_L(1:settings.msToProcess-1).^2)'], ... + '-*'); + + grid (handles(2, 2)); + title (handles(2, 2), 'Correlation results'); + xlabel(handles(2, 2), 'Time (s)'); + axis (handles(2, 2), 'tight'); + + hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... + '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... + '$\sqrt{I_{L}^2 + Q_{L}^2}$'); + + %set interpreter from tex to latex. This will draw \sqrt correctly + set(hLegend, 'Interpreter', 'Latex'); + + %----- PLL discriminator filtered---------------------------------- + plot (handles(3, 1), timeAxisInSeconds, ... + trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess-1), 'b'); + + grid (handles(3, 1)); + axis (handles(3, 1), 'tight'); + xlabel(handles(3, 1), 'Time (s)'); + ylabel(handles(3, 1), 'Amplitude'); + title (handles(3, 1), 'Filtered PLL discriminator'); + + %----- DLL discriminator unfiltered-------------------------------- + plot (handles(3, 2), timeAxisInSeconds, ... + trackResults(channelNr).dllDiscr(1:settings.msToProcess-1), 'r'); + + grid (handles(3, 2)); + axis (handles(3, 2), 'tight'); + xlabel(handles(3, 2), 'Time (s)'); + ylabel(handles(3, 2), 'Amplitude'); + title (handles(3, 2), 'Raw DLL discriminator'); + + %----- DLL discriminator filtered---------------------------------- + plot (handles(3, 3), timeAxisInSeconds, ... + trackResults(channelNr).dllDiscrFilt(1:settings.msToProcess-1), 'b'); + + grid (handles(3, 3)); + axis (handles(3, 3), 'tight'); + xlabel(handles(3, 3), 'Time (s)'); + ylabel(handles(3, 3), 'Amplitude'); + title (handles(3, 3), 'Filtered DLL discriminator'); + +end % for channelNr = channelList From 4a52e74b3157391e88c188b4a8c848cf96ff0ef1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 30 Mar 2018 12:13:48 +0200 Subject: [PATCH 2/2] End all files with a new line --- src/utils/matlab/galileo_e5a_dll_pll_plot_sample.m | 2 +- src/utils/matlab/help_script1.m | 2 +- src/utils/matlab/libs/geoFunctions/cart2utm.m | 2 +- src/utils/matlab/libs/geoFunctions/dms2mat.m | 1 - 4 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/utils/matlab/galileo_e5a_dll_pll_plot_sample.m b/src/utils/matlab/galileo_e5a_dll_pll_plot_sample.m index 40660f06a..ad80b985c 100644 --- a/src/utils/matlab/galileo_e5a_dll_pll_plot_sample.m +++ b/src/utils/matlab/galileo_e5a_dll_pll_plot_sample.m @@ -92,4 +92,4 @@ for N=1:1:channels t = t/1000; plot(t, GNSS_tracking(N).carrier_doppler_hz / 1000); xlabel('Time(s)'); ylabel('Doppler(KHz)'); title(['Doppler frequency channel ' num2str(N)]); -end \ No newline at end of file +end diff --git a/src/utils/matlab/help_script1.m b/src/utils/matlab/help_script1.m index 865d43b83..0716950e8 100644 --- a/src/utils/matlab/help_script1.m +++ b/src/utils/matlab/help_script1.m @@ -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.prn_delay_ms(channel,:),'r') \ No newline at end of file +% plot(GNSS_observables.prn_delay_ms(channel,:),'r') diff --git a/src/utils/matlab/libs/geoFunctions/cart2utm.m b/src/utils/matlab/libs/geoFunctions/cart2utm.m index 48da278ea..3a6034230 100644 --- a/src/utils/matlab/libs/geoFunctions/cart2utm.m +++ b/src/utils/matlab/libs/geoFunctions/cart2utm.m @@ -170,4 +170,4 @@ if neg_geo == 'TRUE ' N = -N + 20000000; end; -%%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%% \ No newline at end of file +%%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/dms2mat.m b/src/utils/matlab/libs/geoFunctions/dms2mat.m index a82b60ccc..a5c449673 100644 --- a/src/utils/matlab/libs/geoFunctions/dms2mat.m +++ b/src/utils/matlab/libs/geoFunctions/dms2mat.m @@ -101,4 +101,3 @@ elseif nargout == 3 else error('Invalid number of output arguments') end -