1dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond/* 2dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Licensed to the Apache Software Foundation (ASF) under one or more 3dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * contributor license agreements. See the NOTICE file distributed with 4dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * this work for additional information regarding copyright ownership. 5dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * The ASF licenses this file to You under the Apache License, Version 2.0 6dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * (the "License"); you may not use this file except in compliance with 7dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * the License. You may obtain a copy of the License at 8dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 9dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * http://www.apache.org/licenses/LICENSE-2.0 10dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 11dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Unless required by applicable law or agreed to in writing, software 12dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * distributed under the License is distributed on an "AS IS" BASIS, 13dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * See the License for the specific language governing permissions and 15dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * limitations under the License. 16dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 17dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondpackage org.apache.commons.math.optimization.general; 18dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 19dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondimport java.util.Arrays; 20dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 21dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondimport org.apache.commons.math.FunctionEvaluationException; 22dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondimport org.apache.commons.math.exception.util.LocalizedFormats; 23dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondimport org.apache.commons.math.optimization.OptimizationException; 24dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondimport org.apache.commons.math.optimization.VectorialPointValuePair; 25dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondimport org.apache.commons.math.util.FastMath; 26dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondimport org.apache.commons.math.util.MathUtils; 27dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 28dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 29dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond/** 30dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * This class solves a least squares problem using the Levenberg-Marquardt algorithm. 31dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 32dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This implementation <em>should</em> work even for over-determined systems 33dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * (i.e. systems having more point than equations). Over-determined systems 34dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * are solved by ignoring the point which have the smallest impact according 35dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * to their jacobian column norm. Only the rank of the matrix and some loop bounds 36dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * are changed to implement this.</p> 37dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 38dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>The resolution engine is a simple translation of the MINPACK <a 39dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * href="http://www.netlib.org/minpack/lmder.f">lmder</a> routine with minor 40dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * changes. The changes include the over-determined resolution, the use of 41dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * inherited convergence checker and the Q.R. decomposition which has been 42dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * rewritten following the algorithm described in the 43dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * P. Lascaux and R. Theodor book <i>Analyse numérique matricielle 44dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * appliquée à l'art de l'ingénieur</i>, Masson 1986.</p> 45dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>The authors of the original fortran version are: 46dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <ul> 47dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Argonne National Laboratory. MINPACK project. March 1980</li> 48dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Burton S. Garbow</li> 49dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Kenneth E. Hillstrom</li> 50dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Jorge J. More</li> 51dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </ul> 52dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * The redistribution policy for MINPACK is available <a 53dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * href="http://www.netlib.org/minpack/disclaimer">here</a>, for convenience, it 54dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * is reproduced below.</p> 55dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 56dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <table border="0" width="80%" cellpadding="10" align="center" bgcolor="#E0E0E0"> 57dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <tr><td> 58dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Minpack Copyright Notice (1999) University of Chicago. 59dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * All rights reserved 60dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </td></tr> 61dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <tr><td> 62dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Redistribution and use in source and binary forms, with or without 63dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * modification, are permitted provided that the following conditions 64dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * are met: 65dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <ol> 66dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Redistributions of source code must retain the above copyright 67dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * notice, this list of conditions and the following disclaimer.</li> 68dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Redistributions in binary form must reproduce the above 69dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * copyright notice, this list of conditions and the following 70dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * disclaimer in the documentation and/or other materials provided 71dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * with the distribution.</li> 72dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>The end-user documentation included with the redistribution, if any, 73dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * must include the following acknowledgment: 74dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <code>This product includes software developed by the University of 75dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Chicago, as Operator of Argonne National Laboratory.</code> 76dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Alternately, this acknowledgment may appear in the software itself, 77dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * if and wherever such third-party acknowledgments normally appear.</li> 78dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li><strong>WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" 79dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE 80dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND 81dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR 82dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES 83dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE 84dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY 85dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR 86dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF 87dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) 88dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION 89dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL 90dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * BE CORRECTED.</strong></li> 91dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li><strong>LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT 92dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF 93dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, 94dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF 95dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF 96dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER 97dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT 98dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * (INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, 99dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE 100dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * POSSIBILITY OF SUCH LOSS OR DAMAGES.</strong></li> 101dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <ol></td></tr> 102dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </table> 103dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @version $Revision: 1073272 $ $Date: 2011-02-22 10:22:25 +0100 (mar. 22 févr. 2011) $ 104dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @since 2.0 105dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 106dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 107dee0849a9704d532af0b550146cbafbaa6ee1d19Raymondpublic class LevenbergMarquardtOptimizer extends AbstractLeastSquaresOptimizer { 108dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 109dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Number of solved point. */ 110dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private int solvedCols; 111dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 112dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Diagonal elements of the R matrix in the Q.R. decomposition. */ 113dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double[] diagR; 114dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 115dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Norms of the columns of the jacobian matrix. */ 116dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double[] jacNorm; 117dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 118dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Coefficients of the Householder transforms vectors. */ 119dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double[] beta; 120dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 121dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Columns permutation array. */ 122dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private int[] permutation; 123dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 124dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Rank of the jacobian matrix. */ 125dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private int rank; 126dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 127dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Levenberg-Marquardt parameter. */ 128dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double lmPar; 129dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 130dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Parameters evolution direction associated with lmPar. */ 131dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double[] lmDir; 132dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 133dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Positive input variable used in determining the initial step bound. */ 134dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double initialStepBoundFactor; 135dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 136dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Desired relative error in the sum of squares. */ 137dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double costRelativeTolerance; 138dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 139dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Desired relative error in the approximate solution parameters. */ 140dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double parRelativeTolerance; 141dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 142dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Desired max cosine on the orthogonality between the function vector 143dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * and the columns of the jacobian. */ 144dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double orthoTolerance; 145dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 146dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** Threshold for QR ranking. */ 147dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private double qrRankingThreshold; 148dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 149dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 150dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Build an optimizer for least squares problems. 151dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>The default values for the algorithm settings are: 152dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <ul> 153dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>{@link #setConvergenceChecker(VectorialConvergenceChecker) vectorial convergence checker}: null</li> 154dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>{@link #setInitialStepBoundFactor(double) initial step bound factor}: 100.0</li> 155dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>{@link #setMaxIterations(int) maximal iterations}: 1000</li> 156dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>{@link #setCostRelativeTolerance(double) cost relative tolerance}: 1.0e-10</li> 157dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>{@link #setParRelativeTolerance(double) parameters relative tolerance}: 1.0e-10</li> 158dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>{@link #setOrthoTolerance(double) orthogonality tolerance}: 1.0e-10</li> 159dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>{@link #setQRRankingThreshold(double) QR ranking threshold}: {@link MathUtils#SAFE_MIN}</li> 160dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </ul> 161dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </p> 162dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>These default values may be overridden after construction. If the {@link 163dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * #setConvergenceChecker vectorial convergence checker} is set to a non-null value, it 164dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * will be used instead of the {@link #setCostRelativeTolerance cost relative tolerance} 165dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * and {@link #setParRelativeTolerance parameters relative tolerance} settings. 166dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 167dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond public LevenbergMarquardtOptimizer() { 168dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 169dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // set up the superclass with a default max cost evaluations setting 170dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond setMaxIterations(1000); 171dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 172dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // default values for the tuning parameters 173dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond setConvergenceChecker(null); 174dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond setInitialStepBoundFactor(100.0); 175dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond setCostRelativeTolerance(1.0e-10); 176dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond setParRelativeTolerance(1.0e-10); 177dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond setOrthoTolerance(1.0e-10); 178dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond setQRRankingThreshold(MathUtils.SAFE_MIN); 179dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 180dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 181dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 182dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 183dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Set the positive input variable used in determining the initial step bound. 184dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * This bound is set to the product of initialStepBoundFactor and the euclidean 185dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * norm of diag*x if nonzero, or else to initialStepBoundFactor itself. In most 186dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * cases factor should lie in the interval (0.1, 100.0). 100.0 is a generally 187dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * recommended value. 188dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 189dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param initialStepBoundFactor initial step bound factor 190dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 191dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond public void setInitialStepBoundFactor(double initialStepBoundFactor) { 192dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond this.initialStepBoundFactor = initialStepBoundFactor; 193dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 194dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 195dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 196dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Set the desired relative error in the sum of squares. 197dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This setting is used only if the {@link #setConvergenceChecker vectorial 198dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * convergence checker} is set to null.</p> 199dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param costRelativeTolerance desired relative error in the sum of squares 200dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 201dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond public void setCostRelativeTolerance(double costRelativeTolerance) { 202dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond this.costRelativeTolerance = costRelativeTolerance; 203dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 204dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 205dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 206dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Set the desired relative error in the approximate solution parameters. 207dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This setting is used only if the {@link #setConvergenceChecker vectorial 208dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * convergence checker} is set to null.</p> 209dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param parRelativeTolerance desired relative error 210dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * in the approximate solution parameters 211dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 212dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond public void setParRelativeTolerance(double parRelativeTolerance) { 213dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond this.parRelativeTolerance = parRelativeTolerance; 214dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 215dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 216dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 217dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Set the desired max cosine on the orthogonality. 218dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This setting is always used, regardless of the {@link #setConvergenceChecker 219dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * vectorial convergence checker} being null or non-null.</p> 220dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param orthoTolerance desired max cosine on the orthogonality 221dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * between the function vector and the columns of the jacobian 222dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 223dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond public void setOrthoTolerance(double orthoTolerance) { 224dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond this.orthoTolerance = orthoTolerance; 225dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 226dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 227dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 228dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Set the desired threshold for QR ranking. 229dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p> 230dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * If the squared norm of a column vector is smaller or equal to this threshold 231dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * during QR decomposition, it is considered to be a zero vector and hence the 232dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * rank of the matrix is reduced. 233dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </p> 234dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param threshold threshold for QR ranking 235dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @since 2.2 236dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 237dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond public void setQRRankingThreshold(final double threshold) { 238dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond this.qrRankingThreshold = threshold; 239dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 240dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 241dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** {@inheritDoc} */ 242dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond @Override 243dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond protected VectorialPointValuePair doOptimize() 244dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond throws FunctionEvaluationException, OptimizationException, IllegalArgumentException { 245dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 246dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // arrays shared with the other private methods 247dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond solvedCols = Math.min(rows, cols); 248dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond diagR = new double[cols]; 249dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond jacNorm = new double[cols]; 250dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond beta = new double[cols]; 251dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond permutation = new int[cols]; 252dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir = new double[cols]; 253dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 254dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // local point 255dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double delta = 0; 256dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double xNorm = 0; 257dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] diag = new double[cols]; 258dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] oldX = new double[cols]; 259dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] oldRes = new double[rows]; 260dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] oldObj = new double[rows]; 261dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] qtf = new double[rows]; 262dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] work1 = new double[cols]; 263dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] work2 = new double[cols]; 264dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] work3 = new double[cols]; 265dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 266dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // evaluate the function at the starting point and calculate its norm 267dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond updateResidualsAndCost(); 268dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 269dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // outer loop 270dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar = 0; 271dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond boolean firstIteration = true; 272dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond VectorialPointValuePair current = new VectorialPointValuePair(point, objective); 273dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond while (true) { 274dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i=0;i<rows;i++) { 275dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond qtf[i]=wresiduals[i]; 276dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 277dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond incrementIterationsCounter(); 278dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 279dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute the Q.R. decomposition of the jacobian matrix 280dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond VectorialPointValuePair previous = current; 281dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond updateJacobian(); 282dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond qrDecomposition(); 283dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 284dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute Qt.res 285dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond qTy(qtf); 286dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // now we don't need Q anymore, 287dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // so let jacobian contain the R matrix with its diagonal elements 288dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = 0; k < solvedCols; ++k) { 289dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pk = permutation[k]; 290dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond wjacobian[k][pk] = diagR[pk]; 291dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 292dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 293dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (firstIteration) { 294dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 295dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // scale the point according to the norms of the columns 296dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // of the initial jacobian 297dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond xNorm = 0; 298dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = 0; k < cols; ++k) { 299dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double dk = jacNorm[k]; 300dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (dk == 0) { 301dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond dk = 1.0; 302dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 303dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double xk = dk * point[k]; 304dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond xNorm += xk * xk; 305dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond diag[k] = dk; 306dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 307dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond xNorm = FastMath.sqrt(xNorm); 308dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 309dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // initialize the step bound delta 310dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond delta = (xNorm == 0) ? initialStepBoundFactor : (initialStepBoundFactor * xNorm); 311dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 312dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 313dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 314dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // check orthogonality between function vector and jacobian columns 315dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double maxCosine = 0; 316dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (cost != 0) { 317dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 318dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 319dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double s = jacNorm[pj]; 320dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (s != 0) { 321dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double sum = 0; 322dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = 0; i <= j; ++i) { 323dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum += wjacobian[i][pj] * qtf[i]; 324dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 325dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond maxCosine = FastMath.max(maxCosine, FastMath.abs(sum) / (s * cost)); 326dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 327dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 328dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 329dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (maxCosine <= orthoTolerance) { 330dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // convergence has been reached 331dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond updateResidualsAndCost(); 332dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond current = new VectorialPointValuePair(point, objective); 333dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond return current; 334dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 335dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 336dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // rescale if necessary 337dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < cols; ++j) { 338dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond diag[j] = FastMath.max(diag[j], jacNorm[j]); 339dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 340dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 341dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // inner loop 342dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (double ratio = 0; ratio < 1.0e-4;) { 343dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 344dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // save the state 345dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 346dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 347dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond oldX[pj] = point[pj]; 348dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 349dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double previousCost = cost; 350dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] tmpVec = residuals; 351dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond residuals = oldRes; 352dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond oldRes = tmpVec; 353dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond tmpVec = objective; 354dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond objective = oldObj; 355dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond oldObj = tmpVec; 356dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 357dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // determine the Levenberg-Marquardt parameter 358dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond determineLMParameter(qtf, delta, diag, work1, work2, work3); 359dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 360dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute the new point and the norm of the evolution direction 361dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double lmNorm = 0; 362dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 363dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 364dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir[pj] = -lmDir[pj]; 365dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond point[pj] = oldX[pj] + lmDir[pj]; 366dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double s = diag[pj] * lmDir[pj]; 367dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmNorm += s * s; 368dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 369dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmNorm = FastMath.sqrt(lmNorm); 370dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // on the first iteration, adjust the initial step bound. 371dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (firstIteration) { 372dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond delta = FastMath.min(delta, lmNorm); 373dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 374dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 375dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // evaluate the function at x + p and calculate its norm 376dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond updateResidualsAndCost(); 377dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 378dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute the scaled actual reduction 379dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double actRed = -1.0; 380dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (0.1 * cost < previousCost) { 381dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double r = cost / previousCost; 382dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond actRed = 1.0 - r * r; 383dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 384dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 385dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute the scaled predicted reduction 386dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // and the scaled directional derivative 387dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 388dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 389dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double dirJ = lmDir[pj]; 390dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[j] = 0; 391dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = 0; i <= j; ++i) { 392dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[i] += wjacobian[i][pj] * dirJ; 393dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 394dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 395dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double coeff1 = 0; 396dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 397dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond coeff1 += work1[j] * work1[j]; 398dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 399dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double pc2 = previousCost * previousCost; 400dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond coeff1 = coeff1 / pc2; 401dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double coeff2 = lmPar * lmNorm * lmNorm / pc2; 402dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double preRed = coeff1 + 2 * coeff2; 403dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double dirDer = -(coeff1 + coeff2); 404dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 405dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // ratio of the actual to the predicted reduction 406dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond ratio = (preRed == 0) ? 0 : (actRed / preRed); 407dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 408dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // update the step bound 409dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (ratio <= 0.25) { 410dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double tmp = 411dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond (actRed < 0) ? (0.5 * dirDer / (dirDer + 0.5 * actRed)) : 0.5; 412dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if ((0.1 * cost >= previousCost) || (tmp < 0.1)) { 413dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond tmp = 0.1; 414dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 415dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond delta = tmp * FastMath.min(delta, 10.0 * lmNorm); 416dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar /= tmp; 417dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } else if ((lmPar == 0) || (ratio >= 0.75)) { 418dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond delta = 2 * lmNorm; 419dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar *= 0.5; 420dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 421dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 422dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // test for successful iteration. 423dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (ratio >= 1.0e-4) { 424dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // successful iteration, update the norm 425dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond firstIteration = false; 426dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond xNorm = 0; 427dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = 0; k < cols; ++k) { 428dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double xK = diag[k] * point[k]; 429dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond xNorm += xK * xK; 430dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 431dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond xNorm = FastMath.sqrt(xNorm); 432dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond current = new VectorialPointValuePair(point, objective); 433dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 434dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // tests for convergence. 435dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (checker != null) { 436dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // we use the vectorial convergence checker 437dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (checker.converged(getIterations(), previous, current)) { 438dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond return current; 439dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 440dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 441dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } else { 442dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // failed iteration, reset the previous values 443dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond cost = previousCost; 444dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 445dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 446dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond point[pj] = oldX[pj]; 447dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 448dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond tmpVec = residuals; 449dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond residuals = oldRes; 450dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond oldRes = tmpVec; 451dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond tmpVec = objective; 452dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond objective = oldObj; 453dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond oldObj = tmpVec; 454dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 455dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (checker==null) { 456dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (((FastMath.abs(actRed) <= costRelativeTolerance) && 457dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond (preRed <= costRelativeTolerance) && 458dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond (ratio <= 2.0)) || 459dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond (delta <= parRelativeTolerance * xNorm)) { 460dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond return current; 461dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 462dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 463dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // tests for termination and stringent tolerances 464dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // (2.2204e-16 is the machine epsilon for IEEE754) 465dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if ((FastMath.abs(actRed) <= 2.2204e-16) && (preRed <= 2.2204e-16) && (ratio <= 2.0)) { 466dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond throw new OptimizationException(LocalizedFormats.TOO_SMALL_COST_RELATIVE_TOLERANCE, 467dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond costRelativeTolerance); 468dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } else if (delta <= 2.2204e-16 * xNorm) { 469dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond throw new OptimizationException(LocalizedFormats.TOO_SMALL_PARAMETERS_RELATIVE_TOLERANCE, 470dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond parRelativeTolerance); 471dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } else if (maxCosine <= 2.2204e-16) { 472dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond throw new OptimizationException(LocalizedFormats.TOO_SMALL_ORTHOGONALITY_TOLERANCE, 473dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond orthoTolerance); 474dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 475dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 476dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 477dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 478dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 479dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 480dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 481dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 482dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 483dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Determine the Levenberg-Marquardt parameter. 484dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This implementation is a translation in Java of the MINPACK 485dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <a href="http://www.netlib.org/minpack/lmpar.f">lmpar</a> 486dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * routine.</p> 487dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This method sets the lmPar and lmDir attributes.</p> 488dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>The authors of the original fortran function are:</p> 489dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <ul> 490dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Argonne National Laboratory. MINPACK project. March 1980</li> 491dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Burton S. Garbow</li> 492dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Kenneth E. Hillstrom</li> 493dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Jorge J. More</li> 494dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </ul> 495dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>Luc Maisonobe did the Java translation.</p> 496dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 497dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param qy array containing qTy 498dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param delta upper bound on the euclidean norm of diagR * lmDir 499dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param diag diagonal matrix 500dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param work1 work array 501dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param work2 work array 502dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param work3 work array 503dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 504dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private void determineLMParameter(double[] qy, double delta, double[] diag, 505dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] work1, double[] work2, double[] work3) { 506dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 507dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute and store in x the gauss-newton direction, if the 508dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // jacobian is rank-deficient, obtain a least squares solution 509dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < rank; ++j) { 510dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir[permutation[j]] = qy[j]; 511dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 512dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = rank; j < cols; ++j) { 513dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir[permutation[j]] = 0; 514dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 515dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = rank - 1; k >= 0; --k) { 516dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pk = permutation[k]; 517dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double ypk = lmDir[pk] / diagR[pk]; 518dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = 0; i < k; ++i) { 519dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir[permutation[i]] -= ypk * wjacobian[i][pk]; 520dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 521dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir[pk] = ypk; 522dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 523dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 524dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // evaluate the function at the origin, and test 525dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // for acceptance of the Gauss-Newton direction 526dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double dxNorm = 0; 527dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 528dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 529dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double s = diag[pj] * lmDir[pj]; 530dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[pj] = s; 531dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond dxNorm += s * s; 532dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 533dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond dxNorm = FastMath.sqrt(dxNorm); 534dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double fp = dxNorm - delta; 535dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (fp <= 0.1 * delta) { 536dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar = 0; 537dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond return; 538dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 539dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 540dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // if the jacobian is not rank deficient, the Newton step provides 541dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // a lower bound, parl, for the zero of the function, 542dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // otherwise set this bound to zero 543dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double sum2; 544dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double parl = 0; 545dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (rank == solvedCols) { 546dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 547dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 548dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[pj] *= diag[pj] / dxNorm; 549dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 550dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum2 = 0; 551dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 552dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 553dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double sum = 0; 554dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = 0; i < j; ++i) { 555dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum += wjacobian[i][pj] * work1[permutation[i]]; 556dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 557dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double s = (work1[pj] - sum) / diagR[pj]; 558dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[pj] = s; 559dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum2 += s * s; 560dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 561dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond parl = fp / (delta * sum2); 562dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 563dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 564dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // calculate an upper bound, paru, for the zero of the function 565dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum2 = 0; 566dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 567dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 568dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double sum = 0; 569dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = 0; i <= j; ++i) { 570dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum += wjacobian[i][pj] * qy[i]; 571dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 572dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum /= diag[pj]; 573dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum2 += sum * sum; 574dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 575dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double gNorm = FastMath.sqrt(sum2); 576dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double paru = gNorm / delta; 577dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (paru == 0) { 578dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // 2.2251e-308 is the smallest positive real for IEE754 579dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond paru = 2.2251e-308 / FastMath.min(delta, 0.1); 580dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 581dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 582dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // if the input par lies outside of the interval (parl,paru), 583dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // set par to the closer endpoint 584dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar = FastMath.min(paru, FastMath.max(lmPar, parl)); 585dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (lmPar == 0) { 586dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar = gNorm / dxNorm; 587dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 588dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 589dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int countdown = 10; countdown >= 0; --countdown) { 590dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 591dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // evaluate the function at the current value of lmPar 592dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (lmPar == 0) { 593dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar = FastMath.max(2.2251e-308, 0.001 * paru); 594dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 595dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double sPar = FastMath.sqrt(lmPar); 596dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 597dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 598dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[pj] = sPar * diag[pj]; 599dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 600dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond determineLMDirection(qy, work1, work2, work3); 601dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 602dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond dxNorm = 0; 603dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 604dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 605dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double s = diag[pj] * lmDir[pj]; 606dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work3[pj] = s; 607dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond dxNorm += s * s; 608dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 609dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond dxNorm = FastMath.sqrt(dxNorm); 610dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double previousFP = fp; 611dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond fp = dxNorm - delta; 612dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 613dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // if the function is small enough, accept the current value 614dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // of lmPar, also test for the exceptional cases where parl is zero 615dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if ((FastMath.abs(fp) <= 0.1 * delta) || 616dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond ((parl == 0) && (fp <= previousFP) && (previousFP < 0))) { 617dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond return; 618dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 619dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 620dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute the Newton correction 621dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 622dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 623dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[pj] = work3[pj] * diag[pj] / dxNorm; 624dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 625dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 626dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 627dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[pj] /= work2[j]; 628dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double tmp = work1[pj]; 629dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = j + 1; i < solvedCols; ++i) { 630dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work1[permutation[i]] -= wjacobian[i][pj] * tmp; 631dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 632dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 633dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum2 = 0; 634dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 635dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double s = work1[permutation[j]]; 636dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum2 += s * s; 637dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 638dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double correction = fp / (delta * sum2); 639dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 640dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // depending on the sign of the function, update parl or paru. 641dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (fp > 0) { 642dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond parl = FastMath.max(parl, lmPar); 643dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } else if (fp < 0) { 644dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond paru = FastMath.min(paru, lmPar); 645dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 646dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 647dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute an improved estimate for lmPar 648dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmPar = FastMath.max(parl, lmPar + correction); 649dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 650dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 651dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 652dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 653dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 654dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Solve a*x = b and d*x = 0 in the least squares sense. 655dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This implementation is a translation in Java of the MINPACK 656dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <a href="http://www.netlib.org/minpack/qrsolv.f">qrsolv</a> 657dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * routine.</p> 658dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This method sets the lmDir and lmDiag attributes.</p> 659dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>The authors of the original fortran function are:</p> 660dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <ul> 661dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Argonne National Laboratory. MINPACK project. March 1980</li> 662dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Burton S. Garbow</li> 663dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Kenneth E. Hillstrom</li> 664dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <li>Jorge J. More</li> 665dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </ul> 666dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>Luc Maisonobe did the Java translation.</p> 667dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 668dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param qy array containing qTy 669dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param diag diagonal matrix 670dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param lmDiag diagonal elements associated with lmDir 671dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param work work array 672dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 673dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private void determineLMDirection(double[] qy, double[] diag, 674dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double[] lmDiag, double[] work) { 675dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 676dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // copy R and Qty to preserve input and initialize s 677dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // in particular, save the diagonal elements of R in lmDir 678dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 679dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 680dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = j + 1; i < solvedCols; ++i) { 681dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond wjacobian[i][pj] = wjacobian[j][permutation[i]]; 682dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 683dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir[j] = diagR[pj]; 684dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work[j] = qy[j]; 685dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 686dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 687dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // eliminate the diagonal matrix d using a Givens rotation 688dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 689dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 690dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // prepare the row of d to be eliminated, locating the 691dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // diagonal element using p from the Q.R. factorization 692dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 693dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double dpj = diag[pj]; 694dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (dpj != 0) { 695dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond Arrays.fill(lmDiag, j + 1, lmDiag.length, 0); 696dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 697dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDiag[j] = dpj; 698dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 699dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // the transformations to eliminate the row of d 700dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // modify only a single element of Qty 701dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // beyond the first n, which is initially zero. 702dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double qtbpj = 0; 703dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = j; k < solvedCols; ++k) { 704dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pk = permutation[k]; 705dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 706dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // determine a Givens rotation which eliminates the 707dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // appropriate element in the current row of d 708dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (lmDiag[k] != 0) { 709dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 710dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond final double sin; 711dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond final double cos; 712dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double rkk = wjacobian[k][pk]; 713dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (FastMath.abs(rkk) < FastMath.abs(lmDiag[k])) { 714dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond final double cotan = rkk / lmDiag[k]; 715dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sin = 1.0 / FastMath.sqrt(1.0 + cotan * cotan); 716dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond cos = sin * cotan; 717dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } else { 718dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond final double tan = lmDiag[k] / rkk; 719dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond cos = 1.0 / FastMath.sqrt(1.0 + tan * tan); 720dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sin = cos * tan; 721dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 722dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 723dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // compute the modified diagonal element of R and 724dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // the modified element of (Qty,0) 725dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond wjacobian[k][pk] = cos * rkk + sin * lmDiag[k]; 726dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond final double temp = cos * work[k] + sin * qtbpj; 727dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond qtbpj = -sin * work[k] + cos * qtbpj; 728dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work[k] = temp; 729dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 730dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // accumulate the tranformation in the row of s 731dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = k + 1; i < solvedCols; ++i) { 732dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double rik = wjacobian[i][pk]; 733dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond final double temp2 = cos * rik + sin * lmDiag[i]; 734dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDiag[i] = -sin * rik + cos * lmDiag[i]; 735dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond wjacobian[i][pk] = temp2; 736dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 737dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 738dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 739dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 740dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 741dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // store the diagonal element of s and restore 742dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // the corresponding diagonal element of R 743dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDiag[j] = wjacobian[j][permutation[j]]; 744dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond wjacobian[j][permutation[j]] = lmDir[j]; 745dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 746dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 747dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 748dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // solve the triangular system for z, if the system is 749dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // singular, then obtain a least squares solution 750dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int nSing = solvedCols; 751dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < solvedCols; ++j) { 752dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if ((lmDiag[j] == 0) && (nSing == solvedCols)) { 753dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond nSing = j; 754dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 755dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (nSing < solvedCols) { 756dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work[j] = 0; 757dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 758dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 759dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (nSing > 0) { 760dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = nSing - 1; j >= 0; --j) { 761dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pj = permutation[j]; 762dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double sum = 0; 763dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = j + 1; i < nSing; ++i) { 764dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond sum += wjacobian[i][pj] * work[i]; 765dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 766dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond work[j] = (work[j] - sum) / lmDiag[j]; 767dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 768dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 769dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 770dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // permute the components of z back to components of lmDir 771dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = 0; j < lmDir.length; ++j) { 772dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond lmDir[permutation[j]] = work[j]; 773dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 774dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 775dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 776dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 777dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 778dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Decompose a matrix A as A.P = Q.R using Householder transforms. 779dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>As suggested in the P. Lascaux and R. Theodor book 780dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <i>Analyse numérique matricielle appliquée à 781dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * l'art de l'ingénieur</i> (Masson, 1986), instead of representing 782dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * the Householder transforms with u<sub>k</sub> unit vectors such that: 783dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <pre> 784dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * H<sub>k</sub> = I - 2u<sub>k</sub>.u<sub>k</sub><sup>t</sup> 785dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </pre> 786dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * we use <sub>k</sub> non-unit vectors such that: 787dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <pre> 788dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * H<sub>k</sub> = I - beta<sub>k</sub>v<sub>k</sub>.v<sub>k</sub><sup>t</sup> 789dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * </pre> 790dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * where v<sub>k</sub> = a<sub>k</sub> - alpha<sub>k</sub> e<sub>k</sub>. 791dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * The beta<sub>k</sub> coefficients are provided upon exit as recomputing 792dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * them from the v<sub>k</sub> vectors would be costly.</p> 793dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * <p>This decomposition handles rank deficient cases since the tranformations 794dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * are performed in non-increasing columns norms order thanks to columns 795dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * pivoting. The diagonal elements of the R matrix are therefore also in 796dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * non-increasing absolute values order.</p> 797dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @exception OptimizationException if the decomposition cannot be performed 798dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 799dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private void qrDecomposition() throws OptimizationException { 800dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 801dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // initializations 802dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = 0; k < cols; ++k) { 803dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond permutation[k] = k; 804dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double norm2 = 0; 805dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = 0; i < wjacobian.length; ++i) { 806dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double akk = wjacobian[i][k]; 807dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond norm2 += akk * akk; 808dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 809dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond jacNorm[k] = FastMath.sqrt(norm2); 810dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 811dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 812dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // transform the matrix column after column 813dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = 0; k < cols; ++k) { 814dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 815dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // select the column with the greatest norm on active components 816dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int nextColumn = -1; 817dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double ak2 = Double.NEGATIVE_INFINITY; 818dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = k; i < cols; ++i) { 819dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double norm2 = 0; 820dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = k; j < wjacobian.length; ++j) { 821dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double aki = wjacobian[j][permutation[i]]; 822dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond norm2 += aki * aki; 823dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 824dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (Double.isInfinite(norm2) || Double.isNaN(norm2)) { 825dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond throw new OptimizationException(LocalizedFormats.UNABLE_TO_PERFORM_QR_DECOMPOSITION_ON_JACOBIAN, 826dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond rows, cols); 827dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 828dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (norm2 > ak2) { 829dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond nextColumn = i; 830dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond ak2 = norm2; 831dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 832dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 833dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond if (ak2 <= qrRankingThreshold) { 834dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond rank = k; 835dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond return; 836dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 837dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pk = permutation[nextColumn]; 838dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond permutation[nextColumn] = permutation[k]; 839dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond permutation[k] = pk; 840dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 841dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // choose alpha such that Hk.u = alpha ek 842dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double akk = wjacobian[k][pk]; 843dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double alpha = (akk > 0) ? -FastMath.sqrt(ak2) : FastMath.sqrt(ak2); 844dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double betak = 1.0 / (ak2 - akk * alpha); 845dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond beta[pk] = betak; 846dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 847dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // transform the current column 848dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond diagR[pk] = alpha; 849dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond wjacobian[k][pk] -= alpha; 850dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 851dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond // transform the remaining columns 852dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int dk = cols - 1 - k; dk > 0; --dk) { 853dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double gamma = 0; 854dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = k; j < wjacobian.length; ++j) { 855dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond gamma += wjacobian[j][pk] * wjacobian[j][permutation[k + dk]]; 856dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 857dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond gamma *= betak; 858dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int j = k; j < wjacobian.length; ++j) { 859dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond wjacobian[j][permutation[k + dk]] -= gamma * wjacobian[j][pk]; 860dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 861dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 862dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 863dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 864dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 865dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond rank = solvedCols; 866dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 867dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 868dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 869dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond /** 870dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * Compute the product Qt.y for some Q.R. decomposition. 871dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * 872dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond * @param y vector to multiply (will be overwritten with the result) 873dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond */ 874dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond private void qTy(double[] y) { 875dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int k = 0; k < cols; ++k) { 876dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond int pk = permutation[k]; 877dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond double gamma = 0; 878dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = k; i < rows; ++i) { 879dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond gamma += wjacobian[i][pk] * y[i]; 880dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 881dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond gamma *= beta[pk]; 882dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond for (int i = k; i < rows; ++i) { 883dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond y[i] -= gamma * wjacobian[i][pk]; 884dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 885dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 886dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond } 887dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond 888dee0849a9704d532af0b550146cbafbaa6ee1d19Raymond} 889