kopia lustrzana https://github.com/mobilinkd/m17-cxx-demod
Merge kalman filter clock recovery changes.
Rob Riggs
commit
5ff118718c
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -415,7 +415,7 @@
|
|||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.10.1"
|
||||
"version": "3.10.8"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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_; }
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
Plik diff jest za duży
Load Diff
|
|
@ -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;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
|
||||
#include <algorithm>
|
||||
#include <cstdlib>
|
||||
#include <cstdint>
|
||||
#include <cassert>
|
||||
#include <array>
|
||||
#include <bitset>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue