From 01f8f542bc2d0a79d85e1fa46293e6f58aee665f Mon Sep 17 00:00:00 2001
From: Christoph Mayer <hcab14@gmail.com>
Date: Wed, 11 Sep 2019 16:30:36 +0200
Subject: [PATCH] added more channel filters

---
 lib/CMakeLists.txt       |   3 ++
 lib/adaptive_dfe_impl.cc |  14 +++--
 lib/kalman_exp.cc        | 103 +++++++++++++++++++++++++++++++++++++
 lib/kalman_exp.hpp       |  43 ++++++++++++++++
 lib/kalman_hsu.cc        | 107 +++++++++++++++++++++++++++++++++++++++
 lib/kalman_hsu.hpp       |  56 ++++++++++++++++++++
 lib/nlms.cc              |  56 ++++++++++++++++++++
 lib/nlms.hpp             |  40 +++++++++++++++
 lib/rls.cc               |   6 ++-
 9 files changed, 423 insertions(+), 5 deletions(-)
 create mode 100644 lib/kalman_exp.cc
 create mode 100644 lib/kalman_exp.hpp
 create mode 100644 lib/kalman_hsu.cc
 create mode 100644 lib/kalman_hsu.hpp
 create mode 100644 lib/nlms.cc
 create mode 100644 lib/nlms.hpp

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 <cassert>
+#include "kalman_exp.hpp"
+#include <volk/volk.h>
+
+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<n; ++i)
+    _p[n*i +i] = gr_complex(0.1f); // TODO?
+}
+
+gr_complex const* kalman_exp::update(gr_complex const* beg,
+                                     gr_complex const* end) {
+  assert(end-beg > 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<n; ++i) {
+    _gain[i] = 0;
+    volk_32fc_x2_conjugate_dot_prod_32fc(&_gain[i], &_p[n*i], beg, n);
+  }
+
+  // alpha = H*P*H^{\dagger} + R
+  gr_complex alpha = 0;
+  volk_32fc_x2_dot_prod_32fc(&alpha, &_gain[0], beg, n);
+  alpha += _r;
+
+  // gain = gain / real(alpha)
+  volk_32f_s32f_multiply_32f((float*)&_gain[0], (float const*)&_gain[0], 1.0f/std::real(alpha), 2*n);
+
+  // temp = 1 - G*H
+  for (unsigned i=0; i<n; ++i) {
+    volk_32fc_s32fc_multiply_32fc(&_temp[n*i], beg, -_gain[i], n);
+    _temp[n*i+i] += 1.0f;
+  }
+
+  // T1 = temp * P
+  // P  = T1 * temp^{\dagger} + G*R*G^{\dagger}
+  for (unsigned i=0; i<n; ++i) {
+    // P = P^T so we can use a VOLK kernel below
+    for (unsigned j=0; j<n; ++j)
+      std::swap(_p[n*i+j], _p[n*j+i]);
+
+    for (unsigned j=0; j<n; ++j) {
+      _t1[n*i+j] = 0.0f;
+      volk_32fc_x2_dot_prod_32fc(&_t1[n*i+j], &_temp[n*i], &_p[n*j], n);
+    }
+    for (unsigned j=0; j<n; ++j) {
+      _p[n*i+j] = 0;
+      volk_32fc_x2_conjugate_dot_prod_32fc(&_p[n*i+j], &_t1[n*i], &_temp[n*j], n);
+      _p[n*i+j] += _r * _gain[i]*std::conj(_gain[j]);
+    }
+  }
+
+  return &_gain[0];
+}
+
+void kalman_exp::set_parameters(std::map<std::string, float> 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 <vector>
+
+#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::map<std::string, float>const &);
+
+protected:
+  void resize(size_t);
+
+private:
+  typedef std::vector<gr_complex, volk_allocator<gr_complex> > 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 <cassert>
+#include "kalman_hsu.hpp"
+#include <volk/volk.h>
+
+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<n; ++i)
+    _g[i] /= y;
+
+  _f[0] = std::conj(beg[0]);
+  for (unsigned j=1; j<n; ++j) {
+    _f[j] = _u[idx(0,j)]*std::conj(beg[0]) + std::conj(beg[j]);
+    for (unsigned i=1; i<j; ++i) // TODO: -> SIMD
+      _f[j] += _u[idx(i,j)]*std::conj(beg[i]);
+  }
+
+  for (unsigned j=0; j<n; ++j) { // TODO: -> SIMD
+    _g[j] = _d[j]*_f[j];
+  }
+  _a[0] = e() + std::real(_g[0]*std::conj(_f[0]));
+  for (unsigned j=1; j<n; ++j) {
+    _a[j] = _a[j-1] + std::real(_g[j]*std::conj(_f[j]));
+  }
+
+  float const hq = 1 + q();
+  float const ht = _a[n-1]*q();
+
+  /*float */ y = 1/(_a[0] + ht);
+  _d[0] = _d[0]*hq*(e() + ht)*y;
+
+  for (unsigned j=1; j<n; ++j) {
+    float const b = _a[j-1] + ht;
+    _h[j] = -_f[j]*y;
+
+    y = 1/(_a[j] + ht);
+    _d[j] = _d[j]*hq*b*y;
+
+    for (unsigned i=0; i<j; ++i) { // TODO -> 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<n; ++i)
+    _g[i] *= y;
+
+  return &_g[0];
+}
+
+void kalman_hsu::set_parameters(std::map<std::string, float> 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 <vector>
+
+#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::map<std::string, float>const &);
+
+  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<gr_complex, volk_allocator<gr_complex > > complex_vec_type;
+  typedef std::vector<float, volk_allocator<float> > 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 <cassert>
+#include "nlms.hpp"
+#include <volk/volk.h>
+
+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::map<std::string, float>const & 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 <vector>
+
+#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::map<std::string, float>const &);
+
+protected:
+  void resize(size_t);
+
+private:
+  typedef std::vector<gr_complex, volk_allocator<gr_complex> > complex_vec_type;
+  typedef std::vector<float, volk_allocator<float> > 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();
 }