Merge pull request #219 from swift-nav/refactor

baseline_ set intersection

Author: scottswift

checks/run-coverage.sh

@@ -1,6 +1,7 @@
 # requires gcovr and diff-cover
 # pip install gcovr diff-cover
 set -e
+unset GIT_DIR
 base="$(git rev-parse --show-toplevel)"
 echo "running coverage check from ${base}"
 gcovr -r "${base}" --object-directory . --xml -o "${base}/.coverage.xml"

checks/setup-hooks.sh

@@ -8,3 +8,4 @@ hooks="$(git rev-parse --git-dir)/hooks"
 echo "Installing hook at: ${hooks}"
 # insert value of builddir as first line of pre-commit
 echo "subdir=$builddir" | cat - checks/pre-commit > "$hooks/pre-commit"
+chmod +x "$hooks/pre-commit"

include/libswiftnav/ambiguity_test.h

@@ -129,4 +129,8 @@ void assign_r_vec(residual_mtxs_t *res_mtxs, u8 num_dds, double *dd_measurements
 void assign_r_mean(residual_mtxs_t *res_mtxs, u8 num_dds, double *hypothesis, double *r_mean);
 double get_quadratic_term(residual_mtxs_t *res_mtxs, u8 num_dds, double *hypothesis, double *r_vec);
 
+void print_hyp(void *arg, element_t *elem);
+void print_intersection_state(intersection_count_t *x);
+
+
 #endif /* LIBSWIFTNAV_AMBIGUITY_TEST_H */

include/libswiftnav/baseline.h

@@ -20,6 +20,11 @@
 /** \addtogroup baseline
  * \{ */
 
+typedef struct {
+  double amb;
+  u8 prn;
+} ambiguity_t;
+
 typedef struct {
   double ambs[MAX_CHANNELS-1];
   u8 prns[MAX_CHANNELS];
@@ -46,12 +51,15 @@ s8 least_squares_solve_b_external_ambs(u8 num_dds, const double *ambs,
          const double ref_ecef[3], double b[3],
          bool disable_raim, double raim_threshold);
 
+void diff_ambs(u8 ref_prn, u8 num_ambs, const ambiguity_t *amb_set,
+               double *dd_ambs);
+s8 baseline_(u8 num_sdiffs, const sdiff_t *sdiffs, const double ref_ecef[3],
+             u8 num_ambs, const ambiguity_t *single_ambs,
+             u8 *num_used, double b[3],
+             bool disable_raim, double raim_threshold);
 s8 baseline(u8 num_sdiffs, const sdiff_t *sdiffs, const double ref_ecef[3],
             const ambiguities_t *ambs, u8 *num_used, double b[3],
             bool disable_raim, double raim_threshold);
-s8 baseline_(u8 num_sdiffs, const sdiff_t *sdiffs, const double ref_ecef[3],
-             u8 num_ambs, const u8 *amb_prns, const double *ambs,
-             u8 *num_used, double b[3], bool disable_raim, double raim_threshold);
 
 void ambiguities_init(ambiguities_t *ambs);
 s8 lesq_solve_raim(u8 num_dds_u8, const double *dd_obs,

include/libswiftnav/memory_pool.h

@@ -84,6 +84,5 @@ s32 memory_pool_product_generator(memory_pool_t *pool, void *x0, u32 n_xs, size_
                                   s8 (*init)(void *x, element_t *elem),
                                   s8 (*next)(void *x, u32 n),
                                   void (*prod)(element_t *new, void *x, u32 n, element_t *elem));
-
 #endif /* LIBSWIFTNAV_MEMORY_POOL_H */
 

include/libswiftnav/observation.h

@@ -30,7 +30,11 @@ typedef struct {
   u8 prn;
 } sdiff_t;
 
+int cmp_sdiff(const void *a_, const void *b_);
+int cmp_amb(const void *a_, const void *b_);
+int cmp_amb_sdiff(const void *a_, const void *b_);
 int cmp_sdiff_prn(const void *a_, const void *b_);
+int cmp_amb_prn(const void *a_, const void *b_);
 
 u8 single_diff(u8 n_a, navigation_measurement_t *m_a,
                u8 n_b, navigation_measurement_t *m_b,

include/libswiftnav/printing_utils.h

@@ -14,13 +14,10 @@
 #define LIBSWIFTNAV_PRINTING_UTILS_H
 
 void print_s32_mtx_diff(u32 m, u32 n, s32 *mat1, s32 *mat2);
-void print_hyp(void *arg, element_t *elem);
 void print_double_mtx(double *m, u32 _r, u32 _c);
 void print_pearson_mtx(double *m, u32 dim);
 void print_s32_mtx_diff(u32 m, u32 n, s32 *mat1, s32 *mat2);
 void print_s32_mtx(s32 *mat, u32 m, u32 n);
 void print_s32_gemv(u32 m, u32 n, s32 *M, s32 *v);
-void print_intersection_state(intersection_count_t *x);
-
 
 #endif /* LIBSWIFTNAV_PRINTING_UTILS_H */

include/libswiftnav/sats_management.h

@@ -30,6 +30,8 @@ typedef struct {
   u8 prns[MAX_CHANNELS];
 } sats_management_t;
 
+u8 choose_reference_sat(const u8 num_sats, const sdiff_t *sats);
+
 void init_sats_management(sats_management_t *sats_management,
                           const u8 num_sats, const sdiff_t *sdiffs, sdiff_t *sdiffs_with_ref_first);
 void print_sats_management(sats_management_t *sats_management);

include/libswiftnav/set.h

@@ -48,5 +48,10 @@ s32 intersection(u32 na, size_t sa, const void *as, void *a_out,
                  u32 nb, size_t sb, const void *bs, void *b_out,
                  cmp_fn cmp);
 
+u32 insertion_index(u32 na, size_t sa, const void *as, void *b, cmp_fn cmp);
+u32 remove_element(u32 na, size_t sa, const void *as, void *a_out,
+                   void *b, cmp_fn cmp);
+u32 insert_element(u32 na, size_t sa, const void *as, void *a_out,
+                   void *b, cmp_fn cmp);
 #endif /* LIBSWIFTNAV_SET_H */
 

src/ambiguity_test.c

@@ -1831,4 +1831,68 @@ double get_quadratic_term(residual_mtxs_t *res_mtxs, u8 num_dds, double *hypothe
   return quad_term;
 }
 
+void print_hyp(void *arg, element_t *elem)
+{
+  u8 num_dds = *( (u8 *) arg );
+
+  hypothesis_t *hyp = (hypothesis_t *)elem;
+  printf("[");
+  for (u8 i=0; i< num_dds; i++) {
+    printf("%"PRId32", ", hyp->N[i]);
+  }
+  printf("]: %f\n", hyp->ll);
+}
+
+/** Prints a s64 valued matrix.
+ *
+ * \param m     The number of rows to be printed in the matrices.
+ * \param n     The number of columns in the matrix.
+ * \param mat1  The matrix to be printed.
+ */
+static void print_s64_mtx(s64 *mat, u32 m, u32 n)
+{
+  for (u32 i=0; i < m; i++) {
+    for (u32 j=0; j < n; j++) {
+      printf("%"PRId64", ", mat[i*n + j]);
+    }
+    printf("\n");
+  }
+  printf("\n");
+}
+
+/* Utilities for debugging inclusion algorithm in ambiguity_test.c */
+static void print_Z(s8 label, u8 full_dim, u8 new_dim, z_t * Z)
+{
+  printf("Z %i:\n", label);
+  print_s64_mtx(Z, full_dim, new_dim);
+}
+
+void print_intersection_state(intersection_count_t *x)
+{
+  u8 full_dim = x->old_dim + x->new_dim;
+
+  printf("itr lower bounds:\n");
+  for (u8 i = 0; i < x->new_dim; i++) {
+    printf("%"PRIi64"\n", x->itr_lower_bounds[i]);
+  }
+  printf("itr upper bounds:\n");
+  for (u8 i = 0; i < x->new_dim; i++) {
+    printf("%"PRIi64"\n", x->itr_upper_bounds[i]);
+  }
+  printf("box lower bounds:\n");
+  for (u8 i = 0; i < full_dim; i++) {
+    printf("%"PRIi64"\n", x->box_lower_bounds[i]);
+  }
+  printf("box upper bounds:\n");
+  for (u8 i = 0; i < full_dim; i++) {
+    printf("%"PRIi64"\n", x->box_upper_bounds[i]);
+  }
+  printf("transformation matrix:\n");
+  print_Z(68, full_dim, x->new_dim, x->Z);
+  printf("z1:\n");
+  print_Z(0, full_dim, full_dim, x->Z1);
+  printf("z2_inv:\n");
+  print_Z(0, x->new_dim, x->new_dim, x->Z2_inv);
+}
+
 /** \} */