Interpolation functions

stan/math/prim/fun.hpp

@@ -42,6 +42,7 @@
 #include <stan/math/prim/fun/columns_dot_self.hpp>
 #include <stan/math/prim/fun/conj.hpp>
 #include <stan/math/prim/fun/constants.hpp>
+#include <stan/math/prim/fun/conv_gaus_line.hpp>
 #include <stan/math/prim/fun/copysign.hpp>
 #include <stan/math/prim/fun/corr_constrain.hpp>
 #include <stan/math/prim/fun/corr_free.hpp>
@@ -97,6 +98,7 @@
 #include <stan/math/prim/fun/fmin.hpp>
 #include <stan/math/prim/fun/gamma_p.hpp>
 #include <stan/math/prim/fun/gamma_q.hpp>
+#include <stan/math/prim/fun/gaus_interp.hpp>
 #include <stan/math/prim/fun/get.hpp>
 #include <stan/math/prim/fun/get_base1.hpp>
 #include <stan/math/prim/fun/get_base1_lhs.hpp>

stan/math/prim/fun/conv_gaus_line.hpp

@@ -0,0 +1,67 @@
+#ifndef STAN_MATH_PRIM_FUN_CONV_GAUS_LINE
+#define STAN_MATH_PRIM_FUN_CONV_GAUS_LINE
+
+#include <stan/math/prim/meta.hpp>
+#include <stan/math/prim/prob/normal_cdf.hpp>
+#include <cmath>
+#include <vector>
+
+namespace stan {
+namespace math {
+
+/*
+evaluate the derivative of conv_gaus_line with respect to x
+*/
+double der_conv_gaus_line(double t0, double t1, double a, double b, double x0,
+                          double sig2) {
+  using stan::math::normal_cdf;
+  double pi = stan::math::pi();
+  using std::exp;
+  using std::pow;
+  using std::sqrt;
+  double sig = sqrt(sig2);
+  double y;
+
+  double alpha = sqrt(2 * pi * sig2);
+  y = (a * x0 + b) / alpha
+      * (-exp(-pow(t1 - x0, 2) / (2 * sig2))
+         + exp(-pow(t0 - x0, 2) / (2 * sig2)));
+  y += a * (normal_cdf(t1, x0, sig) - normal_cdf(t0, x0, sig));
+  y -= a / alpha
+       * ((t1 - x0) * exp(-pow(t1 - x0, 2) / (2 * sig2))
+          - (t0 - x0) * exp(-pow(t0 - x0, 2) / (2 * sig2)));
+  return y;
+}
+
+/*
+evaluate the integral
+
+                    | t1
+(2*pi*sig2)**-(1/2) |   (a*t + b) * exp(-(t - x0)^2 / (2*sig2)) dt
+                    | t0
+
+*/
+template <typename Tx>
+double conv_gaus_line(double t0, double t1, double a, double b, Tx const& x,
+                      double sig2) {
+  using stan::math::normal_cdf;
+  double pi = stan::math::pi();
+  using std::exp;
+  using std::pow;
+  using std::sqrt;
+  double sig = sqrt(sig2);
+
+  double y
+      = (a * value_of(x) + b)
+        * (normal_cdf(t1, value_of(x), sig) - normal_cdf(t0, value_of(x), sig));
+  y += -a * sig2 / sqrt(2 * pi * sig2)
+       * (exp(-pow(t1 - value_of(x), 2) / (2 * sig2))
+          - exp(-pow(t0 - value_of(x), 2) / (2 * sig2)));
+
+  return y;
+}
+
+}  // namespace math
+}  // namespace stan
+
+#endif

stan/math/prim/fun/gaus_interp.hpp

