From 423f1d641645ee5c9c0dd7ca146d3e4c178a4e06 Mon Sep 17 00:00:00 2001
From: Thomas Kolb <cfr34k-git@tkolb.de>
Date: Thu, 3 Feb 2022 22:17:37 +0100
Subject: [PATCH] correlator: added functions for phase and frequency
 estimation

---
 impl/src/correlator.c | 56 +++++++++++++++++++++++++++++++++++++++++++
 impl/src/correlator.h | 27 +++++++++++++++++++++
 2 files changed, 83 insertions(+)

diff --git a/impl/src/correlator.c b/impl/src/correlator.c
index 3c33ae8..4c53d59 100644
--- a/impl/src/correlator.c
+++ b/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));
+}
diff --git a/impl/src/correlator.h b/impl/src/correlator.h
index 9e67a56..e62e63d 100644
--- a/impl/src/correlator.h
+++ b/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