gps_pvt 0.3.0 → 0.4.1

data/ext/ninja-scan-light/tool/swig/GPS.i

@@ -20,12 +20,14 @@
 #include "navigation/GPS.h"
 #include "navigation/SBAS.h"
 #include "navigation/QZSS.h"
+#include "navigation/GLONASS.h"
 #include "navigation/RINEX.h"
 
 #include "navigation/GPS_Solver_Base.h"
 #include "navigation/GPS_Solver.h"
 #include "navigation/GPS_Solver_RAIM.h"
 #include "navigation/SBAS_Solver.h"
+#include "navigation/GLONASS_Solver.h"
 
 #if defined(__cplusplus) && (__cplusplus < 201103L)
 namespace std {
@@ -153,6 +155,12 @@ static std::string inspect_str(const VALUE &v){
     $1 = (TYPE($input) == T_ARRAY) ? 1 : 0;
   }
 #endif
+  %ignore canonicalize();
+  %ignore GPS_Time(const int &_week, const float_t &_seconds);
+  %typemap(in, numinputs=0) void *dummy "";
+  GPS_Time(const int &week_, const float_t &seconds_, void *dummy){
+    return &((new GPS_Time<FloatT>(week_, seconds_))->canonicalize());
+  }
   %apply int *OUTPUT { int *week };
   %apply FloatT *OUTPUT { FloatT *seconds };
   void to_a(int *week, FloatT *seconds) const {
@@ -500,6 +508,160 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
 
 %include navigation/SBAS.h
 
+%inline %{
+template <class FloatT>
+struct GLONASS_Ephemeris 
+    : public GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time {
+  typedef typename GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time eph_t;
+  unsigned int super_frame, has_string;
+  typename eph_t::raw_t raw;
+  void invalidate() {
+    super_frame = 0;
+    has_string = 0;
+  }
+  bool is_consistent() const {
+    return has_string == 0x1F;
+  }
+  GLONASS_Ephemeris() : eph_t() {
+    invalidate();
+  }
+  GLONASS_Ephemeris(const eph_t &eph) 
+      : eph_t(eph),
+      super_frame(0), has_string(0), raw() {
+    raw = *this;
+    has_string = 0x1F;
+  }
+};
+%}
+%extend GLONASS_Ephemeris {
+  MAKE_ACCESSOR(svid, unsigned int);
+  
+  MAKE_ACCESSOR(freq_ch, int); // frequency channel to be configured
+  MAKE_ACCESSOR(t_k, unsigned int);
+  MAKE_ACCESSOR(t_b, unsigned int);
+  MAKE_ACCESSOR(M, unsigned int);
+  MAKE_ACCESSOR(gamma_n, FloatT);
+  MAKE_ACCESSOR(tau_n, FloatT);
+
+  MAKE_ACCESSOR(xn, FloatT); MAKE_ACCESSOR(xn_dot, FloatT); MAKE_ACCESSOR(xn_ddot, FloatT);
+  MAKE_ACCESSOR(yn, FloatT); MAKE_ACCESSOR(yn_dot, FloatT); MAKE_ACCESSOR(yn_ddot, FloatT);
+  MAKE_ACCESSOR(zn, FloatT); MAKE_ACCESSOR(zn_dot, FloatT); MAKE_ACCESSOR(zn_ddot, FloatT);
+
+  MAKE_ACCESSOR(B_n, unsigned int);
+  MAKE_ACCESSOR(p, unsigned int);
+  MAKE_ACCESSOR(N_T, unsigned int);
+  MAKE_ACCESSOR(F_T, FloatT);
+  MAKE_ACCESSOR(n, unsigned int);
+  MAKE_ACCESSOR(delta_tau_n, FloatT);
+  MAKE_ACCESSOR(E_n, unsigned int);
+  MAKE_ACCESSOR(P1, unsigned int);
+  MAKE_ACCESSOR(P2, bool);
+  MAKE_ACCESSOR(P4, bool);
+
+  MAKE_ACCESSOR(tau_c, FloatT);
+  MAKE_ACCESSOR(tau_GPS, FloatT);
+  
+  FloatT frequency_L1() const {
+    return self->L1_frequency();
+  };
+  FloatT frequency_L2() const {
+    return self->L2_frequency();
+  };
+  GPS_Time<FloatT> base_time() const {
+    return self->base_time();
+  }
+
+  //MAKE_ACCESSOR(l_n, bool); // exists in both Ephemeris and Time_Properties
+
+  MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int));
+  bool parse(const unsigned int buf[4], const unsigned int &leap_seconds = 0){
+    typedef typename GLONASS_SpaceNode<FloatT>
+        ::template BroadcastedMessage<unsigned int> parser_t;
+    unsigned int super_frame(buf[3] >> 16), frame(buf[3] & 0xF), string_no(parser_t::m(buf));
+    unsigned int has_string(self->has_string);
+    if((has_string > 0) && (self->super_frame != super_frame)){
+      has_string = 0; // clean up
+    }
+    self->super_frame = super_frame;
+    has_string |= (0x1 << (string_no - 1));
+    switch(string_no){
+      case 1: self->raw.template update_string1<0, 0>(buf); break;
+      case 2: self->raw.template update_string2<0, 0>(buf); break;
+      case 3: self->raw.template update_string3<0, 0>(buf); break;
+      case 4: self->raw.template update_string4<0, 0>(buf); break;
+      case 5: {
+        self->raw.template update_string5<0, 0>(buf);
+        if(frame == 4){
+          // TODO: require special care for 50th frame? @see Table 4.9 note (4)
+        }
+        break;
+      }
+    }
+    bool updated(false);
+    if((has_string == 0x1F) && (has_string != self->has_string)){
+      updated = true;
+      // All ephemeris and time info. in the same super frame has been acquired, 
+      // and this block is called once per one same super frame.
+      // Ephemeris_with_Time::raw_t =(cast)=> Ephemeris_with_Time => Ephemeris_with_GPS_Time
+      static_cast<GLONASS_Ephemeris<FloatT>::eph_t &>(*self) 
+          = GLONASS_Ephemeris<FloatT>::eph_t(self->raw);
+      self->t_b_gps += leap_seconds;
+    }
+    self->has_string = has_string;
+    return updated;
+  }
+  FloatT clock_error(
+      const GPS_Time<FloatT> &t_arrival, const FloatT &pseudo_range = 0) const {
+    return self->clock_error(t_arrival, pseudo_range);
+  }
+  %typemap(in,numinputs=0) System_XYZ<FloatT, WGS84> & (System_XYZ<FloatT, WGS84> temp) %{
+    $1 = &temp;
+  %}
+  %typemap(argout) System_XYZ<FloatT, WGS84> & {
+    %append_output(SWIG_NewPointerObj((new $*1_ltype(*$1)), $1_descriptor, SWIG_POINTER_OWN));
+  }
+  void constellation(
+      System_XYZ<FloatT, WGS84> &position, System_XYZ<FloatT, WGS84> &velocity,
+      const GPS_Time<FloatT> &t, const FloatT &pseudo_range = 0) const {
+    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
+        self->constellation(t, pseudo_range));
+    position = res.position;
+    velocity = res.velocity;
+  }
+#if defined(SWIGRUBY)
+  %rename("consistent?") is_consistent;
+  %rename("in_range?") is_in_range;
+#endif
+  bool is_in_range(const GPS_Time<FloatT> &t) const {
+    // "invalidate()" is used to make raw and converted data inconsistent.
+    return self->is_valid(t);
+  }
+}
+
+%extend GLONASS_SpaceNode {
+  %fragment(SWIG_Traits_frag(FloatT));
+  %ignore satellites() const;
+  %ignore satellite(const int &);
+  %ignore latest_ephemeris() const;
+  void register_ephemeris(
+      const int &prn, const GLONASS_Ephemeris<FloatT> &eph,
+      const int &priority_delta = 1){
+    self->satellite(prn).register_ephemeris(eph, priority_delta);
+  }
+  GLONASS_Ephemeris<FloatT> ephemeris(const int &prn) const {
+    return GLONASS_Ephemeris<FloatT>(
+        %const_cast(self, GLONASS_SpaceNode<FloatT> *)->satellite(prn).ephemeris());
+  }
+  int read(const char *fname) {
+    std::fstream fin(fname, std::ios::in | std::ios::binary);
+    typename RINEX_NAV_Reader<FloatT>::space_node_list_t list = {NULL};
+    list.glonass = self;
+    return RINEX_NAV_Reader<FloatT>::read_all(fin, list);
+  }
+}
+
+%include navigation/GLONASS.h
+
 %extend GPS_User_PVT {
   %ignore solver_t;
   %ignore base_t;
@@ -591,8 +753,9 @@ struct GPS_User_PVT
   Matrix<FloatT, Array2D_Dense<FloatT> > G_enu() const {
     return proxy_t::linear_solver_t::rotate_G(base_t::G, base_t::user_position.ecef2enu());
   }
-  typename proxy_t::linear_solver_t linear_solver() const {
-    return typename proxy_t::linear_solver_t(base_t::G, base_t::W, base_t::delta_r);
+  typename proxy_t::linear_solver_t::partial_t linear_solver() const {
+    return typename proxy_t::linear_solver_t(base_t::G, base_t::W, base_t::delta_r)
+        .partial(used_satellites());
   }
   Matrix<FloatT, Array2D_Dense<FloatT> > C() const {
     return linear_solver().C();
@@ -605,14 +768,26 @@ struct GPS_User_PVT
     linear_solver().least_square(res);
     return res;
   }
+  Matrix<FloatT, Array2D_Dense<FloatT> > S_enu(
+      const Matrix<FloatT, Array2D_Dense<FloatT> > &s) const {
+    return proxy_t::linear_solver_t::rotate_S(s, base_t::user_position.ecef2enu());
+  }
   Matrix<FloatT, Array2D_Dense<FloatT> > S_enu() const {
-    return proxy_t::linear_solver_t::rotate_S(S(), base_t::user_position.ecef2enu());
+    return S_enu(S());
+  }
+  Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV(
+      const Matrix<FloatT, Array2D_Dense<FloatT> > &s) const {
+    return linear_solver().slope_HV(s);
   }
   Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV() const {
-    return linear_solver().slope_HV(S());
+    return slope_HV(S());
+  }
+  Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV_enu(
+      const Matrix<FloatT, Array2D_Dense<FloatT> > &s) const {
+    return linear_solver().slope_HV(s, base_t::user_position.ecef2enu());
   }
   Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV_enu() const {
-    return linear_solver().slope_HV(S(), base_t::user_position.ecef2enu());
+    return slope_HV_enu(S());
   }
   
   void fd(const typename base_t::detection_t **out) const {*out = &(base_t::FD);}
@@ -878,6 +1053,25 @@ struct SBAS_SolverOptions
 };
 %}
 
