1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.apache.commons.math.estimation;
18
19 import java.io.Serializable;
20 import java.util.Arrays;
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100 public class LevenbergMarquardtEstimator extends AbstractEstimator implements Serializable {
101
102
103
104
105
106
107
108
109
110
111
112
113
114 public LevenbergMarquardtEstimator() {
115
116
117 setMaxCostEval(1000);
118
119
120 setInitialStepBoundFactor(100.0);
121 setCostRelativeTolerance(1.0e-10);
122 setParRelativeTolerance(1.0e-10);
123 setOrthoTolerance(1.0e-10);
124
125 }
126
127
128
129
130
131
132
133
134
135
136 public void setInitialStepBoundFactor(double initialStepBoundFactor) {
137 this.initialStepBoundFactor = initialStepBoundFactor;
138 }
139
140
141
142
143
144
145
146 public void setCostRelativeTolerance(double costRelativeTolerance) {
147 this.costRelativeTolerance = costRelativeTolerance;
148 }
149
150
151
152
153
154
155
156
157 public void setParRelativeTolerance(double parRelativeTolerance) {
158 this.parRelativeTolerance = parRelativeTolerance;
159 }
160
161
162
163
164
165
166
167
168 public void setOrthoTolerance(double orthoTolerance) {
169 this.orthoTolerance = orthoTolerance;
170 }
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200 public void estimate(EstimationProblem problem)
201 throws EstimationException {
202
203 initializeEstimate(problem);
204
205
206 solvedCols = Math.min(rows, cols);
207 diagR = new double[cols];
208 jacNorm = new double[cols];
209 beta = new double[cols];
210 permutation = new int[cols];
211 lmDir = new double[cols];
212
213
214 double delta = 0, xNorm = 0;
215 double[] diag = new double[cols];
216 double[] oldX = new double[cols];
217 double[] oldRes = new double[rows];
218 double[] work1 = new double[cols];
219 double[] work2 = new double[cols];
220 double[] work3 = new double[cols];
221
222
223 updateResidualsAndCost();
224
225
226 lmPar = 0;
227 boolean firstIteration = true;
228 while (true) {
229
230
231 updateJacobian();
232 qrDecomposition();
233
234
235 qTy(residuals);
236
237
238
239 for (int k = 0; k < solvedCols; ++k) {
240 int pk = permutation[k];
241 jacobian[k * cols + pk] = diagR[pk];
242 }
243
244 if (firstIteration) {
245
246
247
248 xNorm = 0;
249 for (int k = 0; k < cols; ++k) {
250 double dk = jacNorm[k];
251 if (dk == 0) {
252 dk = 1.0;
253 }
254 double xk = dk * parameters[k].getEstimate();
255 xNorm += xk * xk;
256 diag[k] = dk;
257 }
258 xNorm = Math.sqrt(xNorm);
259
260
261 delta = (xNorm == 0) ? initialStepBoundFactor : (initialStepBoundFactor * xNorm);
262
263 }
264
265
266 double maxCosine = 0;
267 if (cost != 0) {
268 for (int j = 0; j < solvedCols; ++j) {
269 int pj = permutation[j];
270 double s = jacNorm[pj];
271 if (s != 0) {
272 double sum = 0;
273 for (int i = 0, index = pj; i <= j; ++i, index += cols) {
274 sum += jacobian[index] * residuals[i];
275 }
276 maxCosine = Math.max(maxCosine, Math.abs(sum) / (s * cost));
277 }
278 }
279 }
280 if (maxCosine <= orthoTolerance) {
281 return;
282 }
283
284
285 for (int j = 0; j < cols; ++j) {
286 diag[j] = Math.max(diag[j], jacNorm[j]);
287 }
288
289
290 for (double ratio = 0; ratio < 1.0e-4;) {
291
292
293 for (int j = 0; j < solvedCols; ++j) {
294 int pj = permutation[j];
295 oldX[pj] = parameters[pj].getEstimate();
296 }
297 double previousCost = cost;
298 double[] tmpVec = residuals;
299 residuals = oldRes;
300 oldRes = tmpVec;
301
302
303 determineLMParameter(oldRes, delta, diag, work1, work2, work3);
304
305
306 double lmNorm = 0;
307 for (int j = 0; j < solvedCols; ++j) {
308 int pj = permutation[j];
309 lmDir[pj] = -lmDir[pj];
310 parameters[pj].setEstimate(oldX[pj] + lmDir[pj]);
311 double s = diag[pj] * lmDir[pj];
312 lmNorm += s * s;
313 }
314 lmNorm = Math.sqrt(lmNorm);
315
316
317 if (firstIteration) {
318 delta = Math.min(delta, lmNorm);
319 }
320
321
322 updateResidualsAndCost();
323
324
325 double actRed = -1.0;
326 if (0.1 * cost < previousCost) {
327 double r = cost / previousCost;
328 actRed = 1.0 - r * r;
329 }
330
331
332
333 for (int j = 0; j < solvedCols; ++j) {
334 int pj = permutation[j];
335 double dirJ = lmDir[pj];
336 work1[j] = 0;
337 for (int i = 0, index = pj; i <= j; ++i, index += cols) {
338 work1[i] += jacobian[index] * dirJ;
339 }
340 }
341 double coeff1 = 0;
342 for (int j = 0; j < solvedCols; ++j) {
343 coeff1 += work1[j] * work1[j];
344 }
345 double pc2 = previousCost * previousCost;
346 coeff1 = coeff1 / pc2;
347 double coeff2 = lmPar * lmNorm * lmNorm / pc2;
348 double preRed = coeff1 + 2 * coeff2;
349 double dirDer = -(coeff1 + coeff2);
350
351
352 ratio = (preRed == 0) ? 0 : (actRed / preRed);
353
354
355 if (ratio <= 0.25) {
356 double tmp =
357 (actRed < 0) ? (0.5 * dirDer / (dirDer + 0.5 * actRed)) : 0.5;
358 if ((0.1 * cost >= previousCost) || (tmp < 0.1)) {
359 tmp = 0.1;
360 }
361 delta = tmp * Math.min(delta, 10.0 * lmNorm);
362 lmPar /= tmp;
363 } else if ((lmPar == 0) || (ratio >= 0.75)) {
364 delta = 2 * lmNorm;
365 lmPar *= 0.5;
366 }
367
368
369 if (ratio >= 1.0e-4) {
370
371 firstIteration = false;
372 xNorm = 0;
373 for (int k = 0; k < cols; ++k) {
374 double xK = diag[k] * parameters[k].getEstimate();
375 xNorm += xK * xK;
376 }
377 xNorm = Math.sqrt(xNorm);
378 } else {
379
380 cost = previousCost;
381 for (int j = 0; j < solvedCols; ++j) {
382 int pj = permutation[j];
383 parameters[pj].setEstimate(oldX[pj]);
384 }
385 tmpVec = residuals;
386 residuals = oldRes;
387 oldRes = tmpVec;
388 }
389
390
391 if (((Math.abs(actRed) <= costRelativeTolerance) &&
392 (preRed <= costRelativeTolerance) &&
393 (ratio <= 2.0)) ||
394 (delta <= parRelativeTolerance * xNorm)) {
395 return;
396 }
397
398
399
400 if ((Math.abs(actRed) <= 2.2204e-16) && (preRed <= 2.2204e-16) && (ratio <= 2.0)) {
401 throw new EstimationException("cost relative tolerance is too small ({0})," +
402 " no further reduction in the" +
403 " sum of squares is possible",
404 new Object[] { new Double(costRelativeTolerance) });
405 } else if (delta <= 2.2204e-16 * xNorm) {
406 throw new EstimationException("parameters relative tolerance is too small" +
407 " ({0}), no further improvement in" +
408 " the approximate solution is possible",
409 new Object[] { new Double(parRelativeTolerance) });
410 } else if (maxCosine <= 2.2204e-16) {
411 throw new EstimationException("orthogonality tolerance is too small ({0})," +
412 " solution is orthogonal to the jacobian",
413 new Object[] { new Double(orthoTolerance) });
414 }
415
416 }
417
418 }
419
420 }
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444 private void determineLMParameter(double[] qy, double delta, double[] diag,
445 double[] work1, double[] work2, double[] work3) {
446
447
448
449 for (int j = 0; j < rank; ++j) {
450 lmDir[permutation[j]] = qy[j];
451 }
452 for (int j = rank; j < cols; ++j) {
453 lmDir[permutation[j]] = 0;
454 }
455 for (int k = rank - 1; k >= 0; --k) {
456 int pk = permutation[k];
457 double ypk = lmDir[pk] / diagR[pk];
458 for (int i = 0, index = pk; i < k; ++i, index += cols) {
459 lmDir[permutation[i]] -= ypk * jacobian[index];
460 }
461 lmDir[pk] = ypk;
462 }
463
464
465
466 double dxNorm = 0;
467 for (int j = 0; j < solvedCols; ++j) {
468 int pj = permutation[j];
469 double s = diag[pj] * lmDir[pj];
470 work1[pj] = s;
471 dxNorm += s * s;
472 }
473 dxNorm = Math.sqrt(dxNorm);
474 double fp = dxNorm - delta;
475 if (fp <= 0.1 * delta) {
476 lmPar = 0;
477 return;
478 }
479
480
481
482
483 double sum2, parl = 0;
484 if (rank == solvedCols) {
485 for (int j = 0; j < solvedCols; ++j) {
486 int pj = permutation[j];
487 work1[pj] *= diag[pj] / dxNorm;
488 }
489 sum2 = 0;
490 for (int j = 0; j < solvedCols; ++j) {
491 int pj = permutation[j];
492 double sum = 0;
493 for (int i = 0, index = pj; i < j; ++i, index += cols) {
494 sum += jacobian[index] * work1[permutation[i]];
495 }
496 double s = (work1[pj] - sum) / diagR[pj];
497 work1[pj] = s;
498 sum2 += s * s;
499 }
500 parl = fp / (delta * sum2);
501 }
502
503
504 sum2 = 0;
505 for (int j = 0; j < solvedCols; ++j) {
506 int pj = permutation[j];
507 double sum = 0;
508 for (int i = 0, index = pj; i <= j; ++i, index += cols) {
509 sum += jacobian[index] * qy[i];
510 }
511 sum /= diag[pj];
512 sum2 += sum * sum;
513 }
514 double gNorm = Math.sqrt(sum2);
515 double paru = gNorm / delta;
516 if (paru == 0) {
517
518 paru = 2.2251e-308 / Math.min(delta, 0.1);
519 }
520
521
522
523 lmPar = Math.min(paru, Math.max(lmPar, parl));
524 if (lmPar == 0) {
525 lmPar = gNorm / dxNorm;
526 }
527
528 for (int countdown = 10; countdown >= 0; --countdown) {
529
530
531 if (lmPar == 0) {
532 lmPar = Math.max(2.2251e-308, 0.001 * paru);
533 }
534 double sPar = Math.sqrt(lmPar);
535 for (int j = 0; j < solvedCols; ++j) {
536 int pj = permutation[j];
537 work1[pj] = sPar * diag[pj];
538 }
539 determineLMDirection(qy, work1, work2, work3);
540
541 dxNorm = 0;
542 for (int j = 0; j < solvedCols; ++j) {
543 int pj = permutation[j];
544 double s = diag[pj] * lmDir[pj];
545 work3[pj] = s;
546 dxNorm += s * s;
547 }
548 dxNorm = Math.sqrt(dxNorm);
549 double previousFP = fp;
550 fp = dxNorm - delta;
551
552
553
554 if ((Math.abs(fp) <= 0.1 * delta) ||
555 ((parl == 0) && (fp <= previousFP) && (previousFP < 0))) {
556 return;
557 }
558
559
560 for (int j = 0; j < solvedCols; ++j) {
561 int pj = permutation[j];
562 work1[pj] = work3[pj] * diag[pj] / dxNorm;
563 }
564 for (int j = 0; j < solvedCols; ++j) {
565 int pj = permutation[j];
566 work1[pj] /= work2[j];
567 double tmp = work1[pj];
568 for (int i = j + 1; i < solvedCols; ++i) {
569 work1[permutation[i]] -= jacobian[i * cols + pj] * tmp;
570 }
571 }
572 sum2 = 0;
573 for (int j = 0; j < solvedCols; ++j) {
574 double s = work1[permutation[j]];
575 sum2 += s * s;
576 }
577 double correction = fp / (delta * sum2);
578
579
580 if (fp > 0) {
581 parl = Math.max(parl, lmPar);
582 } else if (fp < 0) {
583 paru = Math.min(paru, lmPar);
584 }
585
586
587 lmPar = Math.max(parl, lmPar + correction);
588
589 }
590 }
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612 private void determineLMDirection(double[] qy, double[] diag,
613 double[] lmDiag, double[] work) {
614
615
616
617 for (int j = 0; j < solvedCols; ++j) {
618 int pj = permutation[j];
619 for (int i = j + 1; i < solvedCols; ++i) {
620 jacobian[i * cols + pj] = jacobian[j * cols + permutation[i]];
621 }
622 lmDir[j] = diagR[pj];
623 work[j] = qy[j];
624 }
625
626
627 for (int j = 0; j < solvedCols; ++j) {
628
629
630
631 int pj = permutation[j];
632 double dpj = diag[pj];
633 if (dpj != 0) {
634 Arrays.fill(lmDiag, j + 1, lmDiag.length, 0);
635 }
636 lmDiag[j] = dpj;
637
638
639
640
641 double qtbpj = 0;
642 for (int k = j; k < solvedCols; ++k) {
643 int pk = permutation[k];
644
645
646
647 if (lmDiag[k] != 0) {
648
649 double sin, cos;
650 double rkk = jacobian[k * cols + pk];
651 if (Math.abs(rkk) < Math.abs(lmDiag[k])) {
652 double cotan = rkk / lmDiag[k];
653 sin = 1.0 / Math.sqrt(1.0 + cotan * cotan);
654 cos = sin * cotan;
655 } else {
656 double tan = lmDiag[k] / rkk;
657 cos = 1.0 / Math.sqrt(1.0 + tan * tan);
658 sin = cos * tan;
659 }
660
661
662
663 jacobian[k * cols + pk] = cos * rkk + sin * lmDiag[k];
664 double temp = cos * work[k] + sin * qtbpj;
665 qtbpj = -sin * work[k] + cos * qtbpj;
666 work[k] = temp;
667
668
669 for (int i = k + 1; i < solvedCols; ++i) {
670 double rik = jacobian[i * cols + pk];
671 temp = cos * rik + sin * lmDiag[i];
672 lmDiag[i] = -sin * rik + cos * lmDiag[i];
673 jacobian[i * cols + pk] = temp;
674 }
675
676 }
677 }
678
679
680
681 int index = j * cols + permutation[j];
682 lmDiag[j] = jacobian[index];
683 jacobian[index] = lmDir[j];
684
685 }
686
687
688
689 int nSing = solvedCols;
690 for (int j = 0; j < solvedCols; ++j) {
691 if ((lmDiag[j] == 0) && (nSing == solvedCols)) {
692 nSing = j;
693 }
694 if (nSing < solvedCols) {
695 work[j] = 0;
696 }
697 }
698 if (nSing > 0) {
699 for (int j = nSing - 1; j >= 0; --j) {
700 int pj = permutation[j];
701 double sum = 0;
702 for (int i = j + 1; i < nSing; ++i) {
703 sum += jacobian[i * cols + pj] * work[i];
704 }
705 work[j] = (work[j] - sum) / lmDiag[j];
706 }
707 }
708
709
710 for (int j = 0; j < lmDir.length; ++j) {
711 lmDir[permutation[j]] = work[j];
712 }
713
714 }
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737 private void qrDecomposition() {
738
739
740 for (int k = 0; k < cols; ++k) {
741 permutation[k] = k;
742 double norm2 = 0;
743 for (int index = k; index < jacobian.length; index += cols) {
744 double akk = jacobian[index];
745 norm2 += akk * akk;
746 }
747 jacNorm[k] = Math.sqrt(norm2);
748 }
749
750
751 for (int k = 0; k < cols; ++k) {
752
753
754 int nextColumn = -1;
755 double ak2 = Double.NEGATIVE_INFINITY;
756 for (int i = k; i < cols; ++i) {
757 double norm2 = 0;
758 int iDiag = k * cols + permutation[i];
759 for (int index = iDiag; index < jacobian.length; index += cols) {
760 double aki = jacobian[index];
761 norm2 += aki * aki;
762 }
763 if (norm2 > ak2) {
764 nextColumn = i;
765 ak2 = norm2;
766 }
767 }
768 if (ak2 == 0) {
769 rank = k;
770 return;
771 }
772 int pk = permutation[nextColumn];
773 permutation[nextColumn] = permutation[k];
774 permutation[k] = pk;
775
776
777 int kDiag = k * cols + pk;
778 double akk = jacobian[kDiag];
779 double alpha = (akk > 0) ? -Math.sqrt(ak2) : Math.sqrt(ak2);
780 double betak = 1.0 / (ak2 - akk * alpha);
781 beta[pk] = betak;
782
783
784 diagR[pk] = alpha;
785 jacobian[kDiag] -= alpha;
786
787
788 for (int dk = cols - 1 - k; dk > 0; --dk) {
789 int dkp = permutation[k + dk] - pk;
790 double gamma = 0;
791 for (int index = kDiag; index < jacobian.length; index += cols) {
792 gamma += jacobian[index] * jacobian[index + dkp];
793 }
794 gamma *= betak;
795 for (int index = kDiag; index < jacobian.length; index += cols) {
796 jacobian[index + dkp] -= gamma * jacobian[index];
797 }
798 }
799
800 }
801
802 rank = solvedCols;
803
804 }
805
806
807
808
809
810
811 private void qTy(double[] y) {
812 for (int k = 0; k < cols; ++k) {
813 int pk = permutation[k];
814 int kDiag = k * cols + pk;
815 double gamma = 0;
816 for (int i = k, index = kDiag; i < rows; ++i, index += cols) {
817 gamma += jacobian[index] * y[i];
818 }
819 gamma *= beta[pk];
820 for (int i = k, index = kDiag; i < rows; ++i, index += cols) {
821 y[i] -= gamma * jacobian[index];
822 }
823 }
824 }
825
826
827 private int solvedCols;
828
829
830 private double[] diagR;
831
832
833 private double[] jacNorm;
834
835
836 private double[] beta;
837
838
839 private int[] permutation;
840
841
842 private int rank;
843
844
845 private double lmPar;
846
847
848 private double[] lmDir;
849
850
851 private double initialStepBoundFactor;
852
853
854 private double costRelativeTolerance;
855
856
857 private double parRelativeTolerance;
858
859
860
861 private double orthoTolerance;
862
863
864 private static final long serialVersionUID = -5705952631533171019L;
865
866 }