Merge kalman filter clock recovery changes.

master
Rob Riggs 2024-07-31 15:58:34 -07:00
commit 5ff118718c
16 zmienionych plików z 1006 dodań i 919 usunięć

Wyświetl plik

@ -30,6 +30,26 @@ find_package(codec2 REQUIRED)
set(Boost_USE_STATIC_LIBS FALSE)
find_package(Boost COMPONENTS program_options REQUIRED)
if (BLAZE_INCLUDE_DIR)
# in cache already
set(BLAZE_FOUND TRUE)
message(STATUS "Have blaze")
else (BLAZE_INCLUDE_DIR)
find_path(BLAZE_INCLUDE_DIR NAMES blaze/Blaze.h
PATHS
${INCLUDE_INSTALL_DIR}
)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(BLAZE DEFAULT_MSG BLAZE_INCLUDE_DIR)
mark_as_advanced(BLAZE_INCLUDE_DIR)
message(STATUS "Found blaze")
endif(BLAZE_INCLUDE_DIR)
# Add subdirectories
add_subdirectory(src)
add_subdirectory(apps)

Wyświetl plik

@ -20,6 +20,8 @@ stream from STDIN and writes out an M17 4-FSK baseband stream at 48k SPS,
This code requires the codec2-devel, boost-devel and gtest-devel packages be installed.
This code also now requires the Blaze C++ math library.
It also requires a modern C++17 compiler (GCC 8 minimum).
### Build Steps

Wyświetl plik

