001/*
002 * Licensed to the Apache Software Foundation (ASF) under one or more
003 * contributor license agreements.  See the NOTICE file distributed with
004 * this work for additional information regarding copyright ownership.
005 * The ASF licenses this file to You under the Apache License, Version 2.0
006 * (the "License"); you may not use this file except in compliance with
007 * the License.  You may obtain a copy of the License at
008 *
009 *      http://www.apache.org/licenses/LICENSE-2.0
010 *
011 * Unless required by applicable law or agreed to in writing, software
012 * distributed under the License is distributed on an "AS IS" BASIS,
013 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
014 * See the License for the specific language governing permissions and
015 * limitations under the License.
016 */
017package org.apache.commons.math4.fitting;
018
019import java.util.ArrayList;
020import java.util.Collection;
021import java.util.List;
022
023import org.apache.commons.math4.analysis.function.HarmonicOscillator;
024import org.apache.commons.math4.exception.MathIllegalStateException;
025import org.apache.commons.math4.exception.NumberIsTooSmallException;
026import org.apache.commons.math4.exception.ZeroException;
027import org.apache.commons.math4.exception.util.LocalizedFormats;
028import org.apache.commons.math4.fitting.leastsquares.LeastSquaresBuilder;
029import org.apache.commons.math4.fitting.leastsquares.LeastSquaresProblem;
030import org.apache.commons.math4.linear.DiagonalMatrix;
031import org.apache.commons.math4.util.FastMath;
032
033/**
034 * Fits points to a {@link
035 * org.apache.commons.math4.analysis.function.HarmonicOscillator.Parametric harmonic oscillator}
036 * function.
037 * <br>
038 * The {@link #withStartPoint(double[]) initial guess values} must be passed
039 * in the following order:
040 * <ul>
041 *  <li>Amplitude</li>
042 *  <li>Angular frequency</li>
043 *  <li>phase</li>
044 * </ul>
045 * The optimal values will be returned in the same order.
046 *
047 * @since 3.3
048 */
049public class HarmonicCurveFitter extends AbstractCurveFitter {
050    /** Parametric function to be fitted. */
051    private static final HarmonicOscillator.Parametric FUNCTION = new HarmonicOscillator.Parametric();
052    /** Initial guess. */
053    private final double[] initialGuess;
054    /** Maximum number of iterations of the optimization algorithm. */
055    private final int maxIter;
056
057    /**
058     * Constructor used by the factory methods.
059     *
060     * @param initialGuess Initial guess. If set to {@code null}, the initial guess
061     * will be estimated using the {@link ParameterGuesser}.
062     * @param maxIter Maximum number of iterations of the optimization algorithm.
063     */
064    private HarmonicCurveFitter(double[] initialGuess,
065                                int maxIter) {
066        this.initialGuess = initialGuess;
067        this.maxIter = maxIter;
068    }
069
070    /**
071     * Creates a default curve fitter.
072     * The initial guess for the parameters will be {@link ParameterGuesser}
073     * computed automatically, and the maximum number of iterations of the
074     * optimization algorithm is set to {@link Integer#MAX_VALUE}.
075     *
076     * @return a curve fitter.
077     *
078     * @see #withStartPoint(double[])
079     * @see #withMaxIterations(int)
080     */
081    public static HarmonicCurveFitter create() {
082        return new HarmonicCurveFitter(null, Integer.MAX_VALUE);
083    }
084
085    /**
086     * Configure the start point (initial guess).
087     * @param newStart new start point (initial guess)
088     * @return a new instance.
089     */
090    public HarmonicCurveFitter withStartPoint(double[] newStart) {
091        return new HarmonicCurveFitter(newStart.clone(),
092                                       maxIter);
093    }
094
095    /**
096     * Configure the maximum number of iterations.
097     * @param newMaxIter maximum number of iterations
098     * @return a new instance.
099     */
100    public HarmonicCurveFitter withMaxIterations(int newMaxIter) {
101        return new HarmonicCurveFitter(initialGuess,
102                                       newMaxIter);
103    }
104
105    /** {@inheritDoc} */
106    @Override
107    protected LeastSquaresProblem getProblem(Collection<WeightedObservedPoint> observations) {
108        // Prepare least-squares problem.
109        final int len = observations.size();
110        final double[] target  = new double[len];
111        final double[] weights = new double[len];
112
113        int i = 0;
114        for (WeightedObservedPoint obs : observations) {
115            target[i]  = obs.getY();
116            weights[i] = obs.getWeight();
117            ++i;
118        }
119
120        final AbstractCurveFitter.TheoreticalValuesFunction model
121            = new AbstractCurveFitter.TheoreticalValuesFunction(FUNCTION,
122                                                                observations);
123
124        final double[] startPoint = initialGuess != null ?
125            initialGuess :
126            // Compute estimation.
127            new ParameterGuesser(observations).guess();
128
129        // Return a new optimizer set up to fit a Gaussian curve to the
130        // observed points.
131        return new LeastSquaresBuilder().
132                maxEvaluations(Integer.MAX_VALUE).
133                maxIterations(maxIter).
134                start(startPoint).
135                target(target).
136                weight(new DiagonalMatrix(weights)).
137                model(model.getModelFunction(), model.getModelFunctionJacobian()).
138                build();
139
140    }
141
142    /**
143     * This class guesses harmonic coefficients from a sample.
144     * <p>The algorithm used to guess the coefficients is as follows:</p>
145     *
146     * <p>We know \( f(t) \) at some sampling points \( t_i \) and want
147     * to find \( a \), \( \omega \) and \( \phi \) such that
148     * \( f(t) = a \cos (\omega t + \phi) \).
149     * </p>
150     *
151     * <p>From the analytical expression, we can compute two primitives :
152     * \[
153     *     If2(t) = \int f^2 dt  = a^2 (t + S(t)) / 2
154     * \]
155     * \[
156     *     If'2(t) = \int f'^2 dt = a^2 \omega^2 (t - S(t)) / 2
157     * \]
158     * where \(S(t) = \frac{\sin(2 (\omega t + \phi))}{2\omega}\)
159     * </p>
160     *
161     * <p>We can remove \(S\) between these expressions :
162     * \[
163     *     If'2(t) = a^2 \omega^2 t - \omega^2 If2(t)
164     * \]
165     * </p>
166     *
167     * <p>The preceding expression shows that \(If'2 (t)\) is a linear
168     * combination of both \(t\) and \(If2(t)\):
169     * \[
170     *   If'2(t) = A t + B If2(t)
171     * \]
172     * </p>
173     *
174     * <p>From the primitive, we can deduce the same form for definite
175     * integrals between \(t_1\) and \(t_i\) for each \(t_i\) :
176     * \[
177     *   If2(t_i) - If2(t_1) = A (t_i - t_1) + B (If2 (t_i) - If2(t_1))
178     * \]
179     * </p>
180     *
181     * <p>We can find the coefficients \(A\) and \(B\) that best fit the sample
182     * to this linear expression by computing the definite integrals for
183     * each sample points.
184     * </p>
185     *
186     * <p>For a bilinear expression \(z(x_i, y_i) = A x_i + B y_i\), the
187     * coefficients \(A\) and \(B\) that minimize a least-squares criterion
188     * \(\sum (z_i - z(x_i, y_i))^2\) are given by these expressions:</p>
189     * \[
190     *   A = \frac{\sum y_i y_i \sum x_i z_i - \sum x_i y_i \sum y_i z_i}
191     *            {\sum x_i x_i \sum y_i y_i - \sum x_i y_i \sum x_i y_i}
192     * \]
193     * \[
194     *   B = \frac{\sum x_i x_i \sum y_i z_i - \sum x_i y_i \sum x_i z_i}
195     *            {\sum x_i x_i \sum y_i y_i - \sum x_i y_i \sum x_i y_i}
196     *
197     * \]
198     *
199     * <p>In fact, we can assume that both \(a\) and \(\omega\) are positive and
200     * compute them directly, knowing that \(A = a^2 \omega^2\) and that
201     * \(B = -\omega^2\). The complete algorithm is therefore:</p>
202     *
203     * For each \(t_i\) from \(t_1\) to \(t_{n-1}\), compute:
204     * \[ f(t_i) \]
205     * \[ f'(t_i) = \frac{f (t_{i+1}) - f(t_{i-1})}{t_{i+1} - t_{i-1}} \]
206     * \[ x_i = t_i  - t_1 \]
207     * \[ y_i = \int_{t_1}^{t_i} f^2(t) dt \]
208     * \[ z_i = \int_{t_1}^{t_i} f'^2(t) dt \]
209     * and update the sums:
210     * \[ \sum x_i x_i, \sum y_i y_i, \sum x_i y_i, \sum x_i z_i, \sum y_i z_i \]
211     *
212     * Then:
213     * \[
214     *  a = \sqrt{\frac{\sum y_i y_i  \sum x_i z_i - \sum x_i y_i \sum y_i z_i }
215     *                 {\sum x_i y_i  \sum x_i z_i - \sum x_i x_i \sum y_i z_i }}
216     * \]
217     * \[
218     *  \omega = \sqrt{\frac{\sum x_i y_i \sum x_i z_i - \sum x_i x_i \sum y_i z_i}
219     *                      {\sum x_i x_i \sum y_i y_i - \sum x_i y_i \sum x_i y_i}}
220     * \]
221     *
222     * <p>Once we know \(\omega\) we can compute:
223     * \[
224     *    fc = \omega f(t) \cos(\omega t) - f'(t) \sin(\omega t)
225     * \]
226     * \[
227     *    fs = \omega f(t) \sin(\omega t) + f'(t) \cos(\omega t)
228     * \]
229     * </p>
230     *
231     * <p>It appears that \(fc = a \omega \cos(\phi)\) and
232     * \(fs = -a \omega \sin(\phi)\), so we can use these
233     * expressions to compute \(\phi\). The best estimate over the sample is
234     * given by averaging these expressions.
235     * </p>
236     *
237     * <p>Since integrals and means are involved in the preceding
238     * estimations, these operations run in \(O(n)\) time, where \(n\) is the
239     * number of measurements.</p>
240     */
241    public static class ParameterGuesser {
242        /** Amplitude. */
243        private final double a;
244        /** Angular frequency. */
245        private final double omega;
246        /** Phase. */
247        private final double phi;
248
249        /**
250         * Simple constructor.
251         *
252         * @param observations Sampled observations.
253         * @throws NumberIsTooSmallException if the sample is too short.
254         * @throws ZeroException if the abscissa range is zero.
255         * @throws MathIllegalStateException when the guessing procedure cannot
256         * produce sensible results.
257         */
258        public ParameterGuesser(Collection<WeightedObservedPoint> observations) {
259            if (observations.size() < 4) {
260                throw new NumberIsTooSmallException(LocalizedFormats.INSUFFICIENT_OBSERVED_POINTS_IN_SAMPLE,
261                                                    observations.size(), 4, true);
262            }
263
264            final WeightedObservedPoint[] sorted
265                = sortObservations(observations).toArray(new WeightedObservedPoint[0]);
266
267            final double aOmega[] = guessAOmega(sorted);
268            a = aOmega[0];
269            omega = aOmega[1];
270
271            phi = guessPhi(sorted);
272        }
273
274        /**
275         * Gets an estimation of the parameters.
276         *
277         * @return the guessed parameters, in the following order:
278         * <ul>
279         *  <li>Amplitude</li>
280         *  <li>Angular frequency</li>
281         *  <li>Phase</li>
282         * </ul>
283         */
284        public double[] guess() {
285            return new double[] { a, omega, phi };
286        }
287
288        /**
289         * Sort the observations with respect to the abscissa.
290         *
291         * @param unsorted Input observations.
292         * @return the input observations, sorted.
293         */
294        private List<WeightedObservedPoint> sortObservations(Collection<WeightedObservedPoint> unsorted) {
295            final List<WeightedObservedPoint> observations = new ArrayList<>(unsorted);
296
297            // Since the samples are almost always already sorted, this
298            // method is implemented as an insertion sort that reorders the
299            // elements in place. Insertion sort is very efficient in this case.
300            WeightedObservedPoint curr = observations.get(0);
301            final int len = observations.size();
302            for (int j = 1; j < len; j++) {
303                WeightedObservedPoint prec = curr;
304                curr = observations.get(j);
305                if (curr.getX() < prec.getX()) {
306                    // the current element should be inserted closer to the beginning
307                    int i = j - 1;
308                    WeightedObservedPoint mI = observations.get(i);
309                    while ((i >= 0) && (curr.getX() < mI.getX())) {
310                        observations.set(i + 1, mI);
311                        if (i-- != 0) {
312                            mI = observations.get(i);
313                        }
314                    }
315                    observations.set(i + 1, curr);
316                    curr = observations.get(j);
317                }
318            }
319
320            return observations;
321        }
322
323        /**
324         * Estimate a first guess of the amplitude and angular frequency.
325         *
326         * @param observations Observations, sorted w.r.t. abscissa.
327         * @throws ZeroException if the abscissa range is zero.
328         * @throws MathIllegalStateException when the guessing procedure cannot
329         * produce sensible results.
330         * @return the guessed amplitude (at index 0) and circular frequency
331         * (at index 1).
332         */
333        private double[] guessAOmega(WeightedObservedPoint[] observations) {
334            final double[] aOmega = new double[2];
335
336            // initialize the sums for the linear model between the two integrals
337            double sx2 = 0;
338            double sy2 = 0;
339            double sxy = 0;
340            double sxz = 0;
341            double syz = 0;
342
343            double currentX = observations[0].getX();
344            double currentY = observations[0].getY();
345            double f2Integral = 0;
346            double fPrime2Integral = 0;
347            final double startX = currentX;
348            for (int i = 1; i < observations.length; ++i) {
349                // one step forward
350                final double previousX = currentX;
351                final double previousY = currentY;
352                currentX = observations[i].getX();
353                currentY = observations[i].getY();
354
355                // update the integrals of f<sup>2</sup> and f'<sup>2</sup>
356                // considering a linear model for f (and therefore constant f')
357                final double dx = currentX - previousX;
358                final double dy = currentY - previousY;
359                final double f2StepIntegral =
360                    dx * (previousY * previousY + previousY * currentY + currentY * currentY) / 3;
361                final double fPrime2StepIntegral = dy * dy / dx;
362
363                final double x = currentX - startX;
364                f2Integral += f2StepIntegral;
365                fPrime2Integral += fPrime2StepIntegral;
366
367                sx2 += x * x;
368                sy2 += f2Integral * f2Integral;
369                sxy += x * f2Integral;
370                sxz += x * fPrime2Integral;
371                syz += f2Integral * fPrime2Integral;
372            }
373
374            // compute the amplitude and pulsation coefficients
375            double c1 = sy2 * sxz - sxy * syz;
376            double c2 = sxy * sxz - sx2 * syz;
377            double c3 = sx2 * sy2 - sxy * sxy;
378            if ((c1 / c2 < 0) || (c2 / c3 < 0)) {
379                final int last = observations.length - 1;
380                // Range of the observations, assuming that the
381                // observations are sorted.
382                final double xRange = observations[last].getX() - observations[0].getX();
383                if (xRange == 0) {
384                    throw new ZeroException();
385                }
386                aOmega[1] = 2 * Math.PI / xRange;
387
388                double yMin = Double.POSITIVE_INFINITY;
389                double yMax = Double.NEGATIVE_INFINITY;
390                for (int i = 1; i < observations.length; ++i) {
391                    final double y = observations[i].getY();
392                    if (y < yMin) {
393                        yMin = y;
394                    }
395                    if (y > yMax) {
396                        yMax = y;
397                    }
398                }
399                aOmega[0] = 0.5 * (yMax - yMin);
400            } else {
401                if (c2 == 0) {
402                    // In some ill-conditioned cases (cf. MATH-844), the guesser
403                    // procedure cannot produce sensible results.
404                    throw new MathIllegalStateException(LocalizedFormats.ZERO_DENOMINATOR);
405                }
406
407                aOmega[0] = FastMath.sqrt(c1 / c2);
408                aOmega[1] = FastMath.sqrt(c2 / c3);
409            }
410
411            return aOmega;
412        }
413
414        /**
415         * Estimate a first guess of the phase.
416         *
417         * @param observations Observations, sorted w.r.t. abscissa.
418         * @return the guessed phase.
419         */
420        private double guessPhi(WeightedObservedPoint[] observations) {
421            // initialize the means
422            double fcMean = 0;
423            double fsMean = 0;
424
425            double currentX = observations[0].getX();
426            double currentY = observations[0].getY();
427            for (int i = 1; i < observations.length; ++i) {
428                // one step forward
429                final double previousX = currentX;
430                final double previousY = currentY;
431                currentX = observations[i].getX();
432                currentY = observations[i].getY();
433                final double currentYPrime = (currentY - previousY) / (currentX - previousX);
434
435                double omegaX = omega * currentX;
436                double cosine = FastMath.cos(omegaX);
437                double sine = FastMath.sin(omegaX);
438                fcMean += omega * currentY * cosine - currentYPrime * sine;
439                fsMean += omega * currentY * sine + currentYPrime * cosine;
440            }
441
442            return FastMath.atan2(-fsMean, fcMean);
443        }
444    }
445}