New receiver feature: optional carrier smoothing of code range observables

src/algorithms/observables/CMakeLists.txt

@@ -5,5 +5,6 @@
 # SPDX-License-Identifier: GPL-3.0-or-later
 #
 
+add_subdirectory(libs)
 add_subdirectory(adapters)
 add_subdirectory(gnuradio_blocks)

src/algorithms/observables/adapters/CMakeLists.txt

@@ -27,6 +27,7 @@ target_link_libraries(obs_adapters
     PUBLIC
         obs_gr_blocks
         algorithms_libs
+        observables_libs
     PRIVATE
         Gflags::gflags
         Glog::glog

src/algorithms/observables/adapters/hybrid_observables.cc

@@ -18,9 +18,9 @@
  * -------------------------------------------------------------------------
  */
 
-
 #include "hybrid_observables.h"
 #include "configuration_interface.h"
+#include "obs_conf.h"
 #include <glog/logging.h>
 #include <ostream>  // for operator<<
 
@@ -34,7 +34,21 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration,
     dump_mat_ = configuration->property(role + ".dump_mat", true);
     dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
 
-    observables_ = hybrid_observables_gs_make(in_streams_, out_streams_, dump_, dump_mat_, dump_filename_);
+    Obs_Conf conf;
+
+    conf.dump = dump_;
+    conf.dump_mat = dump_mat_;
+    conf.dump_filename = dump_filename_;
+    conf.nchannels_in = in_streams_;
+    conf.nchannels_out = out_streams_;
+
+    conf.enable_carrier_smoothing = configuration->property(role + ".enable_carrier_smoothing", false);
+    conf.smoothing_factor = configuration->property(role + ".smoothing_factor", 200.0);
+    if (conf.enable_carrier_smoothing == true)
+        {
+            LOG(INFO) << "Observables carrier smoothing enabled with smoothing factor " << conf.smoothing_factor;
+        }
+    observables_ = hybrid_observables_gs_make(conf);
     DLOG(INFO) << "Observables block ID (" << observables_->unique_id() << ")";
 }
 

src/algorithms/observables/gnuradio_blocks/CMakeLists.txt

@@ -37,6 +37,7 @@ target_link_libraries(obs_gr_blocks
         Boost::headers
         Gnuradio::blocks
     PRIVATE
+        observables_libs
         algorithms_libs
         core_system_parameters
         Gflags::gflags

src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc

@@ -55,19 +55,15 @@ namespace errorlib = boost::system;
 #endif
 
 
-hybrid_observables_gs_sptr hybrid_observables_gs_make(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, bool dump_mat, const std::string &dump_filename)
+hybrid_observables_gs_sptr hybrid_observables_gs_make(const Obs_Conf &conf_)
 {
-    return hybrid_observables_gs_sptr(new hybrid_observables_gs(nchannels_in, nchannels_out, dump, dump_mat, dump_filename));
+    return hybrid_observables_gs_sptr(new hybrid_observables_gs(conf_));
 }
 
 
-hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in,
+hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block("hybrid_observables_gs",
-    uint32_t nchannels_out,
+                                                                          gr::io_signature::make(conf_.nchannels_in, conf_.nchannels_in, sizeof(Gnss_Synchro)),
-    bool dump,
+                                                                          gr::io_signature::make(conf_.nchannels_out, conf_.nchannels_out, sizeof(Gnss_Synchro)))
-    bool dump_mat,
-    const std::string &dump_filename) : gr::block("hybrid_observables_gs",
-                                            gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)),
-                                            gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro)))
 {
     // PVT input message port
     this->message_port_register_in(pmt::mp("pvt_to_observables"));
@@ -75,12 +71,12 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in,
 
     // Send Channel status to gnss_flowgraph
     this->message_port_register_out(pmt::mp("status"));
-
+    d_conf = conf_;
-    d_dump = dump;
+    d_dump = conf_.dump;
-    d_dump_mat = dump_mat and d_dump;
+    d_dump_mat = conf_.dump_mat and d_dump;
-    d_dump_filename = dump_filename;
+    d_dump_filename = conf_.dump_filename;
-    d_nchannels_out = nchannels_out;
+    d_nchannels_out = conf_.nchannels_out;
-    d_nchannels_in = nchannels_in;
+    d_nchannels_in = conf_.nchannels_in;
     d_gnss_synchro_history = std::make_shared<Gnss_circular_deque<Gnss_Synchro>>(1000, d_nchannels_out);
 
     // ############# ENABLE DATA FILE LOG #################
@@ -134,6 +130,12 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in,
     // rework
     d_Rx_clock_buffer.set_capacity(10);  // 10*20 ms = 200 ms of data in buffer
     d_Rx_clock_buffer.clear();           // Clear all the elements in the buffer
+
+    d_channel_last_pll_lock = std::vector<bool>(d_nchannels_out, false);
+    d_channel_last_pseudorange_smooth = std::vector<double>(d_nchannels_out, 0.0);
+    d_channel_last_carrier_phase_rads = std::vector<double>(d_nchannels_out, 0.0);
+
+    d_smooth_filter_M = conf_.smoothing_factor;
 }
 
 
