gps_pvt 0.5.1 → 0.6.2

data/ext/ninja-scan-light/tool/navigation/coordinate.h

@@ -51,6 +51,7 @@
 #endif
 
 #include "param/vector3.h"
+#include "param/quaternion.h"
 #include "param/matrix.h"
 
 #include "WGS84.h"
@@ -130,6 +131,73 @@ class System_3D {
       in >> self[2];
       return in;
     }
+
+
+    ///< Coordinate rotation
+    struct Rotation {
+      Quaternion<FloatT> q;
+      Rotation() : q(1, 0, 0, 0) {}
+      Rotation &then(const Quaternion<FloatT> &q_) {
+        q *= q_;
+        return *this;
+      }
+      Rotation &then(const Rotation &rot) {
+        return then(rot.q);
+      }
+      Rotation &then_x(const FloatT &rad) {
+        return then(Quaternion<FloatT>(
+            std::cos(rad / 2), std::sin(rad / 2), 0, 0));
+      }
+      Rotation &then_y(const FloatT &rad) {
+        return then(Quaternion<FloatT>(
+            std::cos(rad / 2), 0, std::sin(rad / 2), 0));
+      }
+      Rotation &then_z(const FloatT &rad) {
+        return then(Quaternion<FloatT>(
+            std::cos(rad / 2), 0, 0, std::sin(rad / 2)));
+      }
+      operator Matrix<FloatT>() const {
+        return q.getDCM();
+      }
+      /**
+       * Perform coordinate rotation
+       * For example; application of clockwise 90 degree coordinate rotation along to z axis
+       * to vector(1,0,0) yields to vector(0,-1,0).
+       * This is not identical to vector rotation along with some axis,
+       * which corresponds to vector(0,1,0) of example, i.e.,
+       * z-axis 90 degree rotation of vector(1,0,0).
+       *
+       * @param vec target vector to which rotation is applied.
+       */
+      void apply(FloatT (&vec)[3]) const {
+#if 0
+        // as definition: (~q * vec * q).vector();
+        Vector3<FloatT> v(((q.conj() *= Vector3<FloatT>(vec.v)) *= q).vector());
+        vec[0] = v[0];
+        vec[1] = v[1];
+        vec[2] = v[2];
+#else // optimized version
+        FloatT q0(q[0]), qv[3] = {q[1], q[2], q[3]};
+        FloatT q0_(qv[0] * vec[0] + qv[1] * vec[1] + qv[2] * vec[2]), // -q~.vec (i*) vec
+            qv_[3] = { // q~[0] * vec + q~.vec (e*) vec
+              q0 * vec[0] - qv[1] * vec[2] + qv[2] * vec[1], // polarity checked; q~.vec = -qv
+              q0 * vec[1] - qv[2] * vec[0] + qv[0] * vec[2],
+              q0 * vec[2] - qv[0] * vec[1] + qv[1] * vec[0]};
+        vec[0] = q0_ * qv[0] + q0 * qv_[0] + qv_[1] * qv[2] - qv_[2] * qv[1];
+        vec[1] = q0_ * qv[1] + q0 * qv_[1] + qv_[2] * qv[0] - qv_[0] * qv[2];
+        vec[2] = q0_ * qv[2] + q0 * qv_[2] + qv_[0] * qv[1] - qv_[1] * qv[0];
+#endif
+      }
+
+      /**
+       * Perform coordinate rotation by invoking apply(vec.v)
+       *
+       * @see apply(FloatT (&vec)[3])
+       */
+      void apply(System_3D &vec) const {
+        apply(vec.v);
+      }
+    };
 };
 
 template <class FloatT, class Earth> class System_XYZ;
@@ -186,7 +254,6 @@ class System_XYZ : public System_3D<FloatT> {
     }
     
     FloatT dist() const {
-      
       return FloatT(std::sqrt(
           pow2(x()) + pow2(y()) + pow2(z())));
     }
@@ -220,6 +287,19 @@ class System_XYZ : public System_3D<FloatT> {
           std::atan2(y(), x()),
           (p / std::cos(_lat) - n));
     }
+
+    /**
+     * Get coordinates in a delayed (i.e., rotated) ECEF frame.
+     *
+     * @param (delay_sec) delayed seconds (both positive (delayed) and negative (hasten) values
+     * are acceptable)
+     * @return (self_t) coordinates
+     */
+    self_t after(const FloatT &delay_sec) const {
+      FloatT rad(delay_sec * -Earth::Omega_Earth_IAU); // Earth rotation is negative direction (y->x)
+      FloatT crad(std::cos(rad)), srad(std::sin(rad));
+      return self_t(x() * crad - y() * srad, x() * srad + y() * crad, z());
+    }
 };
 
 template <class FloatT, class Earth>

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

@@ -22,6 +22,8 @@
 #include "navigation/QZSS.h"
 #include "navigation/GLONASS.h"
 #include "navigation/RINEX.h"
+#include "navigation/SP3.h"
+#include "navigation/ANTEX.h"
 
 #include "navigation/GPS_Solver_Base.h"
 #include "navigation/GPS_Solver.h"
@@ -300,6 +302,10 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
   GPS_Ephemeris(const typename GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris &eph) 
       : GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris(eph),
       iode_subframe3(eph.iode) {}
+  struct constellation_res_t {
+    System_XYZ<FloatT, WGS84> position, velocity;
+    FloatT clock_error, clock_error_dot;
+  };
 };
 %}
 %extend GPS_Ephemeris {
@@ -387,20 +393,21 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
         break;
     }
   }
-  %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 bool &with_velocity = true) const {
-    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
-        self->constellation(t, pseudo_range, with_velocity));
-    position = res.position;
-    velocity = res.velocity;
+  %typemap(out) constellation_res_t {
+    %append_output(SWIG_NewPointerObj((new System_XYZ<FloatT, WGS84>($1.position)), 
+        $descriptor(System_XYZ<FloatT, WGS84> *), SWIG_POINTER_OWN));
+    %append_output(SWIG_NewPointerObj((new System_XYZ<FloatT, WGS84>($1.velocity)), 
+        $descriptor(System_XYZ<FloatT, WGS84> *), SWIG_POINTER_OWN));
+    %append_output(swig::from($1.clock_error));
+    %append_output(swig::from($1.clock_error_dot));
+  }
+  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
+      const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0) const {
+    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
+        self->constellation(t_tx, dt_transit, true));
+    typename GPS_Ephemeris<FloatT>::constellation_res_t res = {
+        pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot(t_tx)};
+    return res;
   }
 #if defined(SWIGRUBY)
   %rename("consistent?") is_consistent;
@@ -459,15 +466,14 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
   MAKE_ACCESSOR(ddx, FloatT); MAKE_ACCESSOR(ddy, FloatT); MAKE_ACCESSOR(ddz, FloatT);
   MAKE_ACCESSOR(a_Gf0, FloatT);
   MAKE_ACCESSOR(a_Gf1, FloatT);
-  %apply GPS_Ephemeris::System_XYZ<FloatT, WGS84> & { System_XYZ<FloatT, WGS84> & }; // TODO ineffective?
-  void constellation(
-      System_XYZ<FloatT, WGS84> &position, System_XYZ<FloatT, WGS84> &velocity,
-      const GPS_Time<FloatT> &t, const FloatT &pseudo_range = 0,
+  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
+      const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0,
       const bool &with_velocity = true) const {
-    typename SBAS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
-        self->constellation(t, pseudo_range, with_velocity));
-    position = res.position;
-    velocity = res.velocity;
+    typename SBAS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
+        self->constellation(t_tx, dt_transit, with_velocity));
+    typename GPS_Ephemeris<FloatT>::constellation_res_t res = {
+        pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot(t_tx)};
+    return res;
   }
 }
 
@@ -610,23 +616,13 @@ struct GLONASS_Ephemeris
     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;
+  typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
+      const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0) const {
+    typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
+        self->constellation(t_tx, dt_transit));
+    typename GPS_Ephemeris<FloatT>::constellation_res_t res = {
+        pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot()};
+    return res;
   }
 #if defined(SWIGRUBY)
   %rename("consistent?") is_consistent;
@@ -1378,7 +1374,28 @@ struct GPS_Solver
     GPS_SpaceNode<FloatT> space_node;
     GPS_SolverOptions<FloatT> options;
     GPS_SinglePositioning<FloatT> solver;
-    gps_t() : space_node(), options(), solver(space_node) {}
+    struct ephemeris_proxy_t {
+      struct item_t {
+        const void *impl;
+        typename base_t::satellite_t (*impl_select)(
+            const void *,
+            const typename base_t::prn_t &, const typename base_t::gps_time_t &);
+      } gps, qzss;
+      static typename base_t::satellite_t forward(
+          const void *ptr,
+          const typename base_t::prn_t &prn, const typename base_t::gps_time_t &t){
+        const ephemeris_proxy_t *proxy(static_cast<const ephemeris_proxy_t *>(ptr));
+        const item_t &target(((prn >= 193) && (prn <= 202)) ? proxy->qzss : proxy->gps);
+        return target.impl_select(target.impl, prn, t);
+      }
+      ephemeris_proxy_t(GPS_SinglePositioning<FloatT> &solver){
+        gps.impl = qzss.impl = solver.satellites.impl;
+        gps.impl_select = qzss.impl_select = solver.satellites.impl_select;
+        solver.satellites.impl = this;
+        solver.satellites.impl_select = forward;
+      }
+    } ephemeris_proxy;
+    gps_t() : space_node(), options(), solver(space_node), ephemeris_proxy(solver) {}
   } gps;
   struct sbas_t {
     SBAS_SpaceNode<FloatT> space_node;
@@ -1636,6 +1653,101 @@ template <class FloatT>
 struct RINEX_Observation {};
 }
 
+%extend SP3 {
+  %typemap(in,numinputs=0) int count[ANY] (int temp[$1_dim0]) "$1 = temp;"
+  %typemap(argout) int count[ANY] {
+    for(int i(0); i < $1_dim0; ++i){
+      %append_output(SWIG_From(int)($1[i]));
+    }
+  }
+  void satellites(int count[SP3::SYS_SYSTEMS]) const {
+    typename SP3_Product<FloatT>::satellite_count_t x(self->satellite_count());
+    count[SP3<FloatT>::SYS_GPS] = x.gps;
+    count[SP3<FloatT>::SYS_SBAS] = x.sbas;
+    count[SP3<FloatT>::SYS_QZSS] = x.qzss;
+    count[SP3<FloatT>::SYS_GLONASS] = x.glonass;
+    count[SP3<FloatT>::SYS_GALILEO] = x.galileo;
+    count[SP3<FloatT>::SYS_BEIDOU] = x.beidou;
+  }
+
+} 
+%inline {
+template <class FloatT>
+struct SP3 : public SP3_Product<FloatT> {
+  int read(const char *fname) {
+    std::fstream fin(fname, std::ios::in | std::ios::binary);
+    return SP3_Reader<FloatT>::read_all(fin, *this);
+  }
+  enum system_t {
+    SYS_GPS,
+    SYS_SBAS,
+    SYS_QZSS,
+    SYS_GLONASS,
+    SYS_GALILEO,
+    SYS_BEIDOU,
+    SYS_SYSTEMS,
+  };
+  bool push(GPS_Solver<FloatT> &solver, const system_t &sys) const {
+    switch(sys){
+      case SYS_GPS:
+        return SP3_Product<FloatT>::push(
+            solver.gps.ephemeris_proxy.gps, SP3_Product<FloatT>::SYSTEM_GPS);
+      case SYS_SBAS:
+        return SP3_Product<FloatT>::push(
+            solver.sbas.solver.satellites, SP3_Product<FloatT>::SYSTEM_SBAS);
+      case SYS_QZSS:
+        return SP3_Product<FloatT>::push(
+            solver.gps.ephemeris_proxy.qzss, SP3_Product<FloatT>::SYSTEM_QZSS);
+      case SYS_GLONASS:
+        return SP3_Product<FloatT>::push(
+            solver.glonass.solver.satellites, SP3_Product<FloatT>::SYSTEM_GLONASS);
+      case SYS_GALILEO:
+      case SYS_BEIDOU:
+      default:
+        break;
+    }
+    return false;
+  }
+  bool push(GPS_Solver<FloatT> &solver) const {
+    system_t target[] = {
+      SYS_GPS,
+      SYS_SBAS,
+      SYS_QZSS,
+      SYS_GLONASS,
+      //SYS_GALILEO,
+      //SYS_BEIDOU,
+    };
+    for(std::size_t i(0); i < sizeof(target) / sizeof(target[0]); ++i){
+      if(!push(solver, target[i])){return false;}
+    }
+    return true;
+  }
+  System_XYZ<FloatT, WGS84> position(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).position(t);
+  }
+  System_XYZ<FloatT, WGS84> velocity(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).velocity(t);
+  }
+  FloatT clock_error(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).clock_error(t);
+  }
+  FloatT clock_error_dot(
+      const int &sat_id, const GPS_Time<FloatT> &t) const {
+    return SP3_Product<FloatT>::select(sat_id, t).clock_error_dot(t);
+  }
+  int apply_antex(const char *fname) {
+    ANTEX_Product<FloatT> antex;
+    std::fstream fin(fname, std::ios::in | std::ios::binary);
+    int read_items(ANTEX_Reader<FloatT>::read_all(fin, antex));
+    if(read_items < 0){return read_items;}
+    return antex.move_to_antenna_position(*this);
+  }
+};
+}
+
 #undef MAKE_ACCESSOR
 #undef MAKE_VECTOR2ARRAY
 #undef MAKE_ARRAY_INPUT
@@ -1663,6 +1775,7 @@ struct RINEX_Observation {};
 %template(SolverOptions_GLONASS) GLONASS_SolverOptions<type>;
 
 %template(RINEX_Observation) RINEX_Observation<type>;
+%template(SP3) SP3<type>;
 %enddef
 
 CONCRETIZE(double);