diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index a0a944e..14ab2fc 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -35,7 +35,10 @@ list(APPEND digitalhf_sources vector_pll_cc_impl.cc vector_early_late_cc_impl.cc lms.cc + nlms.cc rls.cc + kalman_exp.cc + kalman_hsu.cc ) set(digitalhf_sources "${digitalhf_sources}" PARENT_SCOPE) diff --git a/lib/adaptive_dfe_impl.cc b/lib/adaptive_dfe_impl.cc index 09beff1..5f9a2e9 100644 --- a/lib/adaptive_dfe_impl.cc +++ b/lib/adaptive_dfe_impl.cc @@ -34,7 +34,10 @@ #include "adaptive_dfe_impl.h" #include "lms.hpp" +#include "nlms.hpp" #include "rls.hpp" +#include "kalman_exp.hpp" +#include "kalman_hsu.hpp" namespace gr { namespace digitalhf { @@ -228,9 +231,11 @@ bool adaptive_dfe_impl::start() reset_filter(); GR_LOG_DEBUG(d_logger,str(boost::format("adaptive_dfe_impl::start() nB=%d nF=%d mu=%f alpha=%f") % _nB % _nF % _mu % _alpha)); - _filter_update = lms::make(_mu); - //_filter_update = rls::make(0.001, 0.9999); + // _filter_update = nlms::make(0.5); + // _filter_update = rls::make(0.001f, 0.999f); // not stable + // _filter_update = kalman_exp::make(1.0f, 0.999f); // too slow + // _filter_update = kalman_hsu::make(0.008f, 0.1f); // not stable return true; } bool adaptive_dfe_impl::stop() @@ -338,7 +343,10 @@ adaptive_dfe_impl::recenter_filter_taps() { sum_w += w; sum_wi += w*i; } - ssize_t const idx_max = ssize_t(0.5 + sum_wi/sum_w); + // TODO + static float mp = _nB+1; + mp = 0.9*mp + 0.1*sum_wi/sum_w; + ssize_t const idx_max = ssize_t(0.5 + mp); // GR_LOG_DEBUG(d_logger, str(boost::format("idx_max=%2d abs(tap_max)=%f") % idx_max % std::abs(_taps_samples[idx_max]))); if (idx_max-_nB-1 > +2*_sps) { diff --git a/lib/kalman_exp.cc b/lib/kalman_exp.cc new file mode 100644 index 0000000..66a33fa --- /dev/null +++ b/lib/kalman_exp.cc @@ -0,0 +1,103 @@ +// -*- C++ -*- + +#include +#include "kalman_exp.hpp" +#include + +namespace gr { +namespace digitalhf { + +filter_update::sptr kalman_exp::make(float r, float lambda) { + return filter_update::sptr(new kalman_exp(r, lambda)); +} + +kalman_exp::kalman_exp(float r, float lambda) + : _r(r) + , _lambda(lambda) + , _gain() + , _p() + , _temp() + , _t1() { +} + +kalman_exp::~kalman_exp() { +} + +void kalman_exp::resize(size_t n) { + if (_gain.size() == n && + _p.size() == n*n && + _temp.size() == n*n && + _t1.size() == n*n) + return; + _gain.resize(n); + _p.resize(n*n); + _temp.resize(n*n); + _t1.resize(n*n); + reset(); +} + +void kalman_exp::reset() { + size_t const n = _gain.size(); + std::fill_n(_gain.begin(), n, 0); + std::fill_n(_p.begin(), n*n, 0); + for (size_t i=0; i 0); + unsigned const n = end - beg; + resize(n); + + // P = P/lambda + volk_32f_s32f_multiply_32f((float*)&_p[0], (float const*)&_p[0], 1.0f/_lambda, 2*n*n); + + // gain = P*H^{\dagger} + for (unsigned i=0; i const& p) { + _r = p.at("r"); + _lambda = p.at("lambda"); +} + +} // namespace digitalhf +} // namespace gr diff --git a/lib/kalman_exp.hpp b/lib/kalman_exp.hpp new file mode 100644 index 0000000..e8f17d1 --- /dev/null +++ b/lib/kalman_exp.hpp @@ -0,0 +1,43 @@ +// -*- c++ -*- + +#ifndef _LIB_KALMAN_EXP_HPP_ +#define _LIB_KALMAN_EXP_HPP_ + +#include + +#include "filter_update.hpp" +#include "volk_allocator.hpp" + +// see +// [1] https://open.library.ubc.ca/collections/ubctheses/831/items/1.0096286 + +namespace gr { +namespace digitalhf { + +class kalman_exp : public filter_update { +public: + kalman_exp(float r, float lambda); + virtual ~kalman_exp(); + + static sptr make(float r, float lambda); + + virtual void reset(); + virtual gr_complex const* update(gr_complex const*, gr_complex const*); + virtual void set_parameters(std::mapconst &); + +protected: + void resize(size_t); + +private: + typedef std::vector > vec_type; + float _lambda; + float _r; + vec_type _gain; + vec_type _p; + vec_type _temp; + vec_type _t1; +} ; + +} // namespace digitalhf +} // namespace gr +#endif // _LIB_KALMAN_EXP_HPP_ diff --git a/lib/kalman_hsu.cc b/lib/kalman_hsu.cc new file mode 100644 index 0000000..2b21952 --- /dev/null +++ b/lib/kalman_hsu.cc @@ -0,0 +1,107 @@ +// -*- C++ -*- + +#include +#include "kalman_hsu.hpp" +#include + +namespace gr { +namespace digitalhf { + +filter_update::sptr kalman_hsu::make(float q=0.008f, float e=0.01f) { + return filter_update::sptr(new kalman_hsu(q, e)); +} + +kalman_hsu::kalman_hsu(float q, float e) + : _q(q) + , _e(e) + , _g() + , _u() + , _d() + , _a() + , _f() + , _h() { +} + +kalman_hsu::~kalman_hsu() { +} + +void kalman_hsu::resize(size_t n) { + if (_g.size() == n && + _u.size() == n*(n-1)/2 && + _d.size() == n && + _a.size() == n && + _f.size() == n && + _h.size() == n) + return; + _g.resize(n); + _u.resize(n*(n-1)/2); + _d.resize(n); + _a.resize(n); + _f.resize(n); + _h.resize(n); + reset(); +} + +void kalman_hsu::reset() { + std::fill_n(_d.begin(), _d.size(), 1); + std::fill_n(_u.begin(), _u.size(), gr_complex(0)); +} + +gr_complex const* kalman_hsu::update(gr_complex const* beg, + gr_complex const* end) { + assert(end-beg > 0); + unsigned const n = end - beg; + resize(n); + static float y=1.0; + + for (unsigned i=0; i SIMD + _f[j] += _u[idx(i,j)]*std::conj(beg[i]); + } + + for (unsigned j=0; j SIMD + _g[j] = _d[j]*_f[j]; + } + _a[0] = e() + std::real(_g[0]*std::conj(_f[0])); + for (unsigned j=1; j SIMD + gr_complex const b0 = _u[idx(i,j)]; + _u[idx(i,j)] = b0 + _h[j]*std::conj(_g[i]); + _g[i] += _g[j]*std::conj(b0); + } + } + + for (unsigned i=0; i const& p) { + _q = p.at("q"); + _e = p.at("E"); +} + +} // namespace digitalhf +} // namespace gr diff --git a/lib/kalman_hsu.hpp b/lib/kalman_hsu.hpp new file mode 100644 index 0000000..94da938 --- /dev/null +++ b/lib/kalman_hsu.hpp @@ -0,0 +1,56 @@ +// -*- c++ -*- + +#ifndef _LIB_KALMAN_HSU_HPP_ +#define _LIB_KALMAN_HSU_HPP_ + +#include + +#include "filter_update.hpp" +#include "volk_allocator.hpp" + + +namespace gr { +namespace digitalhf { + +// see +// [1] IEEE TRANSACTIONS ON INFORMATION THEORY, VOL. IT-28, NO. 5, SEPTEMBER 1982 753 +// "Square Root Kalman Filtering for High-Speed Data Received over Fading Dispersive HF Channels +// FRANK M. HSU +// [2] https://open.library.ubc.ca/collections/ubctheses/831/items/1.0096286 + +class kalman_hsu : public filter_update { +public: + kalman_hsu(float q, float e); + virtual ~kalman_hsu(); + + static sptr make(float q, float e); + + virtual void reset(); + virtual gr_complex const* update(gr_complex const*, gr_complex const*); + virtual void set_parameters(std::mapconst &); + + inline float q() const { return _q; } + inline float e() const { return _e; } + +protected: + void resize(size_t); + + inline unsigned idx(unsigned i, unsigned j) const { + return i+j*(j-1)/2; // lower-triangular matrix index -> linear index + } +private: + typedef std::vector > complex_vec_type; + typedef std::vector > real_vec_type; + float _q; + float _e; + complex_vec_type _g; // n -- kaman gain + complex_vec_type _u; // n*(n-1)/2 + real_vec_type _d; // n + real_vec_type _a; // n + complex_vec_type _f; // n + complex_vec_type _h; // n +} ; + +} // namespace digitalhf +} // namespace gr +#endif // _LIB_KALMAN_HSU_HPP_ diff --git a/lib/nlms.cc b/lib/nlms.cc new file mode 100644 index 0000000..a676009 --- /dev/null +++ b/lib/nlms.cc @@ -0,0 +1,56 @@ +// -*- C++ -*- + +#include +#include "nlms.hpp" +#include + +namespace gr { +namespace digitalhf { + +filter_update::sptr nlms::make(float mu) { + return filter_update::sptr(new nlms(mu)); +} + +nlms::nlms(float mu) + : _mu(mu) + , _gain() + , _tmp() + , _t1() { +} + +nlms::~nlms() { +} + +void nlms::resize(size_t n) { + if (_gain.size() == n) + return; + _gain.resize(n); + _tmp.resize(n); + _t1.resize(n); + std::fill_n(_gain.begin(), n, 0); +} + +void nlms::reset() { + std::fill_n(_gain.begin(), _gain.size(), 0); +} + +gr_complex const* nlms::update(gr_complex const* beg, + gr_complex const* end) { + assert(end-beg > 0); + size_t const n = end - beg; + resize(n); + volk_32fc_conjugate_32fc(&_tmp[0], beg, n); + volk_32fc_magnitude_squared_32f(&_t1[0], beg, n); + float norm = 0.0f; + volk_32f_accumulator_s32f(&norm, &_t1[0], n); + volk_32f_s32f_multiply_32f((float*)&_gain[0], (float const*)&_tmp[0], _mu/norm, 2*n); + + return &_gain.front(); +} + +void nlms::set_parameters(std::mapconst & p) { + _mu = p.at("mu"); +} + +} // namespace digitalhf +} // namespace gr diff --git a/lib/nlms.hpp b/lib/nlms.hpp new file mode 100644 index 0000000..9e2a9ef --- /dev/null +++ b/lib/nlms.hpp @@ -0,0 +1,40 @@ +// -*- c++ -*- + +#ifndef _LIB_NLMS_HPP_ +#define _LIB_NLMS_HPP_ + +#include + +#include "filter_update.hpp" +#include "volk_allocator.hpp" + + +namespace gr { +namespace digitalhf { + +class nlms : public filter_update { +public: + nlms(float mu); + virtual ~nlms(); + + static sptr make(float mu); + + virtual void reset(); + virtual gr_complex const* update(gr_complex const*, gr_complex const*); + virtual void set_parameters(std::mapconst &); + +protected: + void resize(size_t); + +private: + typedef std::vector > complex_vec_type; + typedef std::vector > real_vec_type; + float _mu; + complex_vec_type _gain; + complex_vec_type _tmp; + real_vec_type _t1; +} ; + +} // namespace digitalhf +} // namespace gr +#endif // _LIB_NLMS_HPP_ diff --git a/lib/rls.cc b/lib/rls.cc index ea68b1f..c5301d2 100644 --- a/lib/rls.cc +++ b/lib/rls.cc @@ -15,7 +15,8 @@ rls::rls(float delta, float lambda) : _delta(delta) , _lambda(lambda) , _gain() - , _inv_corr() { + , _inv_corr() + , _tmp() { } rls::~rls() { @@ -60,8 +61,9 @@ gr_complex const* rls::update(gr_complex const* beg, volk_32fc_s32fc_multiply_32fc(&_tmp[0], &_gain[0], -_pu[i], n); volk_32fc_x2_add_32fc(&_inv_corr[k], &_inv_corr[k], &_tmp[0], n); volk_32f_s32f_multiply_32f((float*)&_inv_corr[k], (float const*)&_inv_corr[k], 1.0f/_lambda, 2*n); - _inv_corr[k+i] += _delta; + _inv_corr[k+i] = std::real(_inv_corr[k+i]); } + return &_gain.front(); }