@@ -525,6 +527,75 @@ void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data)
         }
 }
 
+void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
+{
+    std::vector<Gnss_Synchro>::iterator it;
+    for (it = data.begin(); it != data.end(); it++)
+        {
+            if (it->Flag_valid_pseudorange)
+                {
+                    //0. get wavelength for the current signal
+                    double wavelength_m = 0;
+                    switch (mapStringValues_[it->Signal])
+                        {
+                        case evGPS_1C:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ1;
+                            break;
+                        case evGPS_L5:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ5;
+                            break;
+                        case evSBAS_1C:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ1;
+                            break;
+                        case evGAL_1B:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ1;
+                            break;
+                        case evGAL_5X:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ5;
+                            break;
+                        case evGPS_2S:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ2;
+                            break;
+                        case evBDS_B3:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ3_BDS;
+                            break;
+                        case evGLO_1G:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ1_GLO;
+                            break;
+                        case evGLO_2G:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ2_GLO;
+                            break;
+                        case evBDS_B1:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ1_BDS;
+                            break;
+                        case evBDS_B2:
+                            wavelength_m = SPEED_OF_LIGHT / FREQ2_BDS;
+                            break;
+                        default:
+                            break;
+                        }
+
+                    //todo: propagate the PLL lock status in Gnss_Synchro
+                    //1. check if last PLL lock status was false and initialize last d_channel_last_pseudorange_smooth
+                    if (d_channel_last_pll_lock.at(it->Channel_ID) == true)
+                        {
+                            //2. Compute the smoothed pseudorange for this channel
+                            // Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/)
+                            double r_sm = d_channel_last_pseudorange_smooth.at(it->Channel_ID);
+                            double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M);
+                            it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / PI_2) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads.at(it->Channel_ID));
+                        }
+                    d_channel_last_pseudorange_smooth.at(it->Channel_ID) = it->Pseudorange_m;
+                    d_channel_last_carrier_phase_rads.at(it->Channel_ID) = it->Carrier_phase_rads;
+                    d_channel_last_pll_lock.at(it->Channel_ID) = it->Flag_valid_pseudorange;
+                }
+            else
+                {
+                    d_channel_last_pll_lock.at(it->Channel_ID) = false;
+                }
+        }
+}
+
 
 int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)),
     gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
@@ -607,11 +678,16 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
                     compute_pranges(epoch_data);
                 }
 
+            //Carrier smoothing (optional)
+            if (d_conf.enable_carrier_smoothing == true)
+                {
+                    smooth_pseudoranges(epoch_data);
+                }
+            //output the observables set to the PVT block
             for (uint32_t n = 0; n < d_nchannels_out; n++)
                 {
                     out[n][0] = epoch_data[n];
                 }
-
             // report channel status every second
             T_status_report_timer_ms += T_rx_step_ms;
             if (T_status_report_timer_ms >= 1000)

src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h

@@ -23,6 +23,7 @@
 #ifndef GNSS_SDR_HYBRID_OBSERVABLES_GS_H
 #define GNSS_SDR_HYBRID_OBSERVABLES_GS_H
 
+#include "obs_conf.h"
 #include <boost/circular_buffer.hpp>  // for boost::circular_buffer
 #include <boost/shared_ptr.hpp>       // for boost::shared_ptr
 #include <gnuradio/block.h>           // for block
@@ -41,12 +42,7 @@ class Gnss_circular_deque;
 
 using hybrid_observables_gs_sptr = boost::shared_ptr<hybrid_observables_gs>;
 
-hybrid_observables_gs_sptr hybrid_observables_gs_make(
+hybrid_observables_gs_sptr hybrid_observables_gs_make(const Obs_Conf& conf_);
-    unsigned int nchannels_in,
-    unsigned int nchannels_out,
-    bool dump,
-    bool dump_mat,
-    const std::string& dump_filename);
 
 /*!
  * \brief This class implements a block that computes observables
@@ -60,19 +56,33 @@ public:
         gr_vector_const_void_star& input_items, gr_vector_void_star& output_items);
 
 private:
-    friend hybrid_observables_gs_sptr hybrid_observables_gs_make(
+    friend hybrid_observables_gs_sptr hybrid_observables_gs_make(const Obs_Conf& conf_);
-        uint32_t nchannels_in,
-        uint32_t nchannels_out,
-        bool dump,
-        bool dump_mat,
-        const std::string& dump_filename);
 
-    hybrid_observables_gs(
+    hybrid_observables_gs(const Obs_Conf& conf_);
-        uint32_t nchannels_in,
+
-        uint32_t nchannels_out,
+    Obs_Conf d_conf;
-        bool dump,
+
-        bool dump_mat,
+    enum StringValue
-        const std::string& dump_filename);
+    {
+        evGPS_1C,
+        evGPS_2S,
+        evGPS_L5,
+        evSBAS_1C,
+        evGAL_1B,
+        evGAL_5X,
+        evGLO_1G,
+        evGLO_2G,
+        evBDS_B1,
+        evBDS_B2,
+        evBDS_B3
+    };
+
+    std::map<std::string, StringValue> mapStringValues_;
+    std::vector<bool> d_channel_last_pll_lock;
+    std::vector<double> d_channel_last_pseudorange_smooth;
+    std::vector<double> d_channel_last_carrier_phase_rads;
+    double d_smooth_filter_M;
+    void smooth_pseudoranges(std::vector<Gnss_Synchro>& data);
 
     bool T_rx_TOW_set;  // rx time follow GPST
     bool d_dump;

src/algorithms/observables/libs/CMakeLists.txt

@@ -0,0 +1,52 @@
+# Copyright (C) 2012-2019  (see AUTHORS file for a list of contributors)
+#
+# 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 <https://www.gnu.org/licenses/>.
+#
+
+set(OBSERVABLES_LIB_HEADERS ${OBSERVABLES_LIB_HEADERS} obs_conf.h)
+set(OBSERVABLES_LIB_SOURCES ${OBSERVABLES_LIB_SOURCES} obs_conf.cc)
+
+list(SORT OBSERVABLES_LIB_HEADERS)
+list(SORT OBSERVABLES_LIB_SOURCES)
+
+source_group(Headers FILES ${OBSERVABLES_LIB_HEADERS})
+
+add_library(observables_libs
+    ${OBSERVABLES_LIB_SOURCES}
+    ${OBSERVABLES_LIB_HEADERS}
+)
+
+target_link_libraries(observables_libs
+    PRIVATE
+        Gflags::gflags
+        Glog::glog
+        algorithms_libs
+        core_system_parameters
+)
+
+if(ENABLE_CLANG_TIDY)
+    if(CLANG_TIDY_EXE)
+        set_target_properties(observables_libs
+            PROPERTIES
+                CXX_CLANG_TIDY "${DO_CLANG_TIDY}"
+        )
+    endif()
+endif()
+
+set_property(TARGET observables_libs
+    APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
+)

src/algorithms/observables/libs/obs_conf.cc

@@ -0,0 +1,43 @@
+/*!
+ * \file obs_conf.h
+ * \brief Class that contains all the configuration parameters for generic
+ * observables block
+ * \author Javier Arribas, 2020. jarribas(at)cttc.es
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2019  (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 <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "obs_conf.h"
+
+Obs_Conf::Obs_Conf()
+{
+    enable_carrier_smoothing = false;
+    smoothing_factor = 200;
+    nchannels_in = 0;
+    nchannels_out = 0;
+    dump = false;
+    dump_mat = false;
+    dump_filename = "obs_dump.dat";
+}

src/algorithms/observables/libs/obs_conf.h

@@ -0,0 +1,55 @@
+/*!
+ * \file obs_conf.h
+ * \brief Class that contains all the configuration parameters for generic
+ * observables block
+ * \author Javier Arribas, 2020. jarribas(at)cttc.es
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2020  (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 <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_OBS_CONF_H_
+#define GNSS_SDR_OBS_CONF_H_
+
+#include "configuration_interface.h"
+#include <cstddef>
+#include <cstdint>
+#include <string>
+
+class Obs_Conf
+{
+public:
+    bool enable_carrier_smoothing;
+    double smoothing_factor;
+    unsigned int nchannels_in;
+    unsigned int nchannels_out;
+    bool dump;
+    bool dump_mat;
+    std::string dump_filename;
+
+
+    Obs_Conf();
+};
+
+#endif