correlator: added functions for phase and frequency estimation

impl/src/correlator.c

@@ -1,6 +1,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <malloc.h>
+#include <math.h>
 
 #include "correlator.h"
 
@@ -111,3 +112,58 @@ const float complex* correlator_get_conj_search_sequence(correlator_ctx_t *ctx)
 {
 	return ctx->search_sequence;
 }
+
+
+float correlator_get_mean_phase_deviation(correlator_ctx_t *ctx)
+{
+	float complex mean_unrotated_symbol = 0;
+
+	size_t n = (ctx->history_ptr - ctx->search_sequence_len + 1) & ctx->buffer_size_mask;
+
+	for(size_t m = 0; m < ctx->search_sequence_len; m++) {
+		size_t nm = (n + m) & ctx->buffer_size_mask;
+		mean_unrotated_symbol += ctx->search_sequence[m] * ctx->input_history[nm];
+	}
+
+	// no need to divide by ctx->search_sequence_len because division does not change the angle
+	return cargf(mean_unrotated_symbol);
+}
+
+
+/* Louise & Regiannini frequency estimator */
+float correlator_get_mean_frequency_deviation(correlator_ctx_t *ctx)
+{
+	float complex z[ctx->search_sequence_len];
+
+	size_t n = (ctx->history_ptr - ctx->search_sequence_len + 1) & ctx->buffer_size_mask;
+
+	// remove the influence of the data from the received symbols
+	for(size_t m = 0; m < ctx->search_sequence_len; m++) {
+		size_t nm = (n + m) & ctx->buffer_size_mask;
+		z[m] = ctx->search_sequence[m] * ctx->input_history[nm];
+	}
+
+
+	// Calculate averaged phase increments for <N> sub-sequences
+	size_t N = ctx->search_sequence_len/2;
+	float complex R_kappa[N];
+
+	for(size_t kappa = 0; kappa < N; kappa++) {
+		for(size_t k = 0; k < ctx->search_sequence_len - kappa; k++) {
+			R_kappa[kappa] += z[k + kappa] * conj(z[k]);
+		}
+
+		R_kappa[kappa] /= ctx->search_sequence_len - kappa;
+	}
+
+	// Calculate phase estimate (in radians/sample)
+	float complex sum_R_kappa = 0;
+
+	for(size_t kappa = 0; kappa < N; kappa++) {
+		sum_R_kappa += R_kappa[kappa];
+	}
+
+	float arg = cargf(sum_R_kappa);
+
+	return arg / ((float)M_PI * (1 + N));
+}

impl/src/correlator.h

@@ -112,5 +112,32 @@ void correlator_get_input_history(correlator_ctx_t *ctx, float complex *history)
  */
 const float complex* correlator_get_conj_search_sequence(correlator_ctx_t *ctx);
 
+/*!
+ * \brief Calculate mean phase deviation between current input and search sequence.
+ *
+ * \details
+ * Calling this probably only makes sense directly after a correlation peak was
+ * found.
+ *
+ * \param ctx          The correlator context to use.
+ * \returns            The mean phase deviation in radians.
+ */
+float correlator_get_mean_phase_deviation(correlator_ctx_t *ctx);
+
+/*!
+ * \brief Calculate mean frequency deviation between current input and search sequence.
+ *
+ * \details
+ * Calling this probably only makes sense directly after a correlation peak was
+ * found.
+ *
+ * This uses the Louise & Regiannini frequency estimation algorithm for known
+ * symbol sequences.
+ *
+ * \param ctx          The correlator context to use.
+ * \returns            The mean frequency deviation in radians/symbol.
+ */
+float correlator_get_mean_frequency_deviation(correlator_ctx_t *ctx);
+
 
 #endif // CORRELATOR_H