Hatch filter [do not merge, for reference only]

include/libswiftnav/signal.h

@@ -26,6 +26,7 @@
 
 #define CONSTELLATION_GPS 0
 #define CONSTELLATION_SBAS 1
+#define CONSTELLATION_INVALID 255
 
 #define BAND_L1 0
 
@@ -55,5 +56,12 @@ static inline bool sid_is_equal(const gnss_signal_t a, const gnss_signal_t b)
   return sid_compare(a, b) == 0;
 }
 
+/** Sets a signal ID such that it will not compare equal with any valid signal
+ * ID. */
+static inline void sid_mark_invalid(gnss_signal_t *s)
+{
+  s->constellation = CONSTELLATION_INVALID;
+}
+
 #endif /* LIBSWIFTNAV_SIGNAL_H */
 

include/libswiftnav/track.h

@@ -172,6 +172,15 @@ typedef struct {
   u16 lock_counter;
 } navigation_measurement_t;
 
+typedef struct {
+  double last_pseudorange;
+  double last_carrier_phase;
+  gnss_signal_t sid;
+  u16 last_lock_counter;
+} hatch_state_t;
+
+#define HATCH_N_DEFAULT 5
+
 void calc_loop_gains(float bw, float zeta, float k, float loop_freq,
                      float *b0, float *b1);
 float costas_discriminator(float I, float Q);
@@ -231,12 +240,18 @@ void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0,
                   float cutoff_freq, float loop_freq);
 float cn0_est(cn0_est_state_t *s, float I, float Q);
 
+void hatch_filter_init(hatch_state_t *s, u8 ns);
+void hatch_filter_update(hatch_state_t *s, u8 ns, double N, u8 nm,
+                         navigation_measurement_t *nms);
+
 void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[],
                                  navigation_measurement_t nav_meas[],
-                                 double nav_time, ephemeris_t ephemerides[]);
+                                 double nav_time_tc, gps_time_t *nav_time,
+                                 ephemeris_t ephemerides[]);
 void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[],
                                   navigation_measurement_t* nav_meas[],
-                                  double nav_time, ephemeris_t* ephemerides[]);
+                                  double nav_time_tc, gps_time_t *nav_time,
+                                  ephemeris_t* ephemerides[]);
 
 int nav_meas_cmp(const void *a, const void *b);
 u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new,

src/track.c

@@ -14,13 +14,16 @@
 #include <string.h>
 #include <math.h>
 #include <float.h>
+#include <assert.h>
 
 #include "constants.h"
 #include "prns.h"
 #include "track.h"
 #include "ephemeris.h"
 #include "tropo.h"
 #include "coord_system.h"
