1 /**********************************************************
2 * Version $Id$
3 *********************************************************/
4
5 ///////////////////////////////////////////////////////////
6 // //
7 // SAGA //
8 // //
9 // System for Automated Geoscientific Analyses //
10 // //
11 // Tool Library //
12 // Table_Calculus //
13 // //
14 //-------------------------------------------------------//
15 // //
16 // LMFit.cpp //
17 // //
18 // Copyright (C) 2003 by //
19 // Andre Ringeler //
20 // //
21 //-------------------------------------------------------//
22 // //
23 // This file is part of 'SAGA - System for Automated //
24 // Geoscientific Analyses'. SAGA is free software; you //
25 // can redistribute it and/or modify it under the terms //
26 // of the GNU General Public License as published by the //
27 // Free Software Foundation, either version 2 of the //
28 // License, or (at your option) any later version. //
29 // //
30 // SAGA is distributed in the hope that it will be //
31 // useful, but WITHOUT ANY WARRANTY; without even the //
32 // implied warranty of MERCHANTABILITY or FITNESS FOR A //
33 // PARTICULAR PURPOSE. See the GNU General Public //
34 // License for more details. //
35 // //
36 // You should have received a copy of the GNU General //
37 // Public License along with this program; if not, see //
38 // <http://www.gnu.org/licenses/>. //
39 // //
40 //-------------------------------------------------------//
41 // //
42 // e-mail: aringel@gwdg.de //
43 // //
44 // contact: Andre Ringeler //
45 // Institute of Geography //
46 // University of Goettingen //
47 // Goldschmidtstr. 5 //
48 // 37077 Goettingen //
49 // Germany //
50 // //
51 ///////////////////////////////////////////////////////////
52
53 //---------------------------------------------------------
54
55 #include "LMFit.h"
56
TLMFit(vector<double> Xdata,vector<double> Ydata,vector<double> Param,void (* CfuncP)(double x,vector<double> ca,double & y,vector<double> & dyda,int na))57 TLMFit::TLMFit(vector<double> Xdata, vector<double> Ydata, vector<double> Param, void(*CfuncP)(double x, vector<double> ca, double &y, vector < double> &dyda, int na))
58 {
59 int i, mfit = 0;
60 chisq = 0;
61 alamda = -1;
62
63 ndata = Xdata.size();
64 nparam = Param.size();
65
66 x.resize(ndata);
67 y.resize(ndata);
68 for (i = 0; i < ndata; i++)
69 {
70 x[i] = Xdata[i];
71 y[i] = Ydata[i];
72 }
73
74 a.resize(nparam);
75 ia.resize(nparam);
76 for (i = 0; i < nparam; i++)
77 {
78 a[i] = Param[i];
79 ia[i] = 1;
80 if (ia[i])
81 mfit++;
82 }
83
84 alpha.resize(mfit);
85 covar.resize(mfit);
86 for (i = 0; i < mfit; i++)
87 {
88 covar[i].resize(mfit);
89 alpha[i].resize(mfit);
90 }
91
92 funcP = CfuncP;
93 }
94 //----------------------------------------------------------------------------
Fit(void)95 void TLMFit::Fit(void)
96 {
97 mrqmin();
98 }
99
gaussj(vector<vector<double>> & aa,int n,vector<vector<double>> & b,int m)100 void TLMFit::gaussj(vector< vector<double> > &aa, int n, vector< vector < double> > &b, int m)
101 {
102 vector < int> indxc, indxr, ipiv;
103 int i, icol, irow, j, k, l, ll;
104 double big, dum, pivinv, temp, test;
105
106 indxc.resize(n);
107 indxr.resize(n);
108 ipiv.resize(n);
109
110 for (j = 0; j < n; j++)
111 ipiv[j] = 0;
112 for (i = 0; i < n; i++)
113 {
114 big = 0.0;
115 for (j = 0; j < n; j++)
116 if (ipiv[j] != 1)
117 for (k = 0; k < n; k++)
118 {
119 if (ipiv[k] == 0)
120 {
121 if (fabs(aa[j][k]) >= big)
122 {
123 big = fabs(aa[j][k]);
124 irow = j;
125 icol = k;
126 }
127 }
128 else if (ipiv[k] > 1)
129 {
130 throw ESingularMatrix(1);
131 }
132 }
133 ++(ipiv[icol]);
134
135 if (irow != icol)
136 {
137 for (l = 0; l < n; l++)
138 SWAP(aa[irow][l], aa[icol][l]);
139 for (l = 0; l < m; l++)
140 SWAP(b[irow][l], b[icol][l]);
141 }
142 indxr[i] = irow;
143 indxc[i] = icol;
144 if (fabs(aa[icol][icol]) < 1E-300)
145 {
146 throw ESingularMatrix(2);
147 }
148 // else
149 test = aa[icol][icol];
150
151 pivinv = 1.0/test;
152 aa[icol][icol] = 1.0;
153 for (l = 0; l < n; l++)
154 aa[icol][l] *= pivinv;
155 for (l = 0; l < m; l++)
156 b[icol][l] *= pivinv;
157
158 for (ll = 0; ll < n; ll++)
159 if (ll != icol)
160 {
161 dum = aa[ll][icol];
162 aa[ll][icol] = 0.0;
163 for (l = 0; l < n; l++)
164 aa[ll][l] -= aa[icol][l]*dum;
165 for (l = 0; l < m; l++)
166 b[ll][l] -= b[icol][l]*dum;
167 }
168 }
169 for (l =(n - 1); l > -1; l--)
170 {
171 if (indxr[l] != indxc[l])
172 for (k = 0; k < n; k++)
173 SWAP(aa[k][indxr[l]], aa[k][indxc[l]]);
174 }
175 }
176 //-----------------------------------------------------------------------
covsrt(int mfit)177 void TLMFit::covsrt(int mfit)
178 {
179 int i, j, k;
180 double temp;
181
182 for (i = mfit; i < nparam; i++)
183 for (j = 0; j < i; j++)
184 covar[i][j] = 0.0;
185 k = mfit;
186 for (j = (nparam - 1); j>-1; j--)
187 {
188 if (ia[j])
189 {
190 for (i = 0; i < nparam; i++)
191 SWAP(covar[i][k], covar[i][j]);
192 for (i = 0; i < nparam; i++)
193 SWAP(covar[k][i], covar[j][i]);
194 k--;
195 }
196 }
197 }
198 //-----------------------------------------------------------------------
mrqcof(vector<double> & ba,vector<vector<double>> & balpha,vector<double> & bbeta)199 void TLMFit::mrqcof(vector<double> &ba, vector< vector<double> > &balpha, vector < double> &bbeta)
200 {
201 vector < double> dyda(nparam, 0);
202 int i, j, k, l, m, mfit = 0;
203 double ymod, wt, dy;
204
205 for (j = 0; j < nparam; j++)
206 if (ia[j] > 0)
207 mfit++;
208
209 for (j = 0; j < mfit; j++)
210 {
211 for (k = 0; k <=j; k++)
212 balpha[j][k] = 0.0;
213 bbeta[j] = 0.0;
214 }
215 chisq = 0.0;
216
217 for (i = 0; i < ndata; i++)
218 {
219 (*funcP)(x[i], ba, ymod, dyda, nparam);
220
221 dy = y[i] - ymod;
222 for (j=-1, l = 0; l < nparam; l++)
223 {
224 if (ia[l])
225 {
226 wt = dyda[l];
227 for (j++, k=-1, m = 0; m <= l; m++)
228 {
229 if (ia[m])
230 balpha[j][++k] += wt*dyda[m];
231 }
232 bbeta[j] += dy*wt;
233 }
234 }
235 chisq += dy*dy;
236 }
237
238 for (j = 1; j < mfit; j++)
239 for (k = 0; k < j; k++)
240 balpha[k][j] = balpha[j][k];
241 }
242 //-----------------------------------------------------------------------
mrqmin()243 void TLMFit::mrqmin()
244 {
245 static vector < double> atry, beta, da;
246 int j, k, l;
247 static int mfit;
248 static double ochisq;
249 static vector< vector < double> > oneda;
250
251 if (alamda < 0.0)
252 {
253 atry.resize(nparam);
254 beta.resize(nparam);
255 da.resize(nparam);
256
257 for (mfit = 0, j = 0; j < nparam; j++)
258 if (ia[j])
259 mfit++;
260 oneda.resize(mfit);
261 for (unsigned int i = 0; i < oneda.size(); i++)
262 oneda[i].resize(1);
263 alamda = 0.001;
264 mrqcof(a, alpha, beta);
265 ochisq = (chisq);
266 for (j = 0; j < nparam; j++)
267 atry[j] =(a[j]);
268 }
269 for (j = 0; j < mfit; j++)
270 {
271 for (k = 0; k < mfit; k++)
272 covar[j][k] = alpha[j][k];
273 covar[j][j] = alpha[j][j]*(1.0 + (alamda));
274 oneda[j][0] = beta[j];
275 }
276 try {gaussj(covar, mfit, oneda, 1);}
277 catch (ESingularMatrix &E)
278 {
279 throw;
280 }
281 for (j = 0; j < mfit; j++)
282 {
283 da[j] = oneda[j][0];
284 }
285 if (alamda == 0.0)
286 {
287 covsrt(mfit);
288 return;
289 }
290 for (j = 0, l = 0; l < nparam; l++)
291 if (ia[l])
292 atry[l] = a[l] + da[j++];
293 mrqcof(atry, covar, da);
294 if (chisq < ochisq)
295 {
296 alamda *= 0.1;
297 ochisq = (chisq);
298 for (j = 0; j < mfit; j++)
299 {
300 for (k = 0; k < mfit; k++)
301 alpha[j][k] = covar[j][k];
302 beta[j] = da[j];
303 }
304 for (j = 0; j < nparam; j++) // Achtung!! in aelteren Versionen war hier ein Fehler
305 a[j] = atry[j];
306 } else
307 {
308 alamda *= 10.0;
309 chisq = ochisq;
310 }
311 }
312 //----------------------------------------------------------------------------
313 // Implementation ESinglularMatrix (error handling for singular matrix)
314 //----------------------------------------------------------------------------
ESingularMatrix(int i)315 ESingularMatrix::ESingularMatrix(int i)
316 {
317 Type = i;
318 }
319 //----------------------------------------------------------------------------
320
321