diff --git a/cpp_src/dsp/lasp_freqsmooth.cpp b/cpp_src/dsp/lasp_freqsmooth.cpp
new file mode 100644
index 0000000..795809b
--- /dev/null
+++ b/cpp_src/dsp/lasp_freqsmooth.cpp
@@ -0,0 +1,87 @@
+// #define DEBUGTRACE_ENABLED
+#include "debugtrace.hpp"
+#include "lasp_freqsmooth.h"
+
+using rte = std::runtime_error;
+
+vd freqSmooth(const vd& freq, const vd& X, const unsigned w) {
+  DEBUGTRACE_ENTER;
+  if (freq.size() < 2) {
+    throw rte("Invalid frequency size");
+  }
+  if (freq.size() != X.size()) {
+    throw rte("Sizes of freq and X do not match");
+  }
+  if (freq.size() > std::numeric_limits<long>::max() / 2) {
+    throw rte("Frequency size limit for smoothing is 2^30");
+  }
+  if (w == 0) {
+    throw rte("Invalid number of octaves");
+  }
+
+  // Smoothing width in unit of number of octaves
+  const d Delta = 1 / d(w);
+
+  d freq_min;
+  if (freq(0) == 0) {
+    freq_min = freq(1);
+  } else {
+    freq_min = freq(0);
+  }
+  const d freq_max = freq(freq.size() - 1);
+  DEBUGTRACE_PRINT(freq_min);
+  DEBUGTRACE_PRINT(freq_max);
+  const vd freq_log =
+      arma::logspace(d_log10(freq_min), d_log10(freq_max), 10 * freq.size());
+  DEBUGTRACE_PRINT("freq_log = ");
+//   std::cerr << freq_log << std::endl;
+
+  const long N = freq_log.size();
+
+  // Interpolate X to logscale
+  vd X_log;
+  DEBUGTRACE_PRINT("X_log = :");
+  arma::interp1(freq, X, freq_log, X_log, "*linear");
+  // Last point is not interpolated well.
+  X_log(N - 1) = X(X.size() - 1);
+//   std::cerr << X_log << std::endl;
+
+  vd Xsm_log(freq_log.size());
+  const d beta = d_log10(N) / d_log10(2) / (N - 1);
+  DEBUGTRACE_PRINT(beta);
+
+  const long mu = int(Delta / 2 / beta);
+  DEBUGTRACE_PRINT(mu);
+  // Long is at least 32 bits. So +/- 2M points length
+  for (long k = 0; k < freq_log.size(); k++) {
+    // const d fcur = freq_log(k);
+    const us idx_start = std::max(k - mu, 0l);
+    const us idx_stop = std::min(k + mu, N - 1);
+    // DEBUGTRACE_PRINT(idx_start)
+    // DEBUGTRACE_PRINT(idx_stop);
+
+    Xsm_log(k) = arma::mean(X_log.subvec(idx_start, idx_stop));
+  }
+  DEBUGTRACE_PRINT("Xsm_log:");
+//   std::cerr << Xsm_log << std::endl;
+
+  // Back-interpolate to a linear scale
+  vd Xsm(freq.size());
+  if (freq(0) == 0) {
+    Xsm(0) = X(0);
+    vd Xsm_gt0;
+    arma::interp1(freq_log, Xsm_log, freq.subvec(1, freq.size() - 1), Xsm_gt0,
+                  "*linear");
+    Xsm.subvec(1, freq.size() - 1) = Xsm_gt0;
+    Xsm(1) = Xsm_log(1);
+
+  } else {
+    Xsm(0) = X(0);
+    arma::interp1(freq_log, Xsm_log, freq, Xsm, "*linear");
+  }
+  Xsm(freq.size() - 1) = Xsm_log(N - 1);
+  DEBUGTRACE_PRINT("Xsm:");
+//   std::cerr << Xsm << std::endl;
+
+  return Xsm;
+}
\ No newline at end of file
diff --git a/cpp_src/dsp/lasp_freqsmooth.h b/cpp_src/dsp/lasp_freqsmooth.h
new file mode 100644
index 0000000..11220ee
--- /dev/null
+++ b/cpp_src/dsp/lasp_freqsmooth.h
@@ -0,0 +1,21 @@
+#pragma once
+#include <vector>
+#include <memory>
+#include "lasp_types.h"
+#include "lasp_mathtypes.h"
+
+
+/**
+ * \addtogroup dsp
+ * @{
+ */
+
+
+/**
+ * @brief Frequency domain smoothing. Returns a smoothed variant of the power
+ * spectrum given as input.
+ * 
+ */
+vd freqSmooth(const vd& freq,const vd& X,const unsigned w);
+
+/** @} */