@ -316,7 +316,7 @@ bool handle_frame(mobilinkd::M17FrameDecoder::output_buffer_t const& frame, int
result = dump_lsf(frame.lsf);
break;
case FrameType::LICH:
std::cerr << "LICH" << std::endl;
std::cerr << "\nLICH" << std::endl;
break;
case FrameType::STREAM:
result = demodulate_audio(frame.stream, viterbi_cost);
@ -341,20 +341,20 @@ void diagnostic_callback(bool dcd, FloatType evm, FloatType deviation, FloatType
{
if (debug) {
std::cerr << "\rdcd: " << std::setw(1) << int(dcd)
<< ", evm: " << std::setfill(' ') << std::setprecision(4) << std::setw(8) << evm * 100 <<"%"
<< ", deviation: " << std::setprecision(4) << std::setw(8) << deviation
<< ", freq offset: " << std::setprecision(4) << std::setw(8) << offset
<< ", locked: " << std::boolalpha << std::setw(6) << locked << std::dec
<< ", clock: " << std::setprecision(7) << std::setw(8) << clock
<< ", sample: " << std::setw(1) << sample_index << ", " << sync_index << ", " << clock_index
<< ", cost: " << viterbi_cost;
<< ", evm: " << std::setfill(' ') << std::setprecision(2) << std::setw(6) << evm * 100 <<"%"
<< ", deviation: " << std::setw(5) << int(deviation)
<< "Hz, freq offset: " << std::setfill(' ') << std::setw(5) << int(offset * 800)
<< "Hz, locked: " << std::boolalpha << std::setw(5) << locked << std::dec
<< ", clock: " << std::setprecision(2) << std::fixed << std::setw(8) << (clock * 1'000'000.0)
<< "ppm, sample: " << std::setw(1) << sample_index << ", " << sync_index << ", " << clock_index
<< ", cost: " << std::setfill(' ') << std::setw(3) << viterbi_cost;
}
if (!dcd && prbs.sync()) { // Seems like there should be a better way to do this.
if (!dcd && (prbs.bits() > 0)) { // Seems like there should be a better way to do this.
prbs.reset();
}
if (prbs.sync() && !quiet) {
if ((prbs.bits() > 0) && !quiet) {
if (!debug) {
std::cerr << '\r';
} else {
@ -486,7 +486,7 @@ int main(int argc, char* argv[])
int16_t sample;
std::cin.read(reinterpret_cast<char*>(&sample), 2);
if (invert_input) sample *= -1;
demod(sample / 44000.0);
demod(sample / 41067.0);
}
std::cerr << std::endl;

Wyświetl plik

@ -415,7 +415,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.1"
"version": "3.10.8"
}
},
"nbformat": 4,

Wyświetl plik

@ -1,7 +1,9 @@
// Copyright 2021 Mobilinkd LLC.
// Copyright 2022 Mobilinkd LLC.
#pragma once
#include "KalmanFilter.h"
#include <array>
#include <cstddef>
#include <cstdint>
@ -11,196 +13,94 @@
namespace mobilinkd
{
/**
* Calculate the phase estimates for each sample position.
*
* This performs a running calculation of the phase of each bit position.
* It is very noisy for individual samples, but quite accurate when
* averaged over an entire M17 frame.
*
* It is designed to be used to calculate the best bit position for each
* frame of data. Samples are collected and averaged. When update() is
* called, the best sample index and clock are estimated, and the counters
* reset for the next frame.
*
* It starts counting bit 0 as the first bit received after a reset.
*
* This is very efficient as it only uses addition and subtraction for
* each bit sample. And uses one multiply and divide per update (per
* frame).
*
* This will permit a clock error of up to 500ppm. This allows up to
* 250ppm error for both transmitter and receiver clocks. This is
* less than one sample per frame when the sample rate is 48000 SPS.
*
* @inv current_index_ is in the interval [0, SAMPLES_PER_SYMBOL).
* @inv sample_index_ is in the interval [0, SAMPLES_PER_SYMBOL).
* @inv clock_ is in the interval [0.9995, 1.0005]
*/
template <typename FloatType, size_t SampleRate, size_t SymbolRate>
class ClockRecovery
template <typename FloatType, size_t SamplesPerSymbol>
struct ClockRecovery
{
static constexpr size_t SAMPLES_PER_SYMBOL = SampleRate / SymbolRate;
static constexpr int8_t MAX_OFFSET = SAMPLES_PER_SYMBOL / 2;
static constexpr FloatType dx = 1.0 / SAMPLES_PER_SYMBOL;
static constexpr FloatType MAX_CLOCK = 1.0005;
static constexpr FloatType MIN_CLOCK = 0.9995;
std::array<FloatType, SAMPLES_PER_SYMBOL> estimates_;
size_t sample_count_ = 0;
uint16_t frame_count_ = 0;
uint8_t sample_index_ = 0;
uint8_t prev_sample_index_ = 0;
uint8_t index_ = 0;
FloatType offset_ = 0.0;
FloatType clock_ = 1.0;
FloatType prev_sample_ = 0.0;
m17::KalmanFilter<FloatType, SamplesPerSymbol> kf_;
size_t count_ = 0;
int8_t sample_index_ = 0;
FloatType clock_estimate_ = 0.;
FloatType sample_estimate_ = 0.;
/**
* Find the sample index.
* Reset the clock recovery after the first sync word is received.
* This is used as the starting state of the Kalman filter. Providing
* the filter with a realistic starting point causes it to converge
* must faster.
*
* @param[in] index starting sample index.
*/
void reset(FloatType index)
{
kf_.reset(index);
count_ = 0;
sample_index_ = index;
clock_estimate_ = 0.;
}
/// Count each sample.
void operator()(FloatType)
{
++count_;
}
/**
* Update the filter with the estimated index from the sync word. The
* result is a new estimate of the state, the remote symbol clock offset
* relative to our clock. It can be faster (>0.0) or slower (<0.0).
*
* @param[in] index is the new symbol sample position from the sync word.
*/
bool update(uint8_t index)
{
auto f = kf_.update(index, count_);
// Constrain sample index to [0..SamplesPerSymbol), wrapping if needed.
sample_estimate_ = f[0];
sample_index_ = int8_t(round(sample_estimate_));
sample_index_ = sample_index_ < 0 ? sample_index_ + SamplesPerSymbol : sample_index_;
sample_index_ = sample_index_ >= int8_t(SamplesPerSymbol) ? sample_index_ - SamplesPerSymbol : sample_index_;
clock_estimate_ = f[1];
count_ = 0;
return true;
}
/**
* This is used when no sync word is found. The sample index is updated
* based on the current clock estimate, the last known good sample
* estimate, and the number of samples processed.
*
* There are @p SAMPLES_PER_INDEX bins. It is expected that half are
* positive values and half are negative. The positive and negative
* bins will be grouped together such that there is a single transition
* from positive values to negative values.
*
* The best bit position is always the position with the positive value
* at that transition point. It will be the bit index with the highest
* energy.
*
* @post sample_index_ contains the best sample point.
* The sample and clock estimates from the filter remain unchanged.
*/
void update_sample_index_()
bool update()
{
uint8_t index = 0;
auto csw = std::fmod((sample_estimate_ + clock_estimate_ * count_), SamplesPerSymbol);
if (csw < 0.) csw += SamplesPerSymbol;
else if (csw >= SamplesPerSymbol) csw -= SamplesPerSymbol;
// Find falling edge.
bool is_positive = false;
for (size_t i = 0; i != SAMPLES_PER_SYMBOL; ++i)
{
FloatType phase = estimates_[i];
// Constrain sample index to [0..SamplesPerSymbol), wrapping if needed.
sample_index_ = int8_t(round(csw));
sample_index_ = sample_index_ < 0 ? sample_index_ + SamplesPerSymbol : sample_index_;
sample_index_ = sample_index_ >= int8_t(SamplesPerSymbol) ? sample_index_ - SamplesPerSymbol : sample_index_;
if (!is_positive && phase > 0)
{
is_positive = true;
}
else if (is_positive && phase < 0)
{
index = i;
break;
}
}
sample_index_ = index == 0 ? SAMPLES_PER_SYMBOL - 1 : index - 1;
}
/**
* Compute the drift in sample points from the last update.
*
* This should never be greater than one.
*/
FloatType calc_offset_()
{
int8_t offset = sample_index_ - prev_sample_index_;
// When in spec, the clock should drift by less than 1 sample per frame.
if (offset >= MAX_OFFSET) [[unlikely]]
{
offset -= SAMPLES_PER_SYMBOL;
}
else if (offset <= -MAX_OFFSET) [[unlikely]]
{
offset += SAMPLES_PER_SYMBOL;
}
return offset;
}
void update_clock_()
{
// update_sample_index_() must be called first.
if (frame_count_ == 0) [[unlikely]]
{
prev_sample_index_ = sample_index_;
offset_ = 0.0;
clock_ = 1.0;
return;
}
offset_ += calc_offset_();
prev_sample_index_ = sample_index_;
clock_ = 1.0 + (offset_ / (frame_count_ * sample_count_));
clock_ = std::min(MAX_CLOCK, std::max(MIN_CLOCK, clock_));
}
public:
ClockRecovery()
{
estimates_.fill(0);
}
/**
* Update clock recovery with the given sample. This will advance the
* current sample index by 1.
*/
void operator()(FloatType sample)
{
FloatType dy = (sample - prev_sample_);
if (sample + prev_sample_ < 0)
{
// Invert the phase estimate when sample midpoint is less than 0.
dy = -dy;
}
prev_sample_ = sample;
estimates_[index_] += dy;
index_ += 1;
if (index_ == SAMPLES_PER_SYMBOL)
{
index_ = 0;
}
sample_count_ += 1;
}
/**
* Reset the state of the clock recovery system. This should be called
* when a new transmission is detected.
*/
void reset()
{
sample_count_ = 0;
frame_count_ = 0;
index_ = 0;
sample_index_ = 0;
estimates_.fill(0);
}
/**
* Return the current sample index. This will always be in the range of
* [0..SAMPLES_PER_SYMBOL).
*/
uint8_t current_index() const
{
return index_;
return true;
}
/**
* Return the estimated sample clock increment based on the last update.
*
*
* The value is only valid after samples have been collected and update()
* has been called.
*/
FloatType clock_estimate() const
{
return clock_;
return clock_estimate_;
}
/**
* Return the estimated "best sample index" based on the last update.
*
*
* The value is only valid after samples have been collected and update()
* has been called.
*/
@ -208,35 +108,7 @@ public:
{
return sample_index_;
}
/**
* Update the sample index and clock estimates, and reset the state for
* the next frame of data.
*
* @pre index_ = 0
* @pre sample_count_ > 0
*
* After this is called, sample_index() and clock_estimate() will have
* valid, updated results.
*
* The more samples between calls to update, the more accurate the
* estimates will be.
*
* @return true if the preconditions are met and the update has been
* performed, otherwise false.
*/
bool update()
{
if (!(sample_count_ != 0 && index_ == 0)) return false;
update_sample_index_();
update_clock_();
frame_count_ = std::min(0x1000, 1 + frame_count_);
sample_count_ = 0;
estimates_.fill(0);
return true;
}
};
} // mobilinkd

Wyświetl plik

@ -85,16 +85,32 @@ struct Correlator
size_t min_count = 0;
size_t max_count = 0;
size_t index = 0;
FloatType min_level = buffer_[sample_index];
FloatType max_level = buffer_[sample_index];
for (size_t i = sample_index; i < buffer_.size(); i += SAMPLES_PER_SYMBOL)
{
min_level = std::min(min_level, buffer_[i]);
max_level = std::max(max_level, buffer_[i]);
}
FloatType avg = max_level + min_level / 2.;
for (size_t i = sample_index; i < buffer_.size(); i += SAMPLES_PER_SYMBOL)
{
tmp[index++] = buffer_[i] * 1000.;
max_sum += buffer_[i] * ((buffer_[i] > 0.));
min_sum += buffer_[i] * ((buffer_[i] < 0.));
max_count += (buffer_[i] > 0.);
min_count += (buffer_[i] < 0.);
bool high = buffer_[i] > avg;
bool low = buffer_[i] < avg;
max_sum += buffer_[i] * high;
min_sum += buffer_[i] * low;
max_count += high;
min_count += low;
}
FloatType mn = min_count > 0 ? min_sum / min_count : min_level;
FloatType mx = max_count > 0 ? max_sum / max_count : max_level;
return std::make_tuple(min_sum / min_count, max_sum / max_count);
return std::make_tuple(mn, mx);
}
@ -140,12 +156,30 @@ struct SyncWord
return (value > limit_1 || value < limit_2) ? value : 0.0;
}
bool is_triggered() const { return triggered_; }
void find_peak(value_type value)
{
triggered_ = false;
timing_index_ = 0;
auto peak_value = value;
uint8_t index = 0;
for (auto f : samples_)
{
if (abs(f) > abs(peak_value))
{
peak_value = f;
timing_index_ = index;
}
index += 1;
}
updated_ = peak_value > 0 ? 1 : -1;
}
size_t operator()(Correlator& correlator)
{
auto value = triggered(correlator);
value_type peak_value = 0;
if (value != 0)
{
if (!triggered_)
@ -159,21 +193,7 @@ struct SyncWord
{
if (triggered_)
{
// Calculate the timing index on the falling edge.
triggered_ = false;
timing_index_ = 0;
peak_value = value;
uint8_t index = 0;
for (auto f : samples_)
{
if (abs(f) > abs(peak_value))
{
peak_value = f;
timing_index_ = index;
}
index += 1;
}
updated_ = peak_value > 0 ? 1 : -1;
find_peak(value);
}
}
return timing_index_;

Wyświetl plik

@ -68,7 +68,7 @@ struct DataCarrierDetect
triggered_ = triggered_ ? level_ > ltrigger_ : level_ > htrigger_;
}
void unlock() { triggered_ = false; }
FloatType level() const { return level_; }
bool dcd() const { return triggered_; }
};

Wyświetl plik

@ -3,127 +3,54 @@
#pragma once
#include "IirFilter.h"
#include "KalmanFilter.h"
#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
namespace mobilinkd {
/**
* Deviation and zero-offset estimator.
*
* Accepts samples which are periodically used to update estimates of the
* input signal deviation and zero offset.
*
* Samples must be provided at the ideal sample point (the point with the
* peak bit energy).
*
* Estimates are expected to be updated at each sync word. But they can
* be updated more frequently, such as during the preamble.
*/
template <typename FloatType>
class FreqDevEstimator
{
using sample_filter_t = BaseIirFilter<FloatType, 3>;
static constexpr FloatType DEVIATION = 2400.;
// IIR with Nyquist of 1/4.
static constexpr std::array<FloatType, 3> dc_b = { 0.09763107, 0.19526215, 0.09763107 };
static constexpr std::array<FloatType, 3> dc_a = { 1. , -0.94280904, 0.33333333 };
static constexpr FloatType MAX_DC_ERROR = 0.2;
FloatType min_est_ = 0.0;
FloatType max_est_ = 0.0;
FloatType min_cutoff_ = 0.0;
FloatType max_cutoff_ = 0.0;
FloatType min_var_ = 0.0;
FloatType max_var_ = 0.0;
size_t min_count_ = 0;
size_t max_count_ = 0;
FloatType deviation_ = 0.0;
FloatType offset_ = 0.0;
FloatType error_ = 0.0;
FloatType idev_ = 1.0;
sample_filter_t dc_filter_{dc_b, dc_a};
m17::SymbolKalmanFilter<FloatType> minFilter_;
m17::SymbolKalmanFilter<FloatType> maxFilter_;
FloatType idev_ = 0.;
FloatType offset_ = 0.;
bool reset_ = true;
public:
void reset()
{
min_est_ = 0.0;
max_est_ = 0.0;
min_var_ = 0.0;
max_var_ = 0.0;
min_count_ = 0;
max_count_ = 0;
min_cutoff_ = 0.0;
max_cutoff_ = 0.0;
}
void reset()
{
reset_ = true;
}
void sample(FloatType sample)
{
if (sample < 1.5 * min_est_)
{
min_count_ = 1;
min_est_ = sample;
min_var_ = 0.0;
min_cutoff_ = min_est_ * 0.666666;
}
else if (sample < min_cutoff_)
{
min_count_ += 1;
min_est_ += sample;
FloatType var = (min_est_ / min_count_) - sample;
min_var_ += var * var;
}
else if (sample > 1.5 * max_est_)
{
max_count_ = 1;
max_est_ = sample;
max_var_ = 0.0;
max_cutoff_ = max_est_ * 0.666666;
}
else if (sample > max_cutoff_)
{
max_count_ += 1;
max_est_ += sample;
FloatType var = (max_est_ / max_count_) - sample;
max_var_ += var * var;
}
}
void update(FloatType minValue, FloatType maxValue)
{
auto mnf = minFilter_.update(minValue, 192);
auto mxf = maxFilter_.update(maxValue, 192);
offset_ = (mxf[0] + mnf[0]) / 2.;
idev_ = 6.0 / (mxf[0] - mnf[0]);
/**
* Update the estimates for deviation, offset, and EVM (error). Note
* that the estimates for error are using a sloppy implementation for
* calculating variance to reduce the memory requirements. This is
* because this is designed for embedded use.
*/
void update()
{
if (max_count_ < 2 || min_count_ < 2) return;
FloatType max_ = max_est_ / max_count_;
FloatType min_ = min_est_ / min_count_;
deviation_ = (max_ - min_) / 6.0;
offset_ = dc_filter_(std::max(std::min(max_ + min_, deviation_ * MAX_DC_ERROR), deviation_ * -MAX_DC_ERROR));
error_ = (std::sqrt(max_var_ / (max_count_ - 1)) + std::sqrt(min_var_ / (min_count_ - 1))) * 0.5;
if (deviation_ > 0) idev_ = 1.0 / deviation_;
min_cutoff_ = offset_ - deviation_ * 2;
max_cutoff_ = offset_ + deviation_ * 2;
max_est_ = max_;
min_est_ = min_;
max_count_ = 1;
min_count_ = 1;
max_var_ = 0.0;
min_var_ = 0.0;
}
if (isnan(mnf) || isnan(mxf)) reset_ = true;
FloatType deviation() const { return deviation_; }
FloatType offset() const { return offset_; }
FloatType error() const { return error_; }
FloatType idev() const { return idev_; }
if (reset_)
{
reset_ = false;
minFilter_.reset(minValue);
maxFilter_.reset(maxValue);
offset_ = (minValue + maxValue) / 2;
idev_ = 6.0 / (maxValue - minValue);
}
}
FloatType idev() const { return idev_; }
FloatType offset() const { return offset_; }
FloatType deviation() const { return DEVIATION / idev_; }
FloatType error() const { return 0.; }
};
} // mobilinkd

Wyświetl plik

@ -0,0 +1,111 @@
// Copyright 2022 Mobilinkd LLC.
#pragma once
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
#include <blaze/math/Matrix.h>
#include <blaze/math/Vector.h>
#include <blaze/Math.h>
#pragma GCC diagnostic pop
#include <cmath>
namespace mobilinkd { namespace m17 {
template <typename FloatType, size_t SamplesPerSymbol>
struct KalmanFilter
{
blaze::StaticVector<FloatType, 2> x;
blaze::StaticMatrix<FloatType, 2, 2> P;
blaze::StaticMatrix<FloatType, 2, 2> F;
blaze::StaticMatrix<FloatType, 1, 2> H = {{1., 0.}};
blaze::StaticMatrix<FloatType, 1, 1> R = {{0.5}};
blaze::StaticMatrix<FloatType, 2, 2> Q = {{6.25e-13, 1.25e-12},{1.25e-12, 2.50e-12}};
KalmanFilter()
{
reset(0.);
}
void reset(FloatType z)
{
x = {z, 0.};
P = {{4., 0.}, {0., 0.00000025}};
F = {{1., 1.}, {0., 1.}};
}
[[gnu::noinline]]
auto update(FloatType z, size_t dt)
{
F(0,1) = FloatType(dt);
x = F * x;
P = F * P * blaze::trans(F) + Q;
auto S = H * P * blaze::trans(H) + R;
auto K = P * blaze::trans(H) * (1.0 / S(0, 0));
// Normalize incoming index
if (z - x[0] < (SamplesPerSymbol / -2.0))
z += SamplesPerSymbol;
else if (z - x[0] > (SamplesPerSymbol / 2.0))
z-= SamplesPerSymbol;
auto y = z - H * x;
x += K * y;
// Normalize the filtered sample point
while (x[0] >= SamplesPerSymbol) x[0] -= SamplesPerSymbol;
while (x[0] < 0) x[0] += SamplesPerSymbol;
P = P - K * H * P;
return x;
}
};
template <typename FloatType>
struct SymbolKalmanFilter
{
blaze::StaticVector<FloatType, 2> x;
blaze::StaticMatrix<FloatType, 2, 2> P;
blaze::StaticMatrix<FloatType, 2, 2> F;
blaze::StaticMatrix<FloatType, 1, 2> H = {{1., 0.}};
blaze::StaticMatrix<FloatType, 1, 1> R = {{0.5}};
blaze::StaticMatrix<FloatType, 2, 2> Q = {{6.25e-13, 1.25e-12},{1.25e-12, 2.50e-12}};
SymbolKalmanFilter()
{
reset(0.);
}
void reset(FloatType z)
{
x = {z, 0.};
P = {{4., 0.}, {0., 0.00000025}};
F = {{1., 1.}, {0., 1.}};
}
[[gnu::noinline]]
auto update(FloatType z, size_t dt)
{
F(0,1) = FloatType(dt);
x = F * x;
P = F * P * blaze::trans(F) + Q;
auto S = H * P * blaze::trans(H) + R;
auto K = P * blaze::trans(H) * (1.0 / S(0, 0));
auto y = z - H * x;
x += K * y;
// Normalize the filtered sample point
P = P - K * H * P;
return x;
}
};
}} // mobilinkd::m17

Wyświetl plik

@ -246,7 +246,7 @@ struct M17FrameDecoder
if (checksum == 0)
{
lich_segments = 0;
lich_segments = 0;
state_ = State::STREAM;
viterbi_cost = 0;
output_buffer.type = FrameType::LSF;
@ -282,16 +282,10 @@ struct M17FrameDecoder
viterbi_cost = viterbi_.decode(depuncture_buffer.stream, decode_buffer.stream);
to_byte_array(decode_buffer.stream, output_buffer.stream);
if ((viterbi_cost < 60) && (output_buffer.stream[0] & 0x80))
{
// fputs("\nEOS\n", stderr);
state_ = State::LSF;
}
output_buffer.type = FrameType::STREAM;
callback_(output_buffer, viterbi_cost);
return state_ == State::LSF ? DecodeResult::EOS : DecodeResult::OK;
return DecodeResult::OK;
}
/**

Wyświetl plik

@ -0,0 +1,85 @@
// Copyright 2020-2022 Mobilinkd LLC <rob@mobilinkd.com>
// All rights reserved.
#pragma once
#include <cmath>
#include <cstdint>
namespace mobilinkd {
/**
* Compute a running standard deviation. Avoids having to store
* all of the samples.
*
* Based on https://dsp.stackexchange.com/a/1187/36581
*/
template <typename FloatType>
struct StandardDeviation
{
FloatType mean{0.0};
FloatType S{0.0};
size_t samples{0};
void reset()
{
mean = 0.0;
S = 0.0;
samples = 0;
}
void capture(float sample)
{
auto prev = mean;
samples += 1;
mean = mean + (sample - mean) / samples;
S = S + (sample - mean) * (sample - prev);
}
FloatType variance() const
{
return samples == 0 ? -1.0 : S / samples;
}
FloatType stdev() const
{
FloatType result = -1.0;
if (samples) result = std::sqrt(variance());
return result;
}
// SNR in dB
FloatType SNR() const
{
return 10.0 * std::log10(mean / stdev());
}
};
template <typename FloatType, size_t N>
struct RunningStandardDeviation
{
FloatType S{1.0};
FloatType alpha{1.0 / N};
void reset()
{
S = 0.0;
}
void capture(float sample)
{
S -= S * alpha;
S += (sample * sample) * alpha;
}
FloatType variance() const
{
return S;
}
FloatType stdev() const
{
return std::sqrt(variance());
}
};
} // mobilinkd

Wyświetl plik

@ -1,83 +1,54 @@
// Copyright 2020 Mobilinkd LLC.
// Copyright 2020-2022 Mobilinkd LLC.
#pragma once
#include "IirFilter.h"
#include <array>
#include <algorithm>
#include <numeric>
#include <optional>
#include <tuple>
#include "StandardDeviation.h"
namespace mobilinkd
{
template <typename FloatType, size_t N>
/**
* Compute the EVM of a symbol. This assumes that the incoming sample has
* been normalized, meaning its offset and scale has been adjusted so that
* nominal values fall exactly on the symbol values of -3, -1, 1, 3. It
* determines the nearest symbol value and computes the variance.
*
* This uses a running standard deviation with a nominal length of 184
* symbols. That is the payload size of an M17 frame.
*/
template <typename FloatType>
struct SymbolEvm
{
using filter_type = BaseIirFilter<FloatType, N>;
using symbol_t = int;
using result_type = std::tuple<symbol_t, FloatType>;
filter_type filter_;
std::optional<FloatType> erasure_limit_;
FloatType evm_ = 0.0;
SymbolEvm(filter_type&& filter, std::optional<FloatType> erasure_limit = std::nullopt)
: filter_(std::forward<filter_type>(filter))
, erasure_limit_(erasure_limit)
{}
FloatType evm() const { return evm_; }
/**
* Decode a normalized sample into a symbol. Symbols
* are decoded into +3, +1, -1, -3. If an erasure limit
* is set, symbols outside this limit are 'erased' and
* returned as 0.
*/
result_type operator()(FloatType sample)
RunningStandardDeviation<FloatType, 184> stddev;
void reset()
{
symbol_t symbol;
FloatType evm;
stddev.reset();
}
sample = std::min(3.0, std::max(-3.0, sample));
FloatType evm() const { return stddev.stdev(); }
void update(FloatType sample)
{
FloatType evm;
if (sample > 2)
{
symbol = 3;
evm = (sample - 3) * 0.333333;
stddev.capture(sample - 3);
}
else if (sample > 0)
{
symbol = 1;
evm = sample - 1;
stddev.capture(sample - 1);
}
else if (sample >= -2)
else if (sample > -2)
{
symbol = -1;
evm = sample + 1;
stddev.capture(sample + 1);
}
else
{
symbol = -3;
evm = (sample + 3) * 0.333333;
stddev.capture(sample + 3);
}
if (erasure_limit_ and (abs(evm) > *erasure_limit_)) symbol = 0;
evm_ = filter_(evm);
return std::make_tuple(symbol, evm);
}
};
template <typename FloatType, size_t N>
SymbolEvm<FloatType, N> makeSymbolEvm(
BaseIirFilter<FloatType, N>&& filter, std::optional<FloatType> erasure_limit = std::nullopt)
{
return std::move(SymbolEvm<FloatType, N>(std::move(filter), erasure_limit));
}
} // mobilinkd

Wyświetl plik

@ -4,6 +4,7 @@
#include <algorithm>
#include <cstdlib>
#include <cstdint>
#include <cassert>
#include <array>
#include <bitset>

Wyświetl plik

@ -21,9 +21,10 @@ class ClockRecoveryTest : public ::testing::Test {
TEST_F(ClockRecoveryTest, construct)
{
auto cr = mobilinkd::ClockRecovery<float, 48000, 4800>();
auto cr = mobilinkd::ClockRecovery<float, 10>();
}
# if 0
TEST_F(ClockRecoveryTest, recover_preamble)
{
// 2400Hz sine wave -- same as M17 preamble.
@ -113,3 +114,5 @@ TEST_F(ClockRecoveryTest, all_phases)
EXPECT_EQ(int(cr.sample_index()), expected[p]);
}
}
#endif

Wyświetl plik

@ -25,31 +25,11 @@ TEST_F(FreqDevEstimatorTest, construct)
TEST_F(FreqDevEstimatorTest, fde_preamble)
{
constexpr std::array<float, 24> input = {1,1,1,1,1,1,1,1,1,1,1,1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
auto fde = mobilinkd::FreqDevEstimator<float>();
std::for_each(input.begin(), input.end(), [&fde](float x){fde.sample(x * 3);});
std::for_each(input.begin(), input.end(), [&fde](float x){fde.sample(x * 3);});
std::for_each(input.begin(), input.end(), [&fde](float x){fde.sample(x * 3);});
fde.update(-3, 3);
fde.update(-3, 3);
fde.update(-3, 3);
fde.update();
EXPECT_NEAR(fde.deviation(), 1, .1);
EXPECT_NEAR(fde.error(), 0, .1);
}
TEST_F(FreqDevEstimatorTest, fde_mixed)
{
constexpr std::array<float, 16> input = {1,1,1,1,1,1,1,1,-1,-1,-1,-1,-1,-1,-1,-1};
auto fde = mobilinkd::FreqDevEstimator<float>();
std::for_each(input.begin(), input.end(), [&fde](float x){fde.sample(x * 3);});
std::for_each(input.begin(), input.end(), [&fde](float x){fde.sample(x);});
std::for_each(input.begin(), input.end(), [&fde](float x){fde.sample(x * 3);});
std::for_each(input.begin(), input.end(), [&fde](float x){fde.sample(x);});
fde.update();
EXPECT_NEAR(fde.deviation(), 1, .1);
EXPECT_NEAR(fde.deviation(), 2400, .1);
EXPECT_NEAR(fde.error(), 0, .1);
}