@@ -0,0 +1,167 @@
+#ifndef STAN_MATH_PRIM_FUN_GAUS_INTERP
+#define STAN_MATH_PRIM_FUN_GAUS_INTERP
+
+#include <stan/math/prim/fun/conv_gaus_line.hpp>
+#include <cmath>
+#include <vector>
+#include <algorithm>
+
+using namespace std;
+
+namespace stan {
+namespace math {
+
+// set tolerance for how close interpolated values need to be to
+// the specified function values
+const double INTERP_TOL = 1e-8;
+const double SIG2_SCALE = 0.1;
+
+/*
+given a set of points (x_i, y_i) with x_i's in increasing order, find
+a_i, b_i such that the line between (x_i, y_i) and (x_{i+1}, y_{i+1}) is
+f(t) = a_i*t + b_i, store the a_i's and b_i's in as and bs.
+*/
+void lin_interp_coefs(int n, std::vector<double> xs, std::vector<double> ys,
+                      std::vector<double>& as, std::vector<double>& bs) {
+  // initialize size of vectors of linear coefficients
+  as.resize(n - 1);
+  bs.resize(n - 1);
+
+  // find slope and intercept between each point
+  for (int i = 0; i < n - 1; i++) {
+    as[i] = (ys[i + 1] - ys[i]) / (xs[i + 1] - xs[i]);
+    bs[i] = -xs[i] * as[i] + ys[i];
+  }
+}
+
+/*
+linear interpolation at one point, x, given the coefficients
+*/
+double lin_interp_pt(int n, vector<double> xs, vector<double> ys,
+                     vector<double> as, vector<double> bs, double x) {
+  // find interval where x lives
+  if (x <= xs[0])
+    return ys[0];
+  if (x >= xs[n - 1])
+    return ys[n - 1];
+
+  auto lb = upper_bound(xs.begin(), xs.end(), x);
+  int ind = distance(xs.begin(), lb) - 1;
+  ind = std::max(0, ind);
+
+  return as[ind] * x + bs[ind];
+}
+
+/*
+linear interpolation at a vector of points
+*/
+vector<double> lin_interp(int n, std::vector<double> xs, std::vector<double> ys,
+                          int n_new, std::vector<double> xs_new) {
+  // compute coefficients of linear interpolation
+  vector<double> as, bs;
+  lin_interp_coefs(n, xs, ys, as, bs);
+
+  // evaluate at new points
+  vector<double> ys_new;
+  for (int i = 0; i < n_new; i++) {
+    ys_new.push_back(lin_interp_pt(n, xs, ys, as, bs, xs_new[i]));
+  }
+  return ys_new;
+}
+
+/*
+given a set of points and a width for the gaussian kernel, do a convolution
+and evaluate at one point, x
+*/
+template <typename Tx>
+inline return_type_t<Tx> interp_gaus_pt(int n, std::vector<double> xs,
+                                        std::vector<double> ys,
+                                        std::vector<double> as,
+                                        std::vector<double> bs, Tx const& x,
+                                        double sig2) {
+  // extend out first and last lines for convolution
+  double sig = std::sqrt(sig2);
+  xs[0] += -10 * sig;
+  xs[n - 1] += 10 * sig;
+
+  // no need to convolve far from center of gaussian, so
+  // get lower and upper indexes for integration bounds
+  auto lb = upper_bound(xs.begin(), xs.end(), x - 10 * sig);
+  int ind_start = distance(xs.begin(), lb) - 1;
+  ind_start = std::max(0, ind_start);
+
+  auto ub = upper_bound(xs.begin(), xs.end(), x + 10 * sig);
+  int ind_end = distance(xs.begin(), ub);
+  ind_end = std::min(n - 1, ind_end);
+
+  // sum convolutions over intervals
+  using return_t = return_type_t<Tx>;
+  return_t y = 0;
+  for (int i = ind_start; i < ind_end; i++) {
+    y += conv_gaus_line(xs[i], xs[i + 1], as[i], bs[i], x, sig2);
+  }
+
+  return y;
+}
+
+/*
+find the smallest difference between successive elements in a sorted vector
+*/
+template <typename Tx>
+double min_diff(int n, std::vector<Tx> xs) {
+  double dmin = value_of(xs[1]) - value_of(xs[0]);
+  for (int i = 1; i < n - 1; i++) {
+    if (value_of(xs[i + 1]) - value_of(xs[i]) < dmin) {
+      dmin = value_of(xs[i + 1]) - value_of(xs[i]);
+    }
+  }
+  return dmin;
+}
+
+/*
+given a set of pairs (x_i, y_i), do a gaussian interpolation through those
+points and evaluate the interpolation at the points xs_new
+*/
+template <typename Tx>
+inline vector<Tx> gaus_interp(int n, std::vector<double> xs,
+                              std::vector<double> ys, int n_new,
+                              std::vector<Tx> xs_new) {
+  // find minimum distance between points for std of gaussian kernel
+  double sig2 = square(min_diff(n, xs) * SIG2_SCALE);
+
+  // copy ys into a new vector
+  std::vector<double> y2s;
+  y2s.resize(n);
+  for (int i = 0; i < n; i++) {
+    y2s[i] = ys[i];
+  }
+
+  // interatively find interpolation that coincides with ys at xs
+  int max_iters = 50;
+  double dmax, dd;
+  std::vector<double> as, bs;
+  for (int j = 0; j < max_iters; j++) {
+    // linear interpolation for new ys
+    lin_interp_coefs(n, xs, y2s, as, bs);
+    dmax = 0;
+    for (int i = 0; i < n; i++) {
+      dd = ys[i] - interp_gaus_pt(n, xs, y2s, as, bs, xs[i], sig2);
+      y2s[i] += dd;
+      dmax = std::max(std::abs(dd), dmax);
+    }
+    if (dmax < INTERP_TOL)
+      break;
+  }
+
+  // fill ys_new with interpolated values at new_xs
+  std::vector<Tx> ys_new;
+  for (int i = 0; i < n_new; i++) {
+    ys_new.push_back(interp_gaus_pt(n, xs, y2s, as, bs, xs_new[i], sig2));
+  }
+  return ys_new;
+}
+
+}  // namespace math
+}  // namespace stan
+
+#endif

