【C#】 平滑化スプラインを実装する

波形に乗ったノイズを取り除く方法はいくつかあるのですが、今回は平滑化スプラインを実装します。ソースはどこかの論文に書いてあったものをC#に移植したものです。まずは平滑化スプラインを行うSplineSmoothingクラスを作成します。

public class SplineSmoothing
{
    private static double[] _x;
    private static double[] _y;
    private static double[] _dy;
    private static double[] _a;
    private static double[] _b;
    private static double[] _c;
    private static double[] _d;
    private static int _n1;
    private static int _n2;

    /// <summary>
    /// スムージング＋内挿したい場合に呼び出すコンストラクタ
    /// </summary>
    /// <param name="x">xの値</param>
    /// <param name="y">xでのyの値</param>
    /// <param name="s">ペナルティ係数（大きい程滑らかになる）</param>
    public SplineSmoothing(IList<double> x, IList<double> y, double s)
    {
        Initialize(x.ToArray(), y.ToArray(), s);
    }

    /// <summary>
    /// スムージングを実行するメソッド
    /// </summary>
    /// <param name="y">yの値</param>
    /// <param name="s">ペナルティ係数（大きい程滑らかになる）</param>
    /// <returns></returns>
    public static double[] Smooth(IList<double> y, double s)
    {
        var x = new double[y.Count];
        for (int i = 0; i < x.Length; ++i)
        {
            x[i] = i;
        }
        Initialize(x, y.ToArray(), s);
        var smoothed = new double[y.Count];
        for (int i = 0; i < smoothed.Length; ++i)
        {
            smoothed[i] = _Interpolate(x[i]);
        }
        return smoothed;
    }

    /// <summary>
    /// 初期化を実行するメソッド
    /// </summary>
    /// <param name="x">xの値</param>
    /// <param name="y">yの値</param>
    /// <param name="s">ペナルティ係数（大きい程滑らかになる）</param>
    private static void Initialize(double[] x, double[] y, double s)
    {
        _x = new double[x.Length + 2];
        _y = new double[_x.Length];
        _dy = new double[_x.Length];
        for (int i = 1; i < _x.Length - 1; ++i)
        {
            _x[i] = x[i - 1];
            _y[i] = y[i - 1];
            _dy[i] = 1.0;
        }
        _n1 = 1;
        _n2 = _x.Length - 2;

        int m1 = _n1 - 1;
        int m2 = _n2 + 1;
        var r = new double[_x.Length];
        var r1 = new double[_x.Length];
        var r2 = new double[_x.Length];
        var u = new double[_x.Length];
        double p;
        r[m1] = r[_n1] = r1[_n2] = r2[_n2] = r2[m2] = u[m1] = u[_n1] = u[_n2] = u[m2] = p = 0.0;
        m1 = _n1 + 1;
        m2 = _n2 - 1;
        _a = new double[_x.Length];
        var t = new double[_x.Length];
        var t1 = new double[_x.Length];
        double h = _x[m1] - _x[_n1];
        double f = (_y[m1] - _y[_n1]) / h;
        double g = 0.0;
        double e;
        for (int i = m1; i <= m2; ++i)
        {
            g = h;
            h = _x[i + 1] - _x[i];
            e = f;
            f = (_y[i + 1] - _y[i]) / h;
            _a[i] = f - e;
            t[i] = 2.0 * (g + h) / 3.0;
            t1[i] = h / 3.0;
            r2[i] = _dy[i - 1] / g;
            r[i] = _dy[i + 1] / h;
            r1[i] = -_dy[i] / g - _dy[i] / h;
        }
        _b = new double[_x.Length];
        _c = new double[_x.Length];
        _d = new double[_x.Length];
        for (int i = m1; i <= m2; ++i)
        {
            _b[i] = r[i] * r[i] + r1[i] * r1[i] + r2[i] * r2[i];
            _c[i] = r[i] * r1[i + 1] + r1[i] * r2[i + 1];
            _d[i] = r[i] * r2[i + 2];
        }
        double f2 = -s;

        int count = 0;
        while (true)
        {
            if (++count > 10000) break;

            for (int i = m1; i <= m2; ++i)
            {
                r1[i - 1] = f * r[i - 1];
                r2[i - 2] = g * r[i - 2];
                r[i] = 1 / (p * _b[i] + t[i] - f * r1[i - 1] - g * r2[i - 2]);
                u[i] = _a[i] - r1[i - 1] * u[i - 1] - r2[i - 2] * u[i - 2];
                f = p * _c[i] + t1[i] - h * r1[i - 1];
                g = h;
                h = _d[i] * p;
            }
            for (int i = m2; i >= m1; --i)
            {
                u[i] = r[i] * u[i] - r1[i] * u[i + 1] - r2[i] * u[i + 2];
            }
            e = h = 0.0;
            var v = new double[_x.Length + 2];
            for (int i = _n1; i <= m2; ++i)
            {
                g = h;
                h = (u[i + 1] - u[i]) / (_x[i + 1] - _x[i]);
                v[i] = (h - g) * _dy[i] * _dy[i];
                e = e + v[i] * (h - g);
            }
            g = v[_n2] = -h * _dy[_n2] * _dy[_n2];
            e = e - g * h;
            g = f2;
            f2 = e * p * p;
            if (f2 >= s || f2 <= g)
            {
                for (int i = _n1; i <= _n2; ++i)
                {
                    _a[i] = _y[i] - p * v[i];
                    _c[i] = u[i];
                }
                for (int i = _n1; i <= m2; ++i)
                {
                    h = _x[i + 1] - _x[i];
                    _d[i] = (_c[i + 1] - _c[i]) / (3 * h);
                    _b[i] = (_a[i + 1] - _a[i]) / h - (h * _d[i] + _c[i]) * h;
                }
                break;
            }
            else
            {
                f = 0.0;
                h = (v[m1] - v[_n1]) / (_x[m1] - _x[_n1]);
                for (int i = m1; i <= m2; ++i)
                {
                    g = h;
                    h = (v[i + 1] - v[i]) / (_x[i + 1] - _x[i]);
                    g = h - g - r1[i - 1] * r[i - 1] - r2[i - 2] * r[i - 2];
                    f = f + g * r[i] * g;
                    r[i] = g;
                }
                h = e - p * f;
                if (h <= 0.0)
                {
                    for (int i = _n1; i <= _n2; ++i)
                    {
                        _a[i] = _y[i] - p * v[i];
                        _c[i] = u[i];
                    }
                    for (int i = _n1; i <= m2; ++i)
                    {
                        h = _x[i + 1] - _x[i];
                        _d[i] = (_c[i + 1] - _c[i]) / (3 * h);
                        _b[i] = (_a[i + 1] - _a[i]) / h - (h * _d[i] + _c[i]) * h;
                    }
                    break;
                }
                else
                {
                    p = p + (s - f2) / ((Math.Sqrt(s / e) + p) * h);
                }
            }
        }
    }