+#include "signal.h"
+#include "set.h"
 
 /** \defgroup track Tracking
  * Functions used in tracking.
@@ -745,8 +748,77 @@ float cn0_est(cn0_est_state_t *s, float I, float Q)
   return s->log_bw - 10.f*log10f(s->nsr);
 }
 
+
+void hatch_filter_init(hatch_state_t *s, u8 ns)
+{
+  for (u8 i=0; i<ns; i++) {
+    sid_mark_invalid(&s[i].sid);
+  }
+}
+
+static void hatch_intersection_f(void *context, u32 n,
+                                 const void *a, const void *b)
+{
+  (void)n;
+  navigation_measurement_t *nm = (navigation_measurement_t *)a;
+  hatch_state_t *s = (hatch_state_t *)b;
+  double N = *(double *)context;
+
+  /* Double checking. */
+  assert(sid_is_equal(s->sid, nm->sid));
+
+  nm->pseudorange = (1 / N) * nm->pseudorange
+                    + ((N - 1) / N) * (s->last_pseudorange
+                                        + nm->carrier_phase
+                                        - s->last_carrier_phase);
+}
+
+static int cmp_hatch_nm(const void *a, const void *b)
+{
+  navigation_measurement_t *nm = (navigation_measurement_t *)a;
+  hatch_state_t *s = (hatch_state_t *)b;
+  return sid_compare(nm->sid, s->sid);
+}
+
+void hatch_filter_update(hatch_state_t *s, u8 ns, double N, u8 nm,
+                         navigation_measurement_t *nms)
+{
+  assert(ns >= nm);
+
+  intersection_map(nm, sizeof(navigation_measurement_t), nms,
+                   ns, sizeof(hatch_state_t), s,
+                   cmp_hatch_nm, &N, hatch_intersection_f);
+
+  u8 i;
+  for (i=0; i<MIN(ns, nm); i++) {
+    s[i].sid = nms[i].sid;
+    s[i].last_pseudorange = nms[i].pseudorange;
+    s[i].last_carrier_phase = nms[i].carrier_phase;
+  }
+  for (; i<ns; i++) {
+    sid_mark_invalid(&s[i].sid);
+  }
+}
+
+/** Calculate observations from tracking channel measurements.
+ *
+ * This function takes flat arrays, for a version taking a arrays of pointers
+ * see calc_navigation_measurement_().
+ *
+ * \param n_channels Number of tracking channel measurements
+ * \param meas Array of tracking channel measurements, length `n_channels`
+ * \param nav_meas Output array for the observations, length `n_channels`
+ * \param nav_time_rx Receiver time at which to calculate the position solution
+ *                    in seconds
+ * \param nav_time Pointer to GPS time at which to calculate the position
+ *                 solution. Can be `NULL` in which case one of the
+ *                 pseudoranges is chosen as a reference and set to a nominal
+ *                 range, implying a certain receiver clock error
+ * \param ephemerides Array of ephemerides
+ */
 void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[],
-                                 navigation_measurement_t nav_meas[], double nav_time,
+                                 navigation_measurement_t nav_meas[],
+                                 double nav_time_rx, gps_time_t *nav_time,
                                  ephemeris_t ephemerides[])
 {
   channel_measurement_t* meas_ptrs[n_channels];
@@ -759,51 +831,96 @@ void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[],
     ephemerides_ptrs[i] = &ephemerides[meas[i].sid.sat];
   }
 
-  calc_navigation_measurement_(n_channels, meas_ptrs, nav_meas_ptrs, nav_time, ephemerides_ptrs);
+  calc_navigation_measurement_(n_channels, meas_ptrs, nav_meas_ptrs,
+                               nav_time_rx, nav_time, ephemerides_ptrs);
 }
 