stan/math/rev/fun.hpp

@@ -28,6 +28,7 @@
 #include <stan/math/rev/fun/cholesky_decompose.hpp>
 #include <stan/math/rev/fun/columns_dot_product.hpp>
 #include <stan/math/rev/fun/columns_dot_self.hpp>
+#include <stan/math/rev/fun/conv_gaus_line.hpp>
 #include <stan/math/rev/fun/conj.hpp>
 #include <stan/math/rev/fun/cos.hpp>
 #include <stan/math/rev/fun/cosh.hpp>

stan/math/rev/fun/conv_gaus_line.hpp

@@ -0,0 +1,45 @@
+#ifndef STAN_MATH_REV_FUN_CONV_GAUS_LINE
+#define STAN_MATH_REV_FUN_CONV_GAUS_LINE
+
+#include <stan/math/rev/meta.hpp>
+#include <stan/math/rev/core.hpp>
+#include <stan/math/prim/fun/conv_gaus_line.hpp>
+
+namespace stan {
+namespace math {
+
+namespace internal {
+class conv_gaus_line_vari : public op_v_vari {
+  double t0_;
+  double t1_;
+  double a_;
+  double b_;
+  double sig2_;
+
+ public:
+  explicit conv_gaus_line_vari(double t0, double t1, double a, double b,
+                               vari* avi, double sig2)
+      : t0_(t0),
+        t1_(t1),
+        a_(a),
+        b_(b),
+        sig2_(sig2),
+        op_v_vari(conv_gaus_line(t0, t1, a, b, avi->val_, sig2), avi) {}
+
+  void chain() {
+    avi_->adj_
+        += adj_ * der_conv_gaus_line(t0_, t1_, a_, b_, avi_->val_, sig2_);
+  }
+};
+
+}  // namespace internal
+
+inline var conv_gaus_line(double t0, double t1, double a, double b,
+                          const var& x, double sig2) {
+  return var(new internal::conv_gaus_line_vari(t0, t1, a, b, x.vi_, sig2));
+}
+
+}  // namespace math
+}  // namespace stan
+
+#endif

test/unit/math/mix/fun/conv_gaus_line_test.cpp

@@ -0,0 +1,35 @@
+#include <test/unit/math/test_ad.hpp>
+#include <limits>
+#include <vector>
+
+/*
+check derivative using finite differences and autodiffing
+*/
+TEST(mathMixGausInterp, gaus_conv_der) {
+  double t0, t1, a, b, x0, sig2, y, h, yn, yp, x0p, x0n, dder, dder2;
+  double alpha, sig;
+  using stan::math::conv_gaus_line;
+
+  t0 = 0;
+  t1 = 5;
+  a = -1;
+  b = 2;
+  x0 = 0.99;
+  sig2 = 2;
+
+  // derivative with finite difference
+  h = 1e-4;
+  x0n = x0 + h;
+  x0p = x0 - h;
+  yn = conv_gaus_line(t0, t1, a, b, x0n, sig2);
+  yp = conv_gaus_line(t0, t1, a, b, x0p, sig2);
+  dder = (yn - yp) / (2 * h);
+
+  // derivative with autodiff
+  using stan::math::var;
+  var v = x0;
+  var vy = conv_gaus_line(t0, t1, a, b, v, sig2);
+  vy.grad();
+
+  ASSERT_NEAR(dder, v.adj(), 1e-5);
+}