    /// <summary>
    /// xにおけるyの値を返すメソッド
    /// </summary>
    /// <param name="x">xの値</param>
    /// <returns>yの値</returns>
    public double Interpolate(double x)
    {
        return _Interpolate(x);
    }

    /// <summary>
    /// xにおけるyの値を返すメソッド
    /// </summary>
    /// <param name="x">xの値</param>
    /// <returns>yの値</returns>
    private static double _Interpolate(double x)
    {
        for (int i = _n1; i < _n2; ++i)
        {
            if ((x >= _x[i]) && (x < _x[i + 1]))
            {
                double h = x - _x[i];
                return ((_d[i] * h + _c[i]) * h + _b[i]) * h + _a[i];
            }
        }

        if (x == _x[_n2])
        {
            double h = x - _x[_n2];
            return ((_d[_n2] * h + _c[_n2]) * h + _b[_n2]) * h + _a[_n2];
        }

        throw new ArgumentOutOfRangeException("x", "Extrapolation is not allowed");
    }
}

次にSplineSmoothingの使い方ですが、Sin波に乗ったノイズを取り除く例を示します。まず、内挿が必要無ければ以下のように簡単に記述できます。

// ノイズ付きのSin波を作成する
var xs = new double[100];
var ys = new double[100];
var random = new Random(1);
for (int i = 0; i < xs.Length; ++i)
{
    double x = 2.0 * Math.PI / xs.Length * i;
    xs[i] = x;
    ys[i] = Math.Sin(x) + (random.NextDouble() - 0.5)/5.0;
}

// スムージングする
double penalty = 0.5;
var smoothedYs = SplineSmoothing.Smooth(ys, penalty);

スムージングに加えて内挿が必要な場合は、まずはコンストラクタでSplineSmoothingクラスを生成し、その後Interpolateメソッドで内挿点を指定すれば良いです。

// ノイズ付きのSin波を作成する
var xs = new double[100];
var ys = new double[100];
var random = new Random(1);
for (int i = 0; i < xs.Length; ++i)
{
    double x = 2.0 * Math.PI / xs.Length * i;
    xs[i] = x;
    ys[i] = Math.Sin(x) + (random.NextDouble() - 0.5)/5.0;
}

// SplineSmoothingクラスを生成する
double penalty = 0.5;
var spline = new SplineSmoothing(xs, ys, penalty);
// 内挿点を指定する
double interpolation = 2.0 * Math.PI / 95.0;
// 内挿点におけるスムージングされたyの値を取得する
double y = spline.Interpolate(interpolation);