View Javadoc
1   /*
2    * Licensed to the Hipparchus project under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *      http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.hipparchus.optim.nonlinear.vector.leastsquares;
18  
19  import org.hipparchus.exception.LocalizedCoreFormats;
20  import org.hipparchus.exception.MathIllegalArgumentException;
21  import org.hipparchus.exception.MathIllegalStateException;
22  import org.hipparchus.exception.NullArgumentException;
23  import org.hipparchus.linear.ArrayRealVector;
24  import org.hipparchus.linear.CholeskyDecomposition;
25  import org.hipparchus.linear.MatrixDecomposer;
26  import org.hipparchus.linear.MatrixUtils;
27  import org.hipparchus.linear.QRDecomposer;
28  import org.hipparchus.linear.RealMatrix;
29  import org.hipparchus.linear.RealVector;
30  import org.hipparchus.optim.ConvergenceChecker;
31  import org.hipparchus.optim.LocalizedOptimFormats;
32  import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation;
33  import org.hipparchus.util.Incrementor;
34  import org.hipparchus.util.Pair;
35  
36  /**
37   * Sequential Gauss-Newton least-squares solver.
38   * <p>
39   * This class solve a least-square problem by solving the normal equations of
40   * the linearized problem at each iteration.
41   * </p>
42   *
43   */
44  public class SequentialGaussNewtonOptimizer implements LeastSquaresOptimizer {
45  
46      /**
47       * The singularity threshold for matrix decompositions. Determines when a
48       * {@link MathIllegalStateException} is thrown. The current value was the
49       * default value for {@link LUDecomposition}.
50       */
51      private static final double SINGULARITY_THRESHOLD = 1e-11;
52  
53      /** Decomposer. */
54      private final MatrixDecomposer decomposer;
55  
56      /** Indicates if normal equations should be formed explicitly. */
57      private final boolean formNormalEquations;
58  
59      /** Old evaluation previously computed. */
60      private final Evaluation oldEvaluation;
61  
62      /** Old jacobian previously computed. */
63      private final RealMatrix oldLhs;
64  
65      /** Old residuals previously computed. */
66      private final RealVector oldRhs;
67  
68      /**
69       * Create a sequential Gauss Newton optimizer.
70       * <p/>
71       * The default for the algorithm is to use QR decomposition, not
72       * form normal equations and have no previous evaluation
73       * </p>
74       *
75       */
76      public SequentialGaussNewtonOptimizer() {
77          this(new QRDecomposer(SINGULARITY_THRESHOLD), false, null);
78      }
79  
80      /**
81       * Create a sequential Gauss Newton optimizer that uses the given matrix
82       * decomposition algorithm to solve the normal equations.
83       * <p>
84       * The {@code decomposer} is used to solve J<sup>T</sup>Jx=J<sup>T</sup>r.
85       * </p>
86       *
87       * @param decomposer the decomposition algorithm to use.
88       * @param formNormalEquations whether the normal equations should be explicitly
89       *                            formed. If {@code true} then {@code decomposer} is used
90       *                            to solve J<sup>T</sup>Jx=J<sup>T</sup>r, otherwise
91       *                            {@code decomposer} is used to solve Jx=r. If {@code
92       *                            decomposer} can only solve square systems then this
93       *                            parameter should be {@code true}.
94       * @param evaluation old evaluation previously computed, null if there are no previous evaluations.
95       */
96      public SequentialGaussNewtonOptimizer(final MatrixDecomposer decomposer,
97                                            final boolean formNormalEquations,
98                                            final Evaluation evaluation) {
99          this.decomposer          = decomposer;
100         this.formNormalEquations = formNormalEquations;
101         this.oldEvaluation       = evaluation;
102         if (evaluation == null) {
103             this.oldLhs = null;
104             this.oldRhs = null;
105         } else {
106             if (formNormalEquations) {
107                 final Pair<RealMatrix, RealVector> normalEquation =
108                                 computeNormalMatrix(evaluation.getJacobian(), evaluation.getResiduals());
109                 // solve the linearized least squares problem
110                 this.oldLhs = normalEquation.getFirst();
111                 this.oldRhs = normalEquation.getSecond();
112             } else {
113                 this.oldLhs = evaluation.getJacobian();
114                 this.oldRhs = evaluation.getResiduals();
115             }
116         }
117     }
118 
119     /**
120      * Get the matrix decomposition algorithm.
121      *
122      * @return the decomposition algorithm.
123      */
124     public MatrixDecomposer getDecomposer() {
125         return decomposer;
126     }
127 
128     /**
129      * Configure the matrix decomposition algorithm.
130      *
131      * @param newDecomposer the decomposition algorithm to use.
132      * @return a new instance.
133      */
134     public SequentialGaussNewtonOptimizer withDecomposer(final MatrixDecomposer newDecomposer) {
135         return new SequentialGaussNewtonOptimizer(newDecomposer,
136                                                   this.isFormNormalEquations(),
137                                                   this.getOldEvaluation());
138     }
139 
140     /**
141      * Get if the normal equations are explicitly formed.
142      *
143      * @return if the normal equations should be explicitly formed. If {@code true} then
144      * {@code decomposer} is used to solve J<sup>T</sup>Jx=J<sup>T</sup>r, otherwise
145      * {@code decomposer} is used to solve Jx=r.
146      */
147     public boolean isFormNormalEquations() {
148         return formNormalEquations;
149     }
150 
151     /**
152      * Configure if the normal equations should be explicitly formed.
153      *
154      * @param newFormNormalEquations whether the normal equations should be explicitly
155      *                               formed. If {@code true} then {@code decomposer} is used
156      *                               to solve J<sup>T</sup>Jx=J<sup>T</sup>r, otherwise
157      *                               {@code decomposer} is used to solve Jx=r. If {@code
158      *                               decomposer} can only solve square systems then this
159      *                               parameter should be {@code true}.
160      * @return a new instance.
161      */
162     public SequentialGaussNewtonOptimizer withFormNormalEquations(final boolean newFormNormalEquations) {
163         return new SequentialGaussNewtonOptimizer(this.getDecomposer(),
164                                                   newFormNormalEquations,
165                                                   this.getOldEvaluation());
166     }
167 
168     /**
169      * Get the previous evaluation used by the optimizer.
170      *
171      * @return the previous evaluation.
172      */
173     public Evaluation getOldEvaluation() {
174         return oldEvaluation;
175     }
176 
177     /**
178      * Configure the previous evaluation used by the optimizer.
179      * <p>
180      * This building method uses a complete evaluation to retrieve
181      * a priori data. Note that as {@link #withAPrioriData(RealVector, RealMatrix)}
182      * generates a fake evaluation and calls this method, either
183      * {@link #withAPrioriData(RealVector, RealMatrix)} or {@link #withEvaluation(LeastSquaresProblem.Evaluation)}
184      * should be called, but not both as the last one called will override the previous one.
185      * </p>
186      * @param previousEvaluation the previous evaluation used by the optimizer.
187      * @return a new instance.
188      */
189     public SequentialGaussNewtonOptimizer withEvaluation(final Evaluation previousEvaluation) {
190         return new SequentialGaussNewtonOptimizer(this.getDecomposer(),
191                                                   this.isFormNormalEquations(),
192                                                   previousEvaluation);
193     }
194 
195     /**
196      * Configure from a priori state and covariance.
197      * <p>
198      * This building method generates a fake evaluation and calls
199      * {@link #withEvaluation(LeastSquaresProblem.Evaluation)}, so either
200      * {@link #withAPrioriData(RealVector, RealMatrix)} or {@link #withEvaluation(LeastSquaresProblem.Evaluation)}
201      * should be called, but not both as the last one called will override the previous one.
202      * <p>
203      * A Cholesky decomposition is used to compute the weighted jacobian from the
204      * a priori covariance. This method uses the default thresholds of the decomposition.
205      * </p>
206      * @param aPrioriState a priori state to use
207      * @param aPrioriCovariance a priori covariance to use
208      * @return a new instance.
209      * @see #withAPrioriData(RealVector, RealMatrix, double, double)
210      */
211     public SequentialGaussNewtonOptimizer withAPrioriData(final RealVector aPrioriState,
212                                                           final RealMatrix aPrioriCovariance) {
213         return withAPrioriData(aPrioriState, aPrioriCovariance,
214                                CholeskyDecomposition.DEFAULT_RELATIVE_SYMMETRY_THRESHOLD,
215                                CholeskyDecomposition.DEFAULT_ABSOLUTE_POSITIVITY_THRESHOLD);
216     }
217 
218     /**
219      * Configure from a priori state and covariance.
220      * <p>
221      * This building method generates a fake evaluation and calls
222      * {@link #withEvaluation(LeastSquaresProblem.Evaluation)}, so either
223      * {@link #withAPrioriData(RealVector, RealMatrix)} or {@link #withEvaluation(LeastSquaresProblem.Evaluation)}
224      * should be called, but not both as the last one called will override the previous one.
225      * <p>
226      * A Cholesky decomposition is used to compute the weighted jacobian from the
227      * a priori covariance.
228      * </p>
229      * @param aPrioriState a priori state to use
230      * @param aPrioriCovariance a priori covariance to use
231      * @param relativeSymmetryThreshold Cholesky decomposition threshold above which off-diagonal
232      * elements are considered too different and matrix not symmetric
233      * @param absolutePositivityThreshold Cholesky decomposition threshold below which diagonal
234      * elements are considered null and matrix not positive definite
235      * @return a new instance.
236      * @since 2.3
237      */
238     public SequentialGaussNewtonOptimizer withAPrioriData(final RealVector aPrioriState,
239                                                           final RealMatrix aPrioriCovariance,
240                                                           final double relativeSymmetryThreshold,
241                                                           final double absolutePositivityThreshold) {
242 
243         // we consider the a priori state and covariance come from a
244         // previous estimation with exactly one observation of each state
245         // component, so partials are the identity matrix, weight is the
246         // square root of inverse of covariance, and residuals are zero
247 
248         // create a fake weighted Jacobian
249         final RealMatrix jTj              = getDecomposer().decompose(aPrioriCovariance).getInverse();
250         final RealMatrix weightedJacobian = new CholeskyDecomposition(jTj,
251                                                                       relativeSymmetryThreshold,
252                                                                       absolutePositivityThreshold).getLT();
253 
254         // create fake zero residuals
255         final RealVector residuals        = MatrixUtils.createRealVector(aPrioriState.getDimension());
256 
257         // combine everything as an evaluation
258         final Evaluation fakeEvaluation   = new AbstractEvaluation(aPrioriState.getDimension()) {
259 
260             /** {@inheritDoc} */
261             @Override
262             public RealVector getResiduals() {
263                 return residuals;
264             }
265 
266             /** {@inheritDoc} */
267             @Override
268             public RealVector getPoint() {
269                 return aPrioriState;
270             }
271 
272             /** {@inheritDoc} */
273             @Override
274             public RealMatrix getJacobian() {
275                 return weightedJacobian;
276             }
277         };
278 
279         return withEvaluation(fakeEvaluation);
280 
281     }
282 
283     /** {@inheritDoc} */
284     @Override
285     public Optimum optimize(final LeastSquaresProblem lsp) {
286         // create local evaluation and iteration counts
287         final Incrementor evaluationCounter = lsp.getEvaluationCounter();
288         final Incrementor iterationCounter = lsp.getIterationCounter();
289         final ConvergenceChecker<Evaluation> checker =
290             lsp.getConvergenceChecker();
291 
292         // Computation will be useless without a checker (see "for-loop").
293         if (checker == null) {
294             throw new NullArgumentException();
295         }
296 
297         RealVector currentPoint = lsp.getStart();
298 
299         if (oldEvaluation != null &&
300             currentPoint.getDimension() != oldEvaluation.getPoint().getDimension()) {
301             throw new MathIllegalStateException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
302                       currentPoint.getDimension(), oldEvaluation.getPoint().getDimension());
303         }
304 
305         // iterate until convergence is reached
306         Evaluation current = null;
307         while (true) {
308             iterationCounter.increment();
309 
310             // evaluate the objective function and its jacobian
311             final Evaluation previous = current;
312 
313             // Value of the objective function at "currentPoint".
314             evaluationCounter.increment();
315             current = lsp.evaluate(currentPoint);
316             final RealVector currentResiduals = current.getResiduals();
317             final RealMatrix weightedJacobian = current.getJacobian();
318 
319             currentPoint = current.getPoint();
320 
321             // Check convergence.
322             if (previous != null &&
323                 checker.converged(iterationCounter.getCount(), previous,
324                                   current)) {
325                 // combine old and new evaluations
326                 final Evaluation combinedEvaluation = oldEvaluation == null ?
327                                                       current :
328                                                       new CombinedEvaluation(oldEvaluation, current);
329                 return Optimum.of(combinedEvaluation, evaluationCounter.getCount(),
330                                   iterationCounter.getCount());
331             }
332 
333            // solve the linearized least squares problem
334             final RealMatrix lhs; // left hand side
335             final RealVector rhs; // right hand side
336             if (this.formNormalEquations) {
337                 final Pair<RealMatrix, RealVector> normalEquation =
338                                 computeNormalMatrix(weightedJacobian, currentResiduals);
339 
340                 lhs = oldLhs == null ?
341                       normalEquation.getFirst() :
342                       normalEquation.getFirst().add(oldLhs); // left hand side
343                 rhs = oldRhs == null ?
344                       normalEquation.getSecond() :
345                       normalEquation.getSecond().add(oldRhs); // right hand side
346             } else {
347                 lhs = oldLhs == null ?
348                       weightedJacobian :
349                       combineJacobians(oldLhs, weightedJacobian);
350                 rhs = oldRhs == null ?
351                       currentResiduals :
352                       combineResiduals(oldRhs, currentResiduals);
353             }
354 
355             final RealVector dX;
356             try {
357                 dX = this.decomposer.decompose(lhs).solve(rhs);
358             } catch (MathIllegalArgumentException e) {
359                 // change exception message
360                 throw new MathIllegalStateException(LocalizedOptimFormats.UNABLE_TO_SOLVE_SINGULAR_PROBLEM,
361                                                     e);
362             }
363             // update the estimated parameters
364             currentPoint = currentPoint.add(dX);
365 
366         }
367     }
368 
369     /** {@inheritDoc} */
370     @Override
371     public String toString() {
372         return "SequentialGaussNewtonOptimizer{" +
373                "decomposer=" + decomposer + '}';
374     }
375 
376     /**
377      * Compute the normal matrix, J<sup>T</sup>J.
378      *
379      * @param jacobian  the m by n jacobian matrix, J. Input.
380      * @param residuals the m by 1 residual vector, r. Input.
381      * @return  the n by n normal matrix and the n by 1 J<sup>Tr</sup> vector.
382      */
383     private static Pair<RealMatrix, RealVector>
384         computeNormalMatrix(final RealMatrix jacobian,
385                             final RealVector residuals) {
386         // since the normal matrix is symmetric, we only need to compute half of
387         // it.
388         final int nR = jacobian.getRowDimension();
389         final int nC = jacobian.getColumnDimension();
390         // allocate space for return values
391         final RealMatrix normal = MatrixUtils.createRealMatrix(nC, nC);
392         final RealVector jTr = new ArrayRealVector(nC);
393         // for each measurement
394         for (int i = 0; i < nR; ++i) {
395             // compute JTr for measurement i
396             for (int j = 0; j < nC; j++) {
397                 jTr.setEntry(j,
398                              jTr.getEntry(j) +
399                                 residuals.getEntry(i) *
400                                                jacobian.getEntry(i, j));
401             }
402 
403             // add the the contribution to the normal matrix for measurement i
404             for (int k = 0; k < nC; ++k) {
405                 // only compute the upper triangular part
406                 for (int l = k; l < nC; ++l) {
407                     normal
408                         .setEntry(k, l,
409                                   normal.getEntry(k,
410                                                   l) +
411                                         jacobian.getEntry(i, k) *
412                                                        jacobian.getEntry(i, l));
413                 }
414             }
415         }
416         // copy the upper triangular part to the lower triangular part.
417         for (int i = 0; i < nC; i++) {
418             for (int j = 0; j < i; j++) {
419                 normal.setEntry(i, j, normal.getEntry(j, i));
420             }
421         }
422         return new Pair<RealMatrix, RealVector>(normal, jTr);
423     }
424 
425     /** Combine Jacobian matrices
426      * @param oldJacobian old Jacobian matrix
427      * @param newJacobian new Jacobian matrix
428      * @return combined Jacobian matrix
429      */
430     private static RealMatrix combineJacobians(final RealMatrix oldJacobian,
431                                                final RealMatrix newJacobian) {
432         final int oldRowDimension    = oldJacobian.getRowDimension();
433         final int oldColumnDimension = oldJacobian.getColumnDimension();
434         final RealMatrix jacobian =
435                         MatrixUtils.createRealMatrix(oldRowDimension + newJacobian.getRowDimension(),
436                                                      oldColumnDimension);
437         jacobian.setSubMatrix(oldJacobian.getData(), 0,               0);
438         jacobian.setSubMatrix(newJacobian.getData(), oldRowDimension, 0);
439         return jacobian;
440     }
441 
442     /** Combine residuals vectors
443      * @param oldResiduals old residuals vector
444      * @param newResiduals new residuals vector
445      * @return combined residuals vector
446      */
447     private static RealVector combineResiduals(final RealVector oldResiduals,
448                                                final RealVector newResiduals) {
449         return oldResiduals.append(newResiduals);
450     }
451 
452     /**
453      * Container with an old and a new evaluation and combine both of them
454      */
455     private static class CombinedEvaluation extends AbstractEvaluation {
456 
457         /** Point of evaluation. */
458         private final RealVector point;
459 
460         /** Derivative at point. */
461         private final RealMatrix jacobian;
462 
463         /** Computed residuals. */
464         private final RealVector residuals;
465 
466         /**
467          * Create an {@link Evaluation} with no weights.
468          *
469          * @param oldEvaluation the old evaluation.
470          * @param newEvaluation the new evaluation
471          */
472         private CombinedEvaluation(final Evaluation oldEvaluation,
473                                    final Evaluation newEvaluation) {
474 
475             super(oldEvaluation.getResiduals().getDimension() +
476                   newEvaluation.getResiduals().getDimension());
477 
478             this.point    = newEvaluation.getPoint();
479             this.jacobian = combineJacobians(oldEvaluation.getJacobian(),
480                                              newEvaluation.getJacobian());
481             this.residuals = combineResiduals(oldEvaluation.getResiduals(),
482                                               newEvaluation.getResiduals());
483         }
484 
485         /** {@inheritDoc} */
486         @Override
487         public RealMatrix getJacobian() {
488             return jacobian;
489         }
490 
491         /** {@inheritDoc} */
492         @Override
493         public RealVector getPoint() {
494             return point;
495         }
496 
497         /** {@inheritDoc} */
498         @Override
499         public RealVector getResiduals() {
500             return residuals;
501         }
502 
503     }
504 
505 }