freq_est: implement L&R-based symbol-independent frequency estimation

The RX chain uses this to acquire an initial estimate of the carrier frequency. The estimate is adjusted on every incoming symbol until a preamble is found.
This commit is contained in:
Thomas Kolb 2023-05-14 15:53:14 +02:00
parent 3b7628882c
commit e42602633e
2 changed files with 110 additions and 29 deletions

View file

@ -2,6 +2,8 @@
#include "freq_est.h"
#include "utils.h"
// 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)
@ -25,28 +27,104 @@ static void linear_regression(size_t nx, const float *y, float *m, float *t)
*t = mean_y - (*m) * mean_x;
}
#if 0
// 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 = arg / (N+1) / 2.0f; /* FIXME: not sure where the factor 2 comes from. */
return freq_offset;
}
float freq_est_unknown_bpsk(const float complex *recv, size_t n, float *final_phase)
{
float phases[n];
float complex recv_sq[n];
// square the symbols and calculate phase
for(size_t i = 0; i < n; i++) {
float complex s = recv[i];
phases[i] = cargf(s * s);
recv_sq[i] = s * s;
}
liquid_unwrap_phase(phases, n);
float freq, phi0;
linear_regression(n, phases, &freq, &phi0);
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) / 2;
*final_phase = (phi0 + n * freq_est) / 2;
}
return freq/2;
return freq_est/2;
}

View file

@ -48,7 +48,7 @@ static bool acquire_preamble(layer1_rx_t *rx, const float complex sample)
// preamble search
float complex corr_out = correlator_step(&rx->preamble_correlator, sample);
if(cabsf(corr_out) > 0.9f * preamble_get_symbol_count()) {
if(cabsf(corr_out) > 0.5f * preamble_get_symbol_count()) {
// Preamble found!
DEBUG_LOG("Preamble found: %.3f > %.3f\n", cabsf(corr_out), 0.5f * preamble_get_symbol_count());
@ -80,33 +80,36 @@ static bool acquire_preamble(layer1_rx_t *rx, const float complex sample)
} else {
// preamble not found.
// Run the frequency estimator in blocks of FREQ_EST_L samples.
// This is an implementation that works with unknown BPSK symbols
// and therefore can be used during ramp-up and preamble.
if(freq_est_history_write_idx == FREQ_EST_L) {
float freq_est = freq_est_unknown_bpsk(freq_est_history, FREQ_EST_L, NULL);
// Run the frequency estimator for every incoming sample overy the last
// FREQ_EST_L samples. This is an implementation that works with unknown
// BPSK symbols and therefore can be used during ramp-up and preamble.
float freq_adjustment = (freq_est / RRC_SPS) * 0.3f;
nco_crcf_adjust_frequency(rx->carrier_coarse_nco, freq_adjustment);
float actual_freq = nco_crcf_get_frequency(rx->carrier_coarse_nco);
if(actual_freq > MAX_COARSE_FREQ_OFFSET) {
fprintf(stderr, ">");
nco_crcf_set_frequency(rx->carrier_coarse_nco, MAX_COARSE_FREQ_OFFSET);
} else if(actual_freq < -MAX_COARSE_FREQ_OFFSET) {
fprintf(stderr, "<");
nco_crcf_set_frequency(rx->carrier_coarse_nco, -MAX_COARSE_FREQ_OFFSET);
}
DEBUG_LOG("Frequency adjustment: %.6f - carrier frequency: %.6f\n", freq_adjustment, nco_crcf_get_frequency(rx->carrier_coarse_nco));
freq_est_history_write_idx = 0;
float complex linearized_history[FREQ_EST_L];
for(size_t i = 0; i < FREQ_EST_L; i++) {
linearized_history[i] = freq_est_history[(i + freq_est_history_write_idx) % FREQ_EST_L];
}
float freq_est = freq_est_unknown_bpsk(linearized_history, FREQ_EST_L, NULL);
float freq_adjustment = freq_est / 32.0f;
nco_crcf_adjust_frequency(rx->carrier_coarse_nco, freq_adjustment);
float actual_freq = nco_crcf_get_frequency(rx->carrier_coarse_nco);
if(actual_freq > MAX_COARSE_FREQ_OFFSET) {
//fprintf(stderr, ">");
nco_crcf_set_frequency(rx->carrier_coarse_nco, MAX_COARSE_FREQ_OFFSET);
} else if(actual_freq < -MAX_COARSE_FREQ_OFFSET) {
//fprintf(stderr, "<");
nco_crcf_set_frequency(rx->carrier_coarse_nco, -MAX_COARSE_FREQ_OFFSET);
}
DEBUG_LOG("Freq. est: %0.6f - Adj: %.6f - carrier frequency: %.6f\n", freq_est, freq_adjustment, nco_crcf_get_frequency(rx->carrier_coarse_nco));
assert(freq_est_history_write_idx < FREQ_EST_L);
freq_est_history[freq_est_history_write_idx] = sample;
freq_est_history_write_idx++;
freq_est_history_write_idx %= FREQ_EST_L;
return false; // preamble not found
}