+%extend GLONASS_SolverOptions {
+  %ignore base_t;
+  %ignore cast_general;
+  MAKE_VECTOR2ARRAY(int);
+}
+%inline %{
+template <class FloatT>
+struct GLONASS_SolverOptions 
+    : public GLONASS_SinglePositioning<FloatT>::options_t, 
+    GPS_SolverOptions_Common<FloatT> {
+  typedef typename GLONASS_SinglePositioning<FloatT>::options_t base_t;
+  void exclude(const int &prn){base_t::exclude_prn.set(prn);}
+  void include(const int &prn){base_t::exclude_prn.reset(prn);}
+  std::vector<int> excluded() const {return base_t::exclude_prn.excluded();}
+  GPS_Solver_GeneralOptions<FloatT> *cast_general(){return this;}
+  const GPS_Solver_GeneralOptions<FloatT> *cast_general() const {return this;}
+};
+%}
+
 %header {
 template <class FloatT>
 struct GPS_RangeCorrector
@@ -906,6 +1100,8 @@ struct GPS_RangeCorrector
   %ignore gps;
   %ignore sbas_t;
   %ignore sbas;
+  %ignore glonass_t;
+  %ignore glonass;
   %ignore select_solver;
   %ignore relative_property;
   %ignore satellite_position;
@@ -1097,6 +1293,8 @@ struct GPS_RangeCorrector
         ID2SYM(rb_intern("gps_tropospheric")),
         ID2SYM(rb_intern("sbas_ionospheric")),
         ID2SYM(rb_intern("sbas_tropospheric")),
+        ID2SYM(rb_intern("glonass_ionospheric")),
+        ID2SYM(rb_intern("glonass_tropospheric")),
       };
       static const VALUE k_opt(ID2SYM(rb_intern("options")));
       static const VALUE k_f_10_7(ID2SYM(rb_intern("f_10_7")));
