mirror of https://github.com/hb9fxq/gr-digitalhf
76 lines
1.8 KiB
C++
76 lines
1.8 KiB
C++
// -*- C++ -*-
|
|
|
|
#include <cassert>
|
|
#include "rls.hpp"
|
|
#include <volk/volk.h>
|
|
|
|
namespace gr {
|
|
namespace digitalhf {
|
|
|
|
filter_update::sptr rls::make(float delta, float lambda) {
|
|
return filter_update::sptr(new rls(delta, lambda));
|
|
}
|
|
|
|
rls::rls(float delta, float lambda)
|
|
: _delta(delta)
|
|
, _lambda(lambda)
|
|
, _gain()
|
|
, _inv_corr()
|
|
, _tmp() {
|
|
}
|
|
|
|
rls::~rls() {
|
|
}
|
|
|
|
void rls::resize(size_t n) {
|
|
if (_gain.size() == n && _inv_corr.size() == n*n)
|
|
return;
|
|
_gain.resize(n);
|
|
_inv_corr.resize(n*n);
|
|
_pu.resize(n);
|
|
_tmp.resize(n);
|
|
reset();
|
|
}
|
|
|
|
void rls::reset() {
|
|
size_t const n = _gain.size();
|
|
std::fill_n(_gain.begin(), n, 0);
|
|
std::fill_n(_inv_corr.begin(), n*n, 0);
|
|
for (size_t i=0; i<n; ++i)
|
|
_inv_corr[n*i +i] = gr_complex(_delta, 0);
|
|
}
|
|
|
|
gr_complex const* rls::update(gr_complex const* beg,
|
|
gr_complex const* end) {
|
|
assert(end-beg > 0);
|
|
unsigned const n = end - beg;
|
|
resize(n);
|
|
|
|
for (unsigned i=0; i<n; ++i)
|
|
volk_32fc_x2_dot_prod_32fc(&_pu[i], &_inv_corr[n*i], beg, n);
|
|
|
|
gr_complex uPu = 0;
|
|
volk_32fc_x2_conjugate_dot_prod_32fc(&uPu, &_pu[0], beg, n);
|
|
|
|
volk_32fc_conjugate_32fc(&_tmp[0], &_pu[0], n);
|
|
volk_32f_s32f_multiply_32f((float*)&_gain[0], (float const*)&_tmp[0], 1.0f/(_lambda + std::real(uPu)/_lambda), 2*n);
|
|
// volk_32fc_s32fc_multiply_32fc(&_gain[0], &_tmp[0], 1/(_lambda + uPu), n);
|
|
|
|
for (unsigned i=0; i<n; ++i) {
|
|
unsigned const k = n*i;
|
|
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);
|
|
}
|
|
|
|
return _gain.data();
|
|
}
|
|
|
|
void rls::set_parameters(std::map<std::string, float> const& p) {
|
|
_delta = p.at("delta");
|
|
_lambda = p.at("lambda");
|
|
}
|
|
|
|
} // namespace digitalhf
|
|
} // namespace gr
|