51 lines
1.6 KiB
C++
51 lines
1.6 KiB
C++
#include "linalg.h"
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <vector>
|
|
|
|
|
|
|
|
std::vector<float> Mengu::dsp::solve_sym_toeplitz(const std::vector<float> &cols, const std::vector<float> &y) {
|
|
|
|
// good ol dynamic programming to repeatedly do each step of levinson_recursion consequtively
|
|
std::vector<float> backward_vec = {1.0f / cols.at(0)}; // y.size assumed small, so use vec
|
|
backward_vec.reserve(y.size());
|
|
|
|
std::vector<float> forward_vec(1);
|
|
|
|
float error = 0.0f;
|
|
|
|
std::vector<float> result = {y.at(0) / cols.at(0)};
|
|
result.reserve(y.size());
|
|
|
|
for (int n = 1; n < y.size(); n++) {
|
|
// calculate the current error and backward vec
|
|
std::copy(backward_vec.crbegin(), backward_vec.crend(), forward_vec.begin());
|
|
error = dot(cols.data() + 1, backward_vec.data(), n);
|
|
|
|
float denom = 1.0f / (1 - error * error);
|
|
|
|
scalar_mul_inplace(denom, backward_vec.data(), backward_vec.size());
|
|
backward_vec.emplace(backward_vec.begin(), 0);
|
|
|
|
|
|
scalar_mul_inplace(-error * denom, forward_vec.data(), forward_vec.size());
|
|
forward_vec.push_back(0);
|
|
|
|
vec_add_inplace(forward_vec, backward_vec);
|
|
|
|
|
|
// calculate this iterations result
|
|
auto cols_iter = cols.crbegin() + (cols.size() - n - 1); // the elements n to 1
|
|
float result_error = dot(cols_iter, cols.crend() - 1, result.cbegin());
|
|
|
|
std::vector<float> scaled_backward_vec = scalar_mul(y[n] - result_error, backward_vec);
|
|
result.push_back(0);
|
|
|
|
vec_add_inplace(scaled_backward_vec, result);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|