@@ -1218,6 +1416,12 @@ struct GPS_Solver
     SBAS_SinglePositioning<FloatT> solver;
     sbas_t() : space_node(), options(), solver(space_node) {}
   } sbas;
+  struct glonass_t {
+    GLONASS_SpaceNode<FloatT> space_node;
+    GLONASS_SolverOptions<FloatT> options;
+    GLONASS_SinglePositioning<FloatT> solver;
+    glonass_t() : space_node(), options(), solver(space_node) {}
+  } glonass;
   SWIG_Object hooks;
   typedef std::vector<GPS_RangeCorrector<FloatT> > user_correctors_t;
   user_correctors_t user_correctors;
@@ -1233,7 +1437,7 @@ struct GPS_Solver
   }
 #endif
   GPS_Solver() : super_t(),
-      gps(), sbas(),
+      gps(), sbas(), glonass(),
       hooks(), user_correctors() {
 #ifdef SWIGRUBY
     hooks = rb_hash_new();
@@ -1245,20 +1449,25 @@ struct GPS_Solver
     tropospheric.push_back(&gps.solver.tropospheric_simplified);
     gps.solver.ionospheric_correction
         = sbas.solver.ionospheric_correction
+        = glonass.solver.ionospheric_correction
         = ionospheric;
     gps.solver.tropospheric_correction
         = sbas.solver.tropospheric_correction
+        = glonass.solver.tropospheric_correction
         = tropospheric;
   }
   GPS_SpaceNode<FloatT> &gps_space_node() {return gps.space_node;}
   GPS_SolverOptions<FloatT> &gps_options() {return gps.options;}
   SBAS_SpaceNode<FloatT> &sbas_space_node() {return sbas.space_node;}
   SBAS_SolverOptions<FloatT> &sbas_options() {return sbas.options;}
