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