hamnet70/impl/src/layer1/freq_est.c

#include <liquid/liquid.h>
#include <math.h>

#include "freq_est.h"

#include "utils.h"

#if 0
// Fits a line y' = m*x' + t to the given points (x, y), where x is the
// zero-based index in the array.
static void linear_regression(size_t nx, const float *y, float *m, float *t)
{
	float mean_x = (nx-1) / 2.0f;
	float mean_y = 0.0f;
	for(unsigned int j = 0; j < nx; j++) {
		mean_y += y[j];
	}
	mean_y /= nx;

	float numerator = 0.0f;
	float denominator = 0.0f;
	for(unsigned int j = 0; j < nx; j++) {
		float delta_index = j - mean_x;
		numerator += delta_index * (y[j] - mean_y);
		denominator += delta_index*delta_index;
	}

	*m = numerator / denominator;
	*t = mean_y - (*m) * mean_x;
}

// Calculates an estimation of the true peak frequency in the signal by
// calculating the FFT und interpolating around the bin with the highest value.
static float fft_true_peak(const float complex *recv, size_t n)
{
	float complex *out = fft_malloc(n * sizeof(float complex));

	fft_run(n, (float complex*)recv, out, LIQUID_FFT_FORWARD, 0);
	fft_shift(out, n); // shifts by n/2

	// search for the maximum-amplitude bin
	size_t peak_pos = 0;
	float peak_amplitude = 0.0f;

	float amp[n];
	for(size_t i = 0; i < n; i++) {
		amp[i] = cabsf(out[i]);
		if(amp[i] > peak_amplitude) {
			peak_amplitude = amp[i];
			peak_pos = i;
		}
	}

	dump_array_f(amp, n, 1.0f, "/tmp/fft_amp.f32");

	// interpolation does not work at the first and last bin
	if(peak_pos == 0 || peak_pos == (n-1)) {
		return (peak_pos - n/2) / n;
	}

	float a1 = amp[peak_pos - 1];
	float a2 = amp[peak_pos];
	float a3 = amp[peak_pos + 1];

	// find fractional offset to peak_pos by quadratic interpolation.
	// Source: https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/
	float d = (a3 - a1) / (2 * (2 * a2 - a1 - a3));

	fft_free(out);

	return ((float)peak_pos + d - (float)(n/2)) / n;
}
#endif


// Frequency estimation algorithm by Luise & Reggiannini
float freq_est_lr(const float complex *recv, size_t l0)
{
	const size_t N = l0/2;

	float complex R[N];

	// calculate R[kappa]
	for(size_t kappa = 1; kappa <= N; kappa++) {
		R[kappa-1] = 0.0f;

		for(size_t k = 0; k < l0 - kappa; k++) {
			R[kappa-1] += recv[k + kappa] * conjf(recv[k]);
		}

		R[kappa-1] /= l0 - kappa;
	}

	// sum over R[kappa]
	float complex R_total = 0.0f;
	for(size_t kappa = 1; kappa <= N; kappa++) {
		R_total += R[kappa-1];
	}

	// calculate phase angle of summed R
	float arg = cargf(R_total);

	// convert to frequency offset in radians per sample
	float freq_offset = 2.0f * arg / (N+1); /* FIXME: check where the factor 2.0 comes from… */

	return freq_offset;
}


float freq_est_unknown_bpsk(const float complex *recv, size_t n, float *final_phase)
{
	float complex recv_sq[n];

	// square the symbols and calculate phase
	for(size_t i = 0; i < n; i++) {
		float complex s = recv[i];
		recv_sq[i] = s * s;
	}

	float freq_est = freq_est_lr(recv_sq, n);
	float phi0 = cargf(recv[0]);

	// outputs must be divided by 2 because squaring the symbols doubles their phase.
	if(final_phase) {
		*final_phase = (phi0 + n * freq_est) / 2;
	}

	return freq_est/2;
}


float freq_est_in_rampup(const float complex *recv, size_t n, float *final_phase)
{
	float complex rotated[n];

	float prev_phase = 0.0f;

	// rotate every second symbol by 180°. As a plausibility check that we are
	// in fact inside the ramp-up, we verify that there is no phase rotation by
	// more than 90° between the symbols.
	for(size_t i = 0; i < n; i++) {
		if((i % 2) == 0) {
			rotated[i] = recv[i];
		} else {
			rotated[i] = -recv[i];
		}

		float phase = cargf(rotated[i]);
		if(i > 0) {
			float phase_delta = phase - prev_phase;
			if(fabsf(phase_delta) > 3.14159f/2.0f) {
				// abort because we either have the wrong signal or too much noise.
				if(final_phase) {
					*final_phase = 0.0f;
				}
				return 0.0f;
			}
		}
		prev_phase = phase;
	}

	// signal is clean (and all symbol information removed), so we run the estimator

	return freq_est_data_free(rotated, n, final_phase);
}


float freq_est_known_symbols(const float complex *recv, const float complex *ref, size_t n, float *final_phase)
{
	float complex symbols[n];

	// remove the data influence from the symbols
	for(size_t i = 0; i < n; i++) {
		symbols[i] = conjf(ref[i]) * recv[i];
	}

	return freq_est_data_free(symbols, n, final_phase);
}


float freq_est_data_free(const float complex *symbols, size_t n, float *final_phase)
{
	float phases[n];

	for(size_t i = 0; i < n; i++) {
		phases[i] = cargf(symbols[i]);
	}

	liquid_unwrap_phase(phases, n);

	float freq, phi0;
	freq = freq_est_lr(symbols, n);

	// calculate the mean of unwrapped symbol phases and from that the
	// de-noised phase at the end of the sequence.
	phi0 = 0.0f;
	for(size_t i = 0; i < n; i++) {
		phi0 += phases[i];
	}
	phi0 /= n;

	if(final_phase) {
		*final_phase = phi0 + (n+1)/2.0f * freq;
	}

	return freq;
}