+  GLONASS_SpaceNode<FloatT> &glonass_space_node() {return glonass.space_node;}
+  GLONASS_SolverOptions<FloatT> &glonass_options() {return glonass.options;}
   const base_t &select_solver(
       const typename base_t::prn_t &prn) const {
     if(prn > 0 && prn <= 32){return gps.solver;}
     if(prn >= 120 && prn <= 158){return sbas.solver;}
     if(prn > 192 && prn <= 202){return gps.solver;}
+    if(prn > 0x100 && prn <= (0x100 + 24)){return glonass.solver;}
     // call order: base_t::solve => this returned by select() 
     //     => relative_property() => select_solver()
     // For not supported satellite, call loop prevention is required.
@@ -1286,6 +1495,8 @@ struct GPS_Solver
     const_cast<gps_t &>(gps).solver.update_options(gps.options);
     const_cast<sbas_t &>(sbas).space_node.update_all_ephemeris(receiver_time);
     const_cast<sbas_t &>(sbas).solver.update_options(sbas.options);
+    const_cast<glonass_t &>(glonass).space_node.update_all_ephemeris(receiver_time);
+    const_cast<glonass_t &>(glonass).solver.update_options(glonass.options);
     return super_t::solve().user_pvt(measurement.items, receiver_time);
   }
   typedef 
@@ -1300,6 +1511,8 @@ struct GPS_Solver
       &gps.solver.tropospheric_correction,
       &sbas.solver.ionospheric_correction,
       &sbas.solver.tropospheric_correction,
+      &glonass.solver.ionospheric_correction,
+      &glonass.solver.tropospheric_correction,
     };
     for(std::size_t i(0); i < sizeof(root) / sizeof(root[0]); ++i){
       do{
@@ -1476,6 +1689,10 @@ struct RINEX_Observation {};
 %template(SpaceNode_SBAS) SBAS_SpaceNode<type>;
 %template(SolverOptions_SBAS) SBAS_SolverOptions<type>;
 
+%template(SpaceNode_GLONASS) GLONASS_SpaceNode<type>;
+%template(Ephemeris_GLONASS) GLONASS_Ephemeris<type>;
+%template(SolverOptions_GLONASS) GLONASS_SolverOptions<type>;
+
 %template(RINEX_Observation) RINEX_Observation<type>;
 %enddef
 

data/ext/ninja-scan-light/tool/swig/SylphideMath.i

@@ -437,8 +437,6 @@ class Matrix : public Matrix_Frozen<T, Array2D_Type, ViewType> {
 #endif
 
     typedef Matrix<T, Array2D_Type, ViewType> self_t;
-    self_t &swapRows(const unsigned int &row1, const unsigned int &row2);
-    self_t &swapColumns(const unsigned int &column1, const unsigned int &column2);
 };
 
 %inline {
@@ -995,7 +993,23 @@ MAKE_TO_S(Matrix_Frozen)
   }
 #endif
 
-  %typemap(out) self_t & "$result = self;"
+  /*
+   * Returning (*this) requires special care;
+   * "self_t &func(){return (*this);}" in a C++ file may generate
+   * a new wrapped object deleted by GC in the target language
+   * unless the care.
+   * 
+   * Work around 1)
+   *   %typemap(in, numinputs=0) self_t *self_p "";
+   *   %typemap(argout) self_t *self_p "$result = self;";
+   *   void func(self_t *self_p){...}
+   *
+   * Work around 2) (useful without overwrite of the original source, but may overfit)
+   *   %typemap(out) self_t & "$result = self;"
+   *   self_t &func(){...; return *$self;}
+   */
+  %typemap(in, numinputs=0) self_t *self_p "";
+  %typemap(argout) self_t *self_p "$result = self;";
 
   T &__setitem__(const unsigned int &row, const unsigned int &column, const T &value) {
     return (($self)->operator()(row, column) = value);
@@ -1012,41 +1026,50 @@ MAKE_TO_S(Matrix_Frozen)
 #endif
   %rename("scalar") getScalar;
   %rename("I") getI;
-  %rename("swap_rows") swapRows;
-  %rename("swap_columns") swapColumns;
+
+  void swap_rows(
+      self_t *self_p,
+      const unsigned int &r1, const unsigned int &r2){
+    $self->swapRows(r1, r2);
+  }
+  void swap_columns(
+      self_t *self_p,
+      const unsigned int &c1, const unsigned int &c2){
+    $self->swapColumns(c1, c2);
+  }
 
   template <class T2, class Array2D_Type2, class ViewType2>
-  self_t &replace(const Matrix_Frozen<T2, Array2D_Type2, ViewType2> &matrix){
-    return $self->replace(matrix);
+  void replace(
+      self_t *self_p,
+      const Matrix_Frozen<T2, Array2D_Type2, ViewType2> &matrix){
+    $self->replace(matrix);
   }
   INSTANTIATE_MATRIX_FUNC(replace, replace);
 
-  self_t &replace(const void *replacer = NULL){
+  void replace(self_t *self_p, const void *replacer = NULL){
     if(!MatrixUtil::replace(*$self, replacer)){
       throw std::runtime_error("Unsupported replacement");
     }
-    return *$self;
   }
 
-  self_t &replace(const T *serialized){
+  void replace(self_t *self_p, const T *serialized){
     if(!MatrixUtil::replace(*$self, serialized)){
       throw std::runtime_error("Unsupported replacement");
     }
-    return *$self;
   }
   
 #ifdef SWIGRUBY
-  %bang swapRows(const unsigned int &, const unsigned int &);
-  %bang swapColumns(const unsigned int &, const unsigned int &);
+  %bang swap_rows;
+  %bang swap_columns;
   %rename("replace!") replace;
   
-  self_t &map_bang(
+  void map_bang(
+      self_t *self_p,
       void (*each_func)(
         const T &src, T *dst,
         const unsigned int &i, const unsigned int &j), 
       const typename MatrixUtil::each_which_t &each_which = MatrixUtil::EACH_ALL){
     MatrixUtil::each(*$self, each_func, each_which, $self);
-    return *$self;
   }
   %rename("map!") map_bang;
   %alias map_bang "collect!,map_with_index!,collect_with_index!";

data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb

@@ -253,6 +253,7 @@ __RINEX_OBS_TEXT__
       t_meas = GPS::Time::new(1849, 172413)
       puts "Measurement time: #{t_meas.to_a} (a.k.a #{"%d/%d/%d %02d:%02d:%02d UTC"%[*t_meas.c_tm]})"
       expect(t_meas.c_tm).to eq([2015, 6, 15, 23, 53, 33])
+      expect(GPS::Time::new(0, t_meas.serialize)).to eq(t_meas)
       
       sn.update_all_ephemeris(t_meas)
       

data/ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb

@@ -214,9 +214,19 @@ shared_examples 'Matrix' do
     it 'is swappable with swap_rows! or swap_cloumns!' do
       mat_builtin = Matrix[*compare_with[0]]
       [:swap_rows!, :swap_columns!].each.with_index{|func, i|
-        params[:rc][i].times.to_a.combination(2).to_a{|a, b|
-          mat[0].send(func, a, b)
-          mat_builtin.send(func, a, b)
+        params[:rc][i].times.to_a.combination(2).to_a.each{|a, b|
+          mat_mod = mat[0].send(func, a, b)
+          case func
+          when :swap_rows!
+            idxs = mat_builtin.row_count.times.to_a
+            idxs[a], idxs[b] = [b, a]
+            mat_builtin = Matrix::rows(mat_builtin.row_vectors.values_at(*idxs))
+          when :swap_columns!
+            idxs = mat_builtin.column_count.times.to_a
+            idxs[a], idxs[b] = [b, a]
+            mat_builtin = Matrix::columns(mat_builtin.column_vectors.values_at(*idxs))
+          end
+          expect(mat_mod).to equal(mat[0])
           expect(mat[0].to_a).to eq(mat_builtin.to_a)
         }
       }

data/lib/gps_pvt/receiver.rb

@@ -7,15 +7,22 @@ require_relative 'GPS'
 
 module GPS_PVT
 class Receiver
+
+  GPS::Time.send(:define_method, :utc){ # send as work around of old Ruby
+    res = c_tm(GPS::Time::guess_leap_seconds(self))
+    res[-1] += (seconds % 1)
+    res
+  }
+
   def self.pvt_items(opt = {})
     opt = {
       :system => [[:GPS, 1..32]],
       :satellites => (1..32).to_a,
     }.merge(opt)
     [[
-      [:week, :itow_rcv, :year, :month, :mday, :hour, :min, :sec],
+      [:week, :itow_rcv, :year, :month, :mday, :hour, :min, :sec_rcv_UTC],
       proc{|pvt|
-        [:week, :seconds, :c_tm].collect{|f| pvt.receiver_time.send(f)}.flatten
+        [:week, :seconds, :utc].collect{|f| pvt.receiver_time.send(f)}.flatten
       }
     ]] + [[
       [:receiver_clock_error_meter, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U],
@@ -146,7 +153,7 @@ class Receiver
       rel_prop
     }
     @debug = {}
-    solver_opts = [:gps_options, :sbas_options].collect{|target|
+    solver_opts = [:gps_options, :sbas_options, :glonass_options].collect{|target|
       @solver.send(target)
     }
     solver_opts.each{|opt|
@@ -261,6 +268,11 @@ class Receiver
             prns.each{|prn| @solver.sbas_options.send(mode, prn)}
           elsif check_sys_svid.call(:QZSS, 193..202) then
             [svid || (193..202).to_a].flatten.each{|prn| @solver.gps_options.send(mode, prn)}
+          elsif check_sys_svid.call(:GLONASS, 1..24, 0x100) then
+            prns = [svid || (1..24).to_a].flatten.collect{|i| (i & 0xFF) + 0x100}
+            labels = prns.collect{|prn| "GLONASS:#{prn & 0xFF}"}
+            update_output.call(:GLONASS, prns, labels)
+            prns.each{|prn| @solver.glonass_options.send(mode, prn & 0xFF)}
           else
             raise "Unknown satellite: #{spec}"
           end
@@ -337,22 +349,23 @@ class Receiver
       [[:@azimuth, az], [:@elevation, el]].each{|k, values|
         self.instance_variable_set(k, Hash[*(sats.zip(values).flatten(1))])
       }
+      mat_S = self.S
       [:@slopeH, :@slopeV] \
-          .zip((self.fd ? self.slope_HV_enu.to_a.transpose : [nil, nil])) \
+          .zip((self.fd ? self.slope_HV_enu(mat_S).to_a.transpose : [nil, nil])) \
           .each{|k, values|
         self.instance_variable_set(k,
             Hash[*(values ? sats.zip(values).flatten(1) : [])])
       }
+      # If a design matrix G has columns larger than 4, 
+      # other states excluding position and time are estimated.
+      @other_state = self.position_solved? \
+          ? (mat_S * self.delta_r.partial(self.used_satellites, 1, 0, 0)).transpose.to_a[0][4..-1] \
+          : []
       instance_variable_get(target)
     }
-    [:azimuth, :elevation, :slopeH, :slopeV].each{|k|
+    [:azimuth, :elevation, :slopeH, :slopeV, :other_state].each{|k|
       eval("define_method(:#{k}){@#{k} || self.post_solution(:@#{k})}")
     }
-    define_method(:other_state){
-      # If a design matrix G has columns larger than 4, 
-      # other states excluding position and time are estimated.
-      (self.G.rows <= 4) ? [] : (self.S * self.delta_r).transpose.to_a[0][4..-1]
-    }
   }
   
   proc{
@@ -361,7 +374,13 @@ class Receiver
       eph.svid = prn
       [prn, eph]
     }.flatten(1)]
-    define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data|
+    eph_glonass_list = Hash[*(1..24).collect{|num|
+      eph = GPS::Ephemeris_GLONASS::new
+      eph.svid = num
+      [num, eph]
+    }.flatten(1)]
+    define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data, *options|
+      opt = options[0] || {}
       case sys
       when :GPS, :QZSS
         next unless eph = eph_list[prn]
@@ -391,6 +410,15 @@ class Receiver
             $stderr.puts str
           } if @debug[:SBAS_IGP] 
         end
+      when :GLONASS
+        next unless eph = eph_glonass_list[prn]
+        leap_sec = @solver.gps_space_node.is_valid_utc ? 
+            @solver.gps_space_node.iono_utc.delta_t_LS :
+            GPS::Time::guess_leap_seconds(t_meas)
+        next unless eph.parse(bcast_data[0..3], leap_sec)
+        eph.freq_ch = opt[:freq_ch] || 0
+        @solver.glonass_space_node.register_ephemeris(prn, eph)
+        eph.invalidate
       end
     }
   }.call
@@ -402,7 +430,7 @@ class Receiver
     ubx = UBX::new(open(ubx_fname))  
     ubx_kind = Hash::new(0)
     
-    after_run = b || proc{|pvt| puts pvt.to_s}
+    after_run = b || proc{|pvt| puts pvt.to_s if pvt}
     
     gnss_serial = proc{|svid, sys|
       if sys then # new numbering
@@ -468,7 +496,11 @@ class Receiver
           }
           sys, svid = gnss_serial.call(*loader.call(36, 2).reverse)
           case sys
-          when :GPS, :QZSS; 
+          when :GPS, :SBAS, :QZSS;
+          when :GLONASS
+            svid += 0x100
+            meas.add(svid, :L1_FREQUENCY, 
+                GPS::SpaceNode_GLONASS::L1_frequency(loader.call(39, 1, "C") - 7))
           else; next
           end
           trk_stat = loader.call(46, 1)[0]
@@ -502,12 +534,14 @@ class Receiver
             })
       when [0x02, 0x13] # RXM-SFRBX
         sys, svid = gnss_serial.call(packet[6 + 1], packet[6])
+        opt = {}
+        opt[:freq_ch] = packet[6 + 3] - 7 if sys == :GLONASS
         register_ephemeris(
             t_meas,
             sys, svid,
             packet.slice(6 + 8, 4 * packet[6 + 4]).each_slice(4).collect{|v|
               v.pack("C*").unpack("V")[0]
-            })
+            }, opt)
       end
     }
     $stderr.puts ", found packets are %s"%[ubx_kind.inspect]
@@ -517,6 +551,7 @@ class Receiver
     items = [
       @solver.gps_space_node,
       @solver.sbas_space_node,
+      @solver.glonass_space_node,
     ].inject(0){|res, sn|
       loaded_items = sn.send(:read, fname)
       raise "Format error! (Not RINEX) #{fname}" if loaded_items < 0
@@ -526,9 +561,10 @@ class Receiver
   end
   
   def parse_rinex_obs(fname, &b)
-    after_run = b || proc{|pvt| puts pvt.to_s}
+    after_run = b || proc{|pvt| puts pvt.to_s if pvt}
     $stderr.print "Reading RINEX observation file (%s)"%[fname]
     types = nil
+    glonass_freq = nil
     count = 0
     GPS::RINEX_Observation::read(fname){|item|
       $stderr.print '.' if (count += 1) % 1000 == 0
@@ -551,12 +587,32 @@ class Receiver
         }.compact]
       }.flatten(1))]
 
+      glonass_freq ||= proc{|spec|
+        # frequency channels described in observation file
+        next {} unless spec
+        Hash[*(spec.collect{|line|
+          line[4..-1].scan(/R(\d{2}).([\s+-]\d)./).collect{|prn, ch|
+            [prn.to_i, GPS::SpaceNode_GLONASS::L1_frequency(ch.to_i)]
+          }
+        }.flatten(2))]
+      }.call(item[:header]["GLONASS SLOT / FRQ #"])
+
       meas = GPS::Measurement::new
       item[:meas].each{|k, v|
         sys, prn = k
         case sys
         when 'G', ' '
+        when 'S'; prn += 100
         when 'J'; prn += 192
+        when 'R'
+          freq = (glonass_freq[prn] ||= proc{|sn|
+            # frequency channels saved with ephemeris
+            sn.update_all_ephemeris(t_meas)
+            next nil unless sn.ephemeris(prn).in_range?(t_meas)
+            sn.ephemeris(prn).frequency_L1
+          }.call(@solver.glonass_space_node))
+          prn += 0x100
+          meas.add(prn, :L1_FREQUENCY, freq) if freq
         else; next
         end
         types[sys] = (types[' '] || []) unless types[sys]

data/lib/gps_pvt/version.rb

@@ -1,5 +1,5 @@
 # frozen_string_literal: true
 
 module GPS_PVT
-  VERSION = "0.3.0"
+  VERSION = "0.4.1"
 end