-void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], double nav_time, ephemeris_t* ephemerides[])
+/** Calculate observations from tracking channel measurements.
+ *
+ * This function takes an array of pointers, for a version taking a flat array
+ * see calc_navigation_measurement().
+ *
+ * \param n_channels Number of tracking channel measurements
+ * \param meas Array of pointers to tracking channel measurements, length
+ *             `n_channels`
+ * \param nav_meas Array of pointers of where to store the output observations,
+ *                 length `n_channels`
+ * \param nav_time_rx Receiver time at which to calculate the position solution
+ *                    in seconds
+ * \param nav_time Pointer to GPS time at which to calculate the position
+ *                 solution. Can be `NULL` in which case one of the
+ *                 pseudoranges is chosen as a reference and set to a nominal
+ *                 range, implying a certain receiver clock error
+ * \param ephemerides Array of pointers to ephemerides
+ */
+void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[],
+                                  navigation_measurement_t* nav_meas[],
+                                  double nav_time_rx, gps_time_t *nav_time,
+                                  ephemeris_t* ephemerides[])
 {
-  double TOTs[n_channels];
-  double min_TOF = -DBL_MAX;
   double clock_err[n_channels], clock_rate_err[n_channels];
 
   for (u8 i=0; i<n_channels; i++) {
-    TOTs[i] = 1e-3 * meas[i]->time_of_week_ms;
-    TOTs[i] += meas[i]->code_phase_chips / 1.023e6;
-    TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6;
-
-    /** \todo Maybe keep track of week number in tracking channel
-        state or derive it from system time. */
-    nav_meas[i]->tot.tow = TOTs[i];
+    nav_meas[i]->sid = meas[i]->sid;
+
+    /* Compute the time of transmit of the signal on the satellite from the
+     * tracking loop parameters. This will be used to compute the pseudorange. */
+    nav_meas[i]->tot.tow = 1e-3 * meas[i]->time_of_week_ms;
+    nav_meas[i]->tot.tow += meas[i]->code_phase_chips / 1.023e6;
+    nav_meas[i]->tot.tow += (nav_time_rx - meas[i]->receiver_time)
+                            * meas[i]->code_phase_rate / 1.023e6;
+    /* For now use the week number from the ephemeris. */
+    /* TODO: Should we use a more reliable source for the week number? */
     gps_time_match_weeks(&nav_meas[i]->tot, &ephemerides[i]->toe);
 
-    nav_meas[i]->raw_doppler = meas[i]->carrier_freq;
-    nav_meas[i]->snr = meas[i]->snr;
-    nav_meas[i]->sid.sat = meas[i]->sid.sat;
-
+    /* Compute the carrier phase measurement. */
     nav_meas[i]->carrier_phase = meas[i]->carrier_phase;
-    nav_meas[i]->carrier_phase += (nav_time - meas[i]->receiver_time) * meas[i]->carrier_freq;
+    nav_meas[i]->carrier_phase += (nav_time_rx - meas[i]->receiver_time)
+                                  * meas[i]->carrier_freq;
 
+    /* For raw Doppler we use the instantaneous carrier frequency from the
+     * tracking loop. */
+    nav_meas[i]->raw_doppler = meas[i]->carrier_freq;
+
+    /* Copy over remaining values. */
+    nav_meas[i]->snr = meas[i]->snr;
     nav_meas[i]->lock_counter = meas[i]->lock_counter;
 
-    /* calc sat clock error */
+    /* Calculate satellite position, velocity and clock error */
     calc_sat_state(ephemerides[i], nav_meas[i]->tot,
                    nav_meas[i]->sat_pos, nav_meas[i]->sat_vel,
                    &clock_err[i], &clock_rate_err[i]);
+  }
 
-    /* remove clock error to put all tots within the same time window */
-    if ((TOTs[i] + clock_err[i]) > min_TOF)
-      min_TOF = TOTs[i];
+  /* To calculate the pseudorange from the time of transmit we need the local
+   * time of reception. */
+  gps_time_t tor;
+  if (nav_time) {
+    /* If we were given a time, use that. */
+    tor = *nav_time;
+  } else {
+    /* If we were not given a recieve time then we can just set one of the
+     * pseudoranges aribtrarily to a nominal value and reference all the other
+     * pseudoranges to that. This doesn't affect the PVT solution but does
+     * potentially correspond to a large receiver clock error. */
+    tor = nav_meas[0]->tot;
+    tor.tow += GPS_NOMINAL_RANGE / GPS_C;
+    tor = normalize_gps_time(tor);
   }
 
   for (u8 i=0; i<n_channels; i++) {
-    nav_meas[i]->raw_pseudorange = (min_TOF - TOTs[i])*GPS_C + GPS_NOMINAL_RANGE;
-
-    nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange \
-                               + clock_err[i]*GPS_C;
-    nav_meas[i]->doppler = nav_meas[i]->raw_doppler + clock_rate_err[i]*GPS_L1_HZ;
-
+    /* The raw pseudorange is just the time of flight divided by the speed of
+     * light. */
+    nav_meas[i]->raw_pseudorange = GPS_C * gpsdifftime(tor, nav_meas[i]->tot);
+
+    /* The corrected pseudorange and Doppler applies the clock error and clock
+     * rate error correction from the ephemeris respectively. */
+    nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange
+                               + clock_err[i] * GPS_C;
+    nav_meas[i]->doppler = nav_meas[i]->raw_doppler
+                           + clock_rate_err[i] * GPS_L1_HZ;
+
+    /* We also apply the clock correction to the time of transmit. */
     nav_meas[i]->tot.tow -= clock_err[i];
     nav_meas[i]->tot = normalize_gps_time(nav_meas[i]->tot);
   }