Merge pull request #334 from ljbade/time-jump-final

Final CORS fix PR

Author: ljbade

include/libswiftnav/observation.h

@@ -48,7 +48,6 @@ u8 make_propagated_sdiffs_wip(u8 n_local, navigation_measurement_t *m_local,
 u8 make_propagated_sdiffs(u8 n_local, navigation_measurement_t *m_local,
                           u8 n_remote, navigation_measurement_t *m_remote,
                           double *remote_dists, double remote_pos_ecef[3],
-                          const ephemeris_t *e[], const gps_time_t *t,
                           sdiff_t *sds);
 
 s8 make_dd_measurements_and_sdiffs(gnss_signal_t ref_sid, const gnss_signal_t *non_ref_sids, u8 num_dds,

include/libswiftnav/track.h

@@ -161,6 +161,7 @@ typedef struct {
 typedef struct {
   double raw_pseudorange;
   double pseudorange;
+  double raw_carrier_phase;
   double carrier_phase;
   double raw_doppler;
   double doppler;
@@ -234,12 +235,12 @@ void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0,
 float cn0_est(cn0_est_state_t *s, float I, float Q);
 
 s8 calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[],
-                               navigation_measurement_t *nav_meas[],
+                               navigation_measurement_t *nav_meas[], gps_time_t *rec_time,
                                const ephemeris_t* e[]);
 
 int nav_meas_cmp(const void *a, const void *b);
 u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new,
                 u8 n_old, navigation_measurement_t *m_old,
-                navigation_measurement_t *m_corrected);
+                navigation_measurement_t *m_corrected, double dt);
 
 #endif /* LIBSWIFTNAV_TRACK_H */

python/swiftnav/observation.pxd

@@ -40,7 +40,6 @@ cdef extern from "libswiftnav/observation.h":
   u8 make_propagated_sdiffs(u8 n_local, navigation_measurement_t *m_local,
                             u8 n_remote, navigation_measurement_t *m_remote,
                             double *remote_dists, double remote_pos_ecef[3],
-                            const ephemeris_t *e[], const gps_time_t *t,
                             sdiff_t *sds)
 
   s8 make_dd_measurements_and_sdiffs(gnss_signal_t ref_sid, const gnss_signal_t *non_ref_sids,

python/swiftnav/observation.pyx

@@ -71,8 +71,7 @@ def single_diff_(m_a, m_b):
 # TODO (Buro): special note that there are a bunch of statically
 # declared things in the .c file for observation.c
 
-def make_propagated_sdiffs_(m_local, m_remote, remote_dists, remote_pos_ecef,
-                            es, GpsTime t):
+def make_propagated_sdiffs_(m_local, m_remote, remote_dists, remote_pos_ecef):
   """Propagates remote measurements to a local time and makes
   sdiffs. When we get two sets of observations that aren't time
   matched to each other (but are internally time matched within each
@@ -93,8 +92,6 @@ def make_propagated_sdiffs_(m_local, m_remote, remote_dists, remote_pos_ecef,
   cdef navigation_measurement_t m_local_[32], m_remote_[32]
   mk_nav_meas_array(m_local, n_local, &m_local_[0])
   mk_nav_meas_array(m_remote, n_remote, &m_remote_[0])
-  cdef ephemeris_t** es_ = <ephemeris_t **>malloc(len(es)*sizeof(ephemeris_t *))
-  mk_ephemeris_array(es, len(es), es_)
   # TODO (Buro): These can be moved up into type hints.
   cdef np.ndarray[np.double_t, ndim=1, mode="c"] remote_dists_ \
     = np.array(remote_dists, dtype=np.double)
@@ -106,10 +103,7 @@ def make_propagated_sdiffs_(m_local, m_remote, remote_dists, remote_pos_ecef,
                                            n_remote, &m_remote_[0],
                                            &remote_dists_[0],
                                            &remote_pos_ecef_[0],
-                                           es_,
-                                           &t._thisptr,
                                            &sdiffs_[0])
-  free(es_)
   return read_sdiff_array(n_sats, &sdiffs_[0])
 
 def make_dd_measurements_and_sdiffs_(GNSSSignal ref_sid, non_ref_sids, sdiffs_in):

python/swiftnav/track.pxd

@@ -171,6 +171,7 @@ cdef extern from "libswiftnav/track.h":
   ctypedef struct navigation_measurement_t:
     double raw_pseudorange
     double pseudorange
+    double raw_carrier_phase
     double carrier_phase
     double raw_doppler
     double doppler
@@ -184,12 +185,13 @@ cdef extern from "libswiftnav/track.h":
 
   s8 calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[],
                                  navigation_measurement_t *nav_meas[],
-                                 const ephemeris_t* e[])
+                                 gps_time_t *rec_time, const ephemeris_t* e[])
+
 
   int nav_meas_cmp(const void *a, const void *b)
   u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new,
                   u8 n_old, navigation_measurement_t *m_old,
-                  navigation_measurement_t *m_corrected)
+                  navigation_measurement_t *m_corrected, double dt)
 
 cdef class Correlation:
   cdef correlation_t _thisptr

python/swiftnav/track.pyx

@@ -496,11 +496,12 @@ cdef class ChannelMeasurement:
 cdef class NavigationMeasurement:
 
   def __init__(self,
-               raw_pseudorange, pseudorange, carrier_phase, raw_doppler,
-               doppler, sat_pos, sat_vel, snr, lock_time,
+               raw_pseudorange, pseudorange, raw_carrier_phase, carrier_phase,
+               raw_doppler, doppler, sat_pos, sat_vel, snr, lock_time,
                GpsTime tot, GNSSSignal sid, lock_counter):
     self._thisptr.raw_pseudorange = raw_pseudorange
     self._thisptr.pseudorange = pseudorange
+    self._thisptr.raw_carrier_phase = raw_carrier_phase
     self._thisptr.carrier_phase = carrier_phase
     self._thisptr.raw_doppler = raw_doppler
     self._thisptr.doppler = doppler
@@ -546,7 +547,8 @@ cdef mk_nav_meas_array(py_nav_meas, u8 n_c_nav_meas, navigation_measurement_t *c
     memcpy(&c_nav_meas[i], &sd_, sizeof(navigation_measurement_t))
 
 # TODO (Buro): Remove mallocs, etc. here. Do all this in-place
-def _calc_navigation_measurement(chan_meas, nav_meas, ephemerides):
+def _calc_navigation_measurement(chan_meas, nav_meas, GpsTime rec_time,
+                                 ephemerides):
   """
   """
   n_channels = len(chan_meas)
@@ -558,14 +560,15 @@ def _calc_navigation_measurement(chan_meas, nav_meas, ephemerides):
     chan_meas_[n] = &((<ChannelMeasurement ?>chan_meas[n])._thisptr)
     nav_meas_[n] = &((<NavigationMeasurement ?>nav_meas[n])._thisptr)
     ephs[n] = &((<Ephemeris ?>ephemerides[n])._thisptr)
-  calc_navigation_measurement(n_channels, chan_meas_, nav_meas_, ephs)
+  calc_navigation_measurement(n_channels, chan_meas_, nav_meas_,
+                              &rec_time._thisptr, ephs)
   free(chan_meas_)
   free(nav_meas_)
   free(ephs)
   return nav_meas
 
 # TODO (Buro): Remove mallocs, etc. here. Also, wow, this is awful
-def _tdcp_doppler(m_new, m_old):
+def _tdcp_doppler(m_new, m_old, dt):
   n_new = len(m_new)
   n_old = len(m_old)
   n_corrected = min(n_new, n_old)
@@ -579,7 +582,7 @@ def _tdcp_doppler(m_new, m_old):
     m_old_[n] = &((<NavigationMeasurement?>m_old[n])._thisptr)
   for n in range(n_corrected):
     m_corrected_[n] = (<NavigationMeasurement?>m_corrected[n])._thisptr
-  n_written = tdcp_doppler(n_new, m_new_[0], n_old, m_old_[0], m_corrected_)
+  n_written = tdcp_doppler(n_new, m_new_[0], n_old, m_old_[0], m_corrected_, dt)
   free(m_new_)
   free(m_old_)
   free(m_corrected_)

python/tests/test_observation.py

@@ -11,6 +11,7 @@
 
 from swiftnav.ephemeris import Ephemeris
 from swiftnav.pvt import calc_PVT_
+import copy
 import numpy as np
 import pytest
 import swiftnav.time as ti
@@ -23,71 +24,71 @@
                            pseudorange=23946993.888943646,
                            raw_pseudorange=23946993.888943646,
                            sat_pos=(-19477278.087422125, -7649508.9457812719, 16674633.163554827),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=1, code=0),
                            pseudorange=22932174.156858064,
                            raw_pseudorange=22932174.156858064,
                            sat_pos=(-9680013.5408340245, -15286326.354385279, 19429449.383770257),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=2, code=0),
                            pseudorange=24373231.648055989,
                            raw_pseudorange=24373231.648055989,
                            sat_pos=(-19858593.085281931, -3109845.8288993631, 17180320.439503901),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=3, code=0),
                            pseudorange=24779663.252316438,
                            raw_pseudorange=24779663.252316438,
                            sat_pos=(6682497.8716542246, -14006962.389166718, 21410456.275678463),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=4, code=0),
                            pseudorange=26948717.022331879,
                            raw_pseudorange=26948717.022331879,
                            sat_pos=(7415370.9916331079, -24974079.044485383, -3836019.0262199985),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=5, code=0),
                            pseudorange=23327405.435463827,
                            raw_pseudorange=23327405.435463827,
                            sat_pos=(-2833466.1648670658, -22755197.793894723, 13160322.082875408),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=6, code=0),
                            pseudorange=27371419.016328193,
                            raw_pseudorange=27371419.016328193,
                            sat_pos=(14881660.383624561, -5825253.4316490609, 21204679.68313824),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=7, code=0),
                            pseudorange=26294221.697782904,
                            raw_pseudorange=26294221.697782904,
                            sat_pos=(12246530.477279386, -22184711.955107089, 7739084.2855069181),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0)),
    t.NavigationMeasurement(sid=s.GNSSSignal(sat=8, code=0),
                            pseudorange=25781999.479948733,
                            raw_pseudorange=25781999.479948733,
                            sat_pos=(-25360766.249484103, -1659033.490658124, 7821492.0398916304),
-                           sat_vel=(0, 0, 0), carrier_phase=0,
+                           sat_vel=(0, 0, 0), raw_carrier_phase=0, carrier_phase=0,
                            raw_doppler=0, doppler=0, lock_counter=0,
                            snr=0, lock_time=0,
                            tot=ti.GpsTime(tow=0, wn=0))]
@@ -140,49 +141,29 @@ def test_prop_single_diff():
                                       pseudorange=21123480.27105955, doppler=0, raw_doppler=0, carrier_phase=-110993309.26669005,
                                       sat_vel=[-1025.0370403901404, -1821.9217799467374, -2091.6303199092254],
                                       lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.92954684512),
-                                      raw_pseudorange=21121324.476403236,
+                                      raw_pseudorange=21121324.476403236, raw_carrier_phase=-110993309.26669005,
                                       snr=30.0, sid=s.GNSSSignal(code=0, sat=0), lock_counter=0),
               t.NavigationMeasurement(sat_pos=[-16580310.849158794, 918714.1939749047, 20731444.258332774],
                                       pseudorange=22432049.84763688, doppler=0, raw_doppler=0, carrier_phase=-117882743.21601027,
                                       sat_vel=[1060.9864205977192, -2411.43509917502, 953.6270954519971],
                                       lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.9251737675),
-                                      raw_pseudorange=22432340.166121125,
+                                      raw_pseudorange=22432340.166121125, raw_carrier_phase=-117882743.21601027,
                                       snr=30.0, sid=s.GNSSSignal(code=0, sat=2), lock_counter=0)]
-  remote_dists = np.array([ 21121393.87562408,  22432814.46819838])
-  base_pos = np.array( [-2704375, -4263211,  3884637])
-  es = [Ephemeris(**{'ura': 0.0, 'healthy': 1,
-                     'xyz': {'acc': [-6.51925802230835e-08, 4.718410826464475e-09, 2.6226835183928943],
-                             'iod': 0, 'a_gf0': 5153.648313522339, 'a_gf1': 0.5977821557062277,
-                             'pos': [5.122274160385132e-09, 19.0625, 259.9375],
-                             'rate': [1.0151416063308716e-06, 6.260350346565247e-06, -6.332993507385254e-08],
-                             'toa': 4096},
-                     'valid': 1,
-                     'sid': {'code': 0, 'sat': 0},
-                     'toe': {'wn': 1876, 'tow': 172800.0},
-                     'kepler': {'inc_dot': 4.4966158735287064e-10, 'tgd': 5.122274160385132e-09, 'omegadot': -8.099980253833005e-09,
-                                'sqrta': 5153.648313522339, 'inc': 0.9634151551139846, 'cus': 6.260350346565247e-06,
-                                'omega0': 0.5977821557062277, 'cuc': 1.0151416063308716e-06, 'm0': 2.6226835183928943,
-                                'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.718410826464475e-09, 'ecc': 0.0049016030970960855,
-                                'cic': -6.332993507385254e-08, 'crs': 19.0625, 'iode': 74, 'iodc': 21845, 'cis': -6.51925802230835e-08,
-                                'crc': 259.9375, 'w': 0.4885959643259506, 'af0': 7.212162017822266e-06, 'af1': 9.094947017729282e-13, 'af2': 0.0},
-                     'fit_interval': 14400}),
-        Ephemeris(**{'ura': 0.0, 'healthy': 1,
-                     'xyz': {'acc': [-6.146728992462158e-08, 4.742340394655613e-09, -0.8114190126645531], 'iod': 0,
-                             'a_gf0': 5153.798839569092, 'a_gf1': 1.6390180338742641,
-                             'pos': [1.862645149230957e-09, -40.5625, 221.625],
-                             'rate': [-2.2239983081817627e-06, 8.001923561096191e-06, 3.725290298461914e-09], 'toa': 0},
-                     'valid': 1,
-                     'sid': {'code': 0, 'sat': 2},
-                     'toe': {'wn': 1876, 'tow': 172800.0},
-                     'kepler': {'inc_dot': -4.475186409476941e-10, 'tgd': 1.862645149230957e-09, 'omegadot': -8.103551831174965e-09,
-                                'sqrta': 5153.798839569092, 'inc': 0.9587534715647247, 'cus': 8.001923561096191e-06,
-                                'omega0': 1.6390180338742641, 'cuc': -2.2239983081817627e-06, 'm0': -0.8114190126645531,
-                                'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.742340394655613e-09, 'ecc': 0.00035172002390027046,
-                                'cic': 3.725290298461914e-09, 'crs': -40.5625, 'iode': 59, 'iodc': 21845, 'cis': -6.146728992462158e-08,
-                                'crc': 221.625, 'w': 2.9037284161724037, 'af0': -9.969808161258698e-07, 'af1': -5.229594535194337e-12, 'af2': 0.0},
-                     'fit_interval': 14400})]
-  gpst = ti.GpsTime(wn=1876, tow=167132)
-  sdiffs = o.make_propagated_sdiffs_(rover_nm, rover_nm, remote_dists, base_pos, es, gpst)
+  base_nm = [t.NavigationMeasurement(sat_pos=[-19277279.74504115, -8215892.62052003, 16367597.39238895],
+                                      pseudorange=21123480.27105955, doppler=0, raw_doppler=0, carrier_phase=-110993309.26669005,
+                                      sat_vel=[-1025.03418813, -1821.90205153, -2091.65477547],
+                                      lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.92954684512),
+                                      raw_pseudorange=21121324.476403236, raw_carrier_phase=-110993309.26669005,
+                                      snr=30.0, sid=s.GNSSSignal(code=0, sat=0), lock_counter=0),
+              t.NavigationMeasurement(sat_pos=[-16580231.46081397, 918533.75723257, 20731515.61249766],
+                                      pseudorange=22432049.84763688, doppler=0, raw_doppler=0, carrier_phase=-117882743.21601027,
+                                      sat_vel=[1060.97989026, -2411.44777346, 953.59410165],
+                                      lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.9251737675),
+                                      raw_pseudorange=22432340.166121125, raw_carrier_phase=-117882743.21601027,
+                                      snr=30.0, sid=s.GNSSSignal(code=0, sat=2), lock_counter=0)]
+  remote_dists = np.array([ 21121393.87562408, 22432814.46819838])
+  base_pos = np.array( [-2704375, -4263211, 3884637])
+  sdiffs = o.make_propagated_sdiffs_(rover_nm, base_nm, remote_dists, base_pos)
   assert len(sdiffs) == 2
   assert sdiffs[0].pseudorange > 0
   assert sdiffs[1].pseudorange > 0

src/baseline.c

@@ -409,8 +409,9 @@ static bool chi_test(double threshold, u8 num_dds,
  *
  *   -`-1`: < 3 dds
  *   -`-2`: dgelsy  error (see lesq_solution_float)
- *   -`-3`: raim check failed, repair failed
+ *   -`-3`: raim check failed, repair failed, ref satellite was bad
  *   -`-4`: raim check failed, not enough sats for repair
+ *   -`-5`: raim check failed, repair failed, more than one acceptable solution
  */
 /* TODO(dsk) update all call sites to use n_used as calculated here.
  * TODO(dsk) add warn/info logging to call sites when repair occurs.
@@ -501,7 +502,7 @@ s8 lesq_solve_raim(u8 num_dds_u8, const double *dd_obs,
     if (n_used) {
       *n_used = 0;
     }
-    return -3;
+    return -5;
   }
 }
 

src/ephemeris.c

@@ -261,7 +261,6 @@ static s8 calc_sat_state_kepler(const ephemeris_t *e,
 
   /* TODO: Implement convergence test using integer difference of doubles,
    * http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm */
-  /* TODO: Bound number of iterations. */
   do {
     ea_old = ea;
     temp = 1.0 - ecc * cos(ea_old);