19 #include "analysis/OrcaKinFit/NewFitterGSL.h"
20 #include <framework/logging/Logger.h>
27 #include "analysis/OrcaKinFit/BaseFitObject.h"
28 #include "analysis/OrcaKinFit/BaseHardConstraint.h"
29 #include "analysis/OrcaKinFit/BaseSoftConstraint.h"
30 #include "analysis/OrcaKinFit/BaseTracer.h"
32 #include <gsl/gsl_block.h>
33 #include <gsl/gsl_vector.h>
34 #include <gsl/gsl_matrix.h>
35 #include <gsl/gsl_permutation.h>
36 #include <gsl/gsl_linalg.h>
37 #include <gsl/gsl_blas.h>
38 #include <gsl/gsl_cdf.h>
47 namespace OrcaKinFit {
49 static int debuglevel = 0;
50 static int nitdebug = 0;
56 : npar(0), ncon(0), nsoft(0), nunm(0), ierr(0), nit(0),
57 fitprob(0), chi2(0), idim(0), x(nullptr), xold(nullptr), xnew(nullptr),
59 dx(nullptr), dxscal(nullptr),
61 y(nullptr), yscal(nullptr),
62 perr(nullptr), v1(nullptr), v2(nullptr),
64 M(nullptr), Mscal(nullptr), W(nullptr), W2(nullptr), W3(nullptr),
65 M1(nullptr), M2(nullptr), M3(nullptr), M4(nullptr), M5(nullptr),
67 CC(nullptr), CC1(nullptr), CCinv(nullptr),
68 permW(nullptr), eigenws(nullptr), eigenwsdim(0),
69 chi2best(0), chi2new(0), chi2old(0), fvalbest(0),
70 scale(0), scalebest(0), stepsize(0), stepbest(0),
71 scalevals{0}, fvals{0} ,
73 try2ndOrderCorr(
true),
81 if (x) gsl_vector_free(x);
82 if (xold) gsl_vector_free(xold);
83 if (xnew) gsl_vector_free(xnew);
85 if (dx) gsl_vector_free(dx);
86 if (dxscal) gsl_vector_free(dxscal);
88 if (y) gsl_vector_free(y);
89 if (yscal) gsl_vector_free(yscal);
90 if (perr) gsl_vector_free(perr);
91 if (v1) gsl_vector_free(v1);
92 if (v2) gsl_vector_free(v2);
94 if (M) gsl_matrix_free(M);
95 if (Mscal) gsl_matrix_free(Mscal);
96 if (W) gsl_matrix_free(W);
97 if (W2) gsl_matrix_free(W2);
98 if (W3) gsl_matrix_free(W3);
99 if (M1) gsl_matrix_free(M1);
100 if (M2) gsl_matrix_free(M2);
101 if (M3) gsl_matrix_free(M3);
102 if (M4) gsl_matrix_free(M4);
103 if (M5) gsl_matrix_free(M5);
105 if (CC) gsl_matrix_free(CC);
106 if (CC1) gsl_matrix_free(CC1);
107 if (CCinv) gsl_matrix_free(CCinv);
108 if (permW) gsl_permutation_free(permW);
109 if (eigenws) gsl_eigen_symm_free(eigenws);
138 eigenws =
nullptr; eigenwsdim = 0;
150 assert(x && (x->size == idim));
151 assert(xold && (xold->size == idim));
152 assert(xnew && (xnew->size == idim));
153 assert(dx && (dx->size == idim));
154 assert(y && (y->size == idim));
155 assert(perr && (perr->size == idim));
156 assert(v1 && (v1->size == idim));
157 assert(v2 && (v2->size == idim));
159 assert(M && (M->size1 == idim && M->size2 == idim));
160 assert(W && (W->size1 == idim && W->size2 == idim));
161 assert(W2 && (W2->size1 == idim && W2->size2 == idim));
162 assert(M1 && (M1->size1 == idim && M1->size2 == idim));
164 assert(permW && (permW->size == idim));
166 gsl_vector_set_zero(x);
167 gsl_vector_set_zero(y);
168 gsl_vector_set_all(perr, 1);
173 B2INFO(
"fit: Start values: \n");
182 if (ifailL)
return -1;
192 traceValues[
"alpha"] = 0;
193 traceValues[
"phi"] = 0;
194 traceValues[
"mu"] = 0;
195 traceValues[
"detW"] = 0;
199 bool converged =
false;
207 if (tracer) tracer->
step(*
this);
212 gsl_blas_dcopy(x, xold);
218 int ifail =
calcNewtonDx(dx, dxscal, x, perr, M, Mscal, y, yscal, W, W2, permW, v1);
222 B2DEBUG(10,
"NewFitterGSL::fit: calcNewtonDx error " << ifail);
228 B2INFO(
"Before test convergence, calcNewtonDe: Result: \n");
229 debug_print(dx,
"dx");
230 debug_print(dxscal,
"dxscal");
234 if (gsl_blas_dasum(dxscal) < 1E-6 * idim) {
243 calcLimitedDx(alpha, mu, xnew, imode, x, v2, dx, dxscal, perr, M, Mscal, W, v1);
245 gsl_blas_dcopy(xnew, x);
254 converged = (abs(chi2new - chi2old) < 0.0001);
267 B2DEBUG(12,
"abs (chi2new - chi2old)=" << abs(chi2new - chi2old) <<
"\n"
268 <<
"fvalbest=" << fvalbest <<
"\n"
269 <<
"abs(fvals[0]-fvalbest)=" << abs(fvals[0] - fvalbest) <<
"\n");
272 }
while (!(converged ||
ierr));
276 if (tracer) tracer->
step(*
this);
285 int ifailw = calcCovMatrix(W, permW, x);
289 for (
auto& fitobject : fitobjects) {
290 for (
int ilocal = 0; ilocal < fitobject->getNPar(); ++ilocal) {
291 int iglobal = fitobject->getGlobalParNum(ilocal);
292 for (
int jlocal = ilocal; jlocal < fitobject->getNPar(); ++jlocal) {
293 int jglobal = fitobject->getGlobalParNum(jlocal);
294 if (iglobal >= 0 && jglobal >= 0)
295 fitobject->setCov(ilocal, jlocal, gsl_matrix_get(CCinv, iglobal, jglobal));
304 B2INFO(
"========= END =========\n");
305 B2INFO(
"Fit objects:\n");
306 for (
auto fo : fitobjects) {
308 B2INFO(fo->getName() <<
": " << *fo <<
", chi2=" << fo->getChi2());
310 B2INFO(
"constraints:\n");
311 for (
auto i = constraints.begin(); i != constraints.end(); ++i) {
314 B2INFO(i - constraints.begin() <<
" " << c->getName() <<
": " << c->getValue() <<
"+-" << c->getError());
316 B2INFO(
"=============================================\n");
323 if (tracer) tracer->
finish(*
this);
326 B2DEBUG(10,
"NewFitterGSL::fit: converged=" << converged
327 <<
", nit=" <<
nit <<
", fitprob=" <<
fitprob);
344 for (
auto& fitobject : fitobjects) {
345 for (
int ilocal = 0; ilocal < fitobject->getNPar(); ++ilocal) {
346 if (!fitobject->isParamFixed(ilocal)) {
347 fitobject->setGlobalParNum(ilocal,
npar);
349 if (!fitobject->isParamMeasured(ilocal)) ++
nunm;
355 ncon = constraints.size();
357 for (
unsigned int icon = 0; icon < constraints.size(); ++icon) {
360 c->setGlobalNum(
npar + icon);
364 nsoft = softconstraints.size();
367 B2ERROR(
"NewFitterGSL::initialize: nunm=" <<
nunm <<
" > ncon+nsoft="
374 ini_gsl_vector(x, idim);
375 ini_gsl_vector(xold, idim);
376 ini_gsl_vector(xnew, idim);
378 ini_gsl_vector(dx, idim);
379 ini_gsl_vector(dxscal, idim);
381 ini_gsl_vector(y, idim);
382 ini_gsl_vector(yscal, idim);
383 ini_gsl_vector(perr, idim);
384 ini_gsl_vector(v1, idim);
385 ini_gsl_vector(v2, idim);
388 ini_gsl_matrix(M, idim, idim);
389 ini_gsl_matrix(Mscal, idim, idim);
390 ini_gsl_matrix(W, idim, idim);
391 ini_gsl_matrix(W2, idim, idim);
392 ini_gsl_matrix(W3, idim, idim);
393 ini_gsl_matrix(M1, idim, idim);
394 ini_gsl_matrix(M2, idim, idim);
395 ini_gsl_matrix(M3, idim, idim);
396 ini_gsl_matrix(M4, idim, idim);
397 ini_gsl_matrix(M5, idim, idim);
399 ini_gsl_matrix(CC, idim, idim);
400 ini_gsl_matrix(CC1, idim, idim);
401 ini_gsl_matrix(CCinv, idim, idim);
403 ini_gsl_permutation(permW, idim);
405 if (eigenws && eigenwsdim != idim) {
406 gsl_eigen_symm_free(eigenws);
409 if (eigenws ==
nullptr) eigenws = gsl_eigen_symm_alloc(idim);
419 for (
auto fo : fitobjects) {
421 chi2 += fo->getChi2();
423 for (
auto bsc : softconstraints) {
425 chi2 += bsc->getChi2();
436 void NewFitterGSL::ini_gsl_permutation(gsl_permutation*& p,
unsigned int size)
439 if (p->size != size) {
440 gsl_permutation_free(p);
441 if (size > 0) p = gsl_permutation_alloc(size);
444 }
else if (size > 0) p = gsl_permutation_alloc(size);
447 void NewFitterGSL::ini_gsl_vector(gsl_vector*& v,
unsigned int size)
451 if (v->size != size) {
453 if (size > 0) v = gsl_vector_alloc(size);
456 }
else if (size > 0) v = gsl_vector_alloc(size);
459 void NewFitterGSL::ini_gsl_matrix(gsl_matrix*& m,
unsigned int size1,
unsigned int size2)
462 if (m->size1 != size1 || m->size2 != size2) {
464 if (size1 * size2 > 0) m = gsl_matrix_alloc(size1, size2);
467 }
else if (size1 * size2 > 0) m = gsl_matrix_alloc(size1, size2);
470 void NewFitterGSL::debug_print(
const gsl_matrix* m,
const char* name)
472 for (
unsigned int i = 0; i < m->size1; ++i)
473 for (
unsigned int j = 0; j < m->size2; ++j)
474 if (gsl_matrix_get(m, i, j) != 0)
475 B2INFO(name <<
"[" << i <<
"][" << j <<
"]=" << gsl_matrix_get(m, i, j));
478 void NewFitterGSL::debug_print(
const gsl_vector* v,
const char* name)
480 for (
unsigned int i = 0; i < v->size; ++i)
481 if (gsl_vector_get(v, i) != 0)
482 B2INFO(name <<
"[" << i <<
"]=" << gsl_vector_get(v, i));
485 void NewFitterGSL::add(gsl_vector* vecz,
const gsl_vector* vecx,
double a,
const gsl_vector* vecy)
490 assert(vecx->size == vecy->size);
491 assert(vecx->size == vecz->size);
493 gsl_blas_dcopy(vecx, vecz);
494 gsl_blas_daxpy(a, vecy, vecz);
502 bool NewFitterGSL::updateParams(gsl_vector* vecx)
505 assert(vecx->size == idim);
506 bool significant =
false;
507 for (
auto i = fitobjects.begin(); i != fitobjects.end(); ++i) {
508 BaseFitObject* fo = *i;
510 bool s = fo->updateParams(vecx->block->data, vecx->size);
512 if (
nit < nitdebug && s) {
513 B2DEBUG(15,
"Significant update for FO " << i - fitobjects.begin() <<
" ("
514 << fo->getName() <<
")\n");
520 bool NewFitterGSL::isfinite(
const gsl_vector* vec)
522 if (vec ==
nullptr)
return true;
523 for (
size_t i = 0; i < vec->size; ++i)
524 if (!std::isfinite(gsl_vector_get(vec, i)))
return false;
528 bool NewFitterGSL::isfinite(
const gsl_matrix* mat)
530 if (mat ==
nullptr)
return true;
531 for (
size_t i = 0; i < mat->size1; ++i)
532 for (
size_t j = 0; j < mat->size2; ++j)
533 if (!std::isfinite(gsl_matrix_get(mat, i, j)))
return false;
538 void NewFitterGSL::fillx(gsl_vector* vecx)
541 assert(vecx->size == idim);
543 gsl_vector_set_zero(vecx);
544 for (
auto fo : fitobjects) {
546 for (
int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
547 if (!fo->isParamFixed(ilocal)) {
548 int iglobal = fo->getGlobalParNum(ilocal);
549 assert(iglobal >= 0 && iglobal <
npar);
550 gsl_vector_set(vecx, iglobal, fo->getParam(ilocal));
556 void NewFitterGSL::fillperr(gsl_vector* vece)
559 assert(vece->size == idim);
560 gsl_vector_set_all(vece, 1);
561 for (
auto fo : fitobjects) {
563 for (
int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
564 if (!fo->isParamFixed(ilocal)) {
565 int iglobal = fo->getGlobalParNum(ilocal);
566 assert(iglobal >= 0 && iglobal <
npar);
567 double e = std::abs(fo->getError(ilocal));
568 gsl_vector_set(vece, iglobal, e ? e : 1);
572 for (
auto c : constraints) {
574 int iglobal = c->getGlobalNum();
575 assert(iglobal >= 0 && iglobal < (
int)idim);
576 double e = c->getError();
577 gsl_vector_set(vece, iglobal, e ? 1 / e : 1);
581 void NewFitterGSL::assembleM(gsl_matrix* MatM,
const gsl_vector* vecx,
bool errorpropagation)
584 assert(MatM->size1 == idim && MatM->size2 == idim);
586 assert(vecx->size == idim);
588 gsl_matrix_set_zero(MatM);
591 for (
auto fo : fitobjects) {
593 fo->addToGlobalChi2DerMatrix(MatM->block->data, MatM->tda);
594 if (!isfinite(MatM)) {
595 B2DEBUG(10,
"NewFitterGSL::assembleM: illegal elements in MatM after adding fo " << *fo <<
":\n");
596 if (debug > 15) debug_print(MatM,
"M");
600 B2INFO(
"After adding covariances from fit objects:\n");
602 debug_print(MatM,
"MatM");
608 for (
auto c : constraints) {
610 int kglobal = c->getGlobalNum();
611 assert(kglobal >= 0 && kglobal < (
int)idim);
612 c->add1stDerivativesToMatrix(MatM->block->data, MatM->tda);
613 if (!isfinite(MatM)) {
614 B2DEBUG(10,
"NewFitterGSL::assembleM: illegal elements in MatM after adding 1st derivatives of constraint " << *c <<
":\n");
615 if (debug > 13) debug_print(MatM,
"M");
618 B2INFO(
"After adding first derivatives of constraint " << c->getName());
620 debug_print(MatM,
"MatM");
621 B2INFO(
"errorpropagation = " << errorpropagation);
625 if (!errorpropagation) c->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda, gsl_vector_get(vecx, kglobal));
626 if (!isfinite(MatM)) {
627 B2DEBUG(10,
"NewFitterGSL::assembleM: illegal elements in MatM after adding 2nd derivatives of constraint " << *c <<
":\n");
628 if (debug > 13) debug_print(MatM,
"MatM");
632 B2INFO(
"After adding derivatives of constraints::\n");
635 B2INFO(
"===========================================::\n");
640 for (
auto bsc : softconstraints) {
642 bsc->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda);
643 if (!isfinite(MatM)) {
644 B2DEBUG(10,
"NewFitterGSL::assembleM: illegal elements in MatM after adding soft constraint " << *bsc <<
":\n");
645 if (debug > 13) debug_print(MatM,
"M");
649 B2INFO(
"After adding soft constraints::\n");
652 B2INFO(
"===========================================::\n");
657 void NewFitterGSL::assembleG(gsl_matrix* MatM,
const gsl_vector* vecx)
660 assert(MatM->size1 == idim && MatM->size2 == idim);
662 assert(vecx->size == idim);
664 gsl_matrix_set_zero(MatM);
666 for (
auto fo : fitobjects) {
668 fo->addToGlobalChi2DerMatrix(MatM->block->data, MatM->tda);
672 for (
auto c : constraints) {
674 int kglobal = c->getGlobalNum();
675 assert(kglobal >= 0 && kglobal < (
int)idim);
676 c->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda, gsl_vector_get(vecx, kglobal));
681 for (
auto bsc : softconstraints) {
683 bsc->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda);
687 void NewFitterGSL::scaleM(gsl_matrix* MatMscal,
const gsl_matrix* MatM,
const gsl_vector* vece)
690 assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
692 assert(MatM->size1 == idim && MatM->size2 == idim);
694 assert(vece->size == idim);
697 for (
unsigned int i = 0; i < idim; ++i)
698 for (
unsigned int j = 0; j < idim; ++j)
699 gsl_matrix_set(MatMscal, i, j,
700 gsl_vector_get(vece, i)*gsl_vector_get(vece, j)*gsl_matrix_get(MatM, i, j));
705 void NewFitterGSL::assembley(gsl_vector* vecy,
const gsl_vector* vecx)
708 assert(vecy->size == idim);
710 assert(vecx->size == idim);
711 gsl_vector_set_zero(vecy);
713 for (
auto fo : fitobjects) {
716 fo->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
721 for (
auto c : constraints) {
723 int kglobal = c->getGlobalNum();
724 assert(kglobal >= 0 && kglobal < (
int)idim);
725 c->addToGlobalChi2DerVector(vecy->block->data, vecy->size, gsl_vector_get(vecx, kglobal));
726 gsl_vector_set(vecy, kglobal, c->getValue());
731 for (
auto bsc : softconstraints) {
733 bsc->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
737 void NewFitterGSL::scaley(gsl_vector* vecyscal,
const gsl_vector* vecy,
const gsl_vector* vece)
740 assert(vecyscal->size == idim);
742 assert(vecy->size == idim);
744 assert(vece->size == idim);
746 gsl_vector_memcpy(vecyscal, vecy);
747 gsl_vector_mul(vecyscal, vece);
750 int NewFitterGSL::assembleChi2Der(gsl_vector* vecy)
753 assert(vecy->size == idim);
755 gsl_vector_set_zero(vecy);
757 for (
auto fo : fitobjects) {
760 int ifail = fo->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
761 if (ifail)
return ifail;
766 for (
auto bsc : softconstraints) {
768 bsc->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
773 void NewFitterGSL::addConstraints(gsl_vector* vecy)
776 assert(vecy->size == idim);
779 for (
auto c : constraints) {
781 int kglobal = c->getGlobalNum();
782 gsl_vector_set(vecy, kglobal, c->getValue());
786 void NewFitterGSL::assembleConstDer(gsl_matrix* MatM)
789 assert(MatM->size1 == idim && MatM->size2 == idim);
791 gsl_matrix_set_zero(MatM);
794 for (
auto c : constraints) {
796 int kglobal = c->getGlobalNum();
797 assert(kglobal >= 0 && kglobal < (
int)idim);
798 c->add1stDerivativesToMatrix(MatM->block->data, MatM->tda);
803 gsl_vector* vecx,
const gsl_vector* vece,
804 gsl_matrix* MatM, gsl_matrix* MatMscal,
805 gsl_vector* vecy, gsl_vector* vecyscal,
806 gsl_matrix* MatW, gsl_matrix* MatW2,
807 gsl_permutation* permw,
812 assert(vecdx->size == idim);
814 assert(vecdxscal->size == idim);
816 assert(vecx->size == idim);
818 assert(vece->size == idim);
820 assert(MatM->size1 == idim && MatM->size2 == idim);
822 assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
824 assert(vecy->size == idim);
826 assert(vecyscal->size == idim);
828 assert(MatW->size1 == idim && MatW->size2 == idim);
830 assert(MatW2->size1 == idim && MatW2->size2 == idim);
832 assert(permw->size == idim);
834 assert(vecw->size == idim);
842 assembleConstDer(MatM);
844 B2DEBUG(12,
"NewFitterGSL::calcNewtonDx: ptLp=" << ptLp <<
" with lambdas from last iteration");
845 }
else if (ncalc == 2) {
847 gsl_vector_view lambda(gsl_vector_subvector(vecx,
npar,
ncon));
848 gsl_vector_set_zero(&lambda.vector);
849 B2DEBUG(12,
"NewFitterGSL::calcNewtonDx: ptLp=" << ptLp <<
" with recalculated lambdas");
850 }
else if (ncalc >= 3) {
851 B2DEBUG(12,
"NewFitterGSL::calcNewtonDx: ptLp=" << ptLp <<
" with zero lambdas");
856 B2INFO(
"calcNewtonDx: before setting up equations: \n");
857 debug_print(vecx,
"x");
860 assembleM(MatM, vecx);
861 if (!isfinite(MatM))
return 1;
863 scaleM(MatMscal, MatM, vece);
864 assembley(vecy, vecx);
865 if (!isfinite(vecy))
return 2;
866 scaley(vecyscal, vecy, vece);
869 B2INFO(
"calcNewtonDx: After setting up equations: \n");
870 debug_print(MatM,
"M");
871 debug_print(MatMscal,
"Mscal");
872 debug_print(vecy,
"y");
873 debug_print(vecyscal,
"yscal");
874 debug_print(vece,
"perr");
875 debug_print(vecx,
"x");
876 debug_print(xnew,
"xnew");
882 double epsLU = 1E-12;
885 solveSystem(vecdxscal, detW, vecyscal, MatMscal, MatW, MatW2, vecw, epsLU, epsSV);
889 traceValues[
"detW"] = detW;
893 gsl_blas_dscal(-1, dxscal);
896 gsl_vector_memcpy(vecdx, vecdxscal);
897 gsl_vector_mul(vecdx, vece);
900 B2INFO(
"calcNewtonDx: Result: \n");
901 debug_print(vecdx,
"dx");
902 debug_print(vecdxscal,
"dxscal");
915 gsl_vector* vecx, gsl_vector* vecdxhat,
916 const gsl_vector* vecdx,
const gsl_vector* vecdxscal,
917 const gsl_vector* vece,
918 const gsl_matrix* MatM,
const gsl_matrix* MatMscal,
919 gsl_matrix* MatW, gsl_vector* vecw
923 assert(vecxnew->size == idim);
925 assert(vecx->size == idim);
927 assert(vecdxhat->size == idim);
929 assert(vecdx->size == idim);
931 assert(vecdxscal->size == idim);
933 assert(vece->size == idim);
935 assert(MatM->size1 == idim && MatM->size2 == idim);
937 assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
939 assert(MatW->size1 == idim && MatW->size2 == idim);
941 assert(vecw->size == idim);
946 add(vecxnew, vecx, alpha, vecdx);
949 B2INFO(
"calcLimitedDx: After solving equations: \n");
950 debug_print(vecx,
"x");
951 gsl_blas_dcopy(vecx, vecw);
952 gsl_vector_div(vecw, vece);
953 debug_print(vecw,
"xscal");
954 debug_print(vecxnew,
"xnew");
955 gsl_blas_dcopy(vecxnew, vecw);
956 gsl_vector_div(vecw, vece);
957 debug_print(vecw,
"xnewscal");
960 mu =
calcMu(vecx, vece, vecdx, vecdxscal, vecxnew, MatM, MatMscal, vecw);
969 add(vecw, vecx, eps, vecdx);
971 double dphinum = (
meritFunction(mu, vecw, vece) - phi0) / eps;
974 B2INFO(
"analytic der: " << dphi0 <<
", num=" << dphinum);
978 traceValues[
"alpha"] = 0;
979 traceValues[
"phi"] = phi0;
980 traceValues[
"mu"] = mu;
981 if (tracer) tracer->
substep(*
this, 0);
984 updateParams(vecxnew);
988 B2DEBUG(12,
"calcLimitedDx: phi0=" << phi0 <<
", dphi0=" << dphi0
989 <<
", phiR = " << phiR <<
", mu=" << mu
990 <<
", threshold = " << phi0 + eta * dphi0
991 <<
" => test=" << (phiR > phi0 + eta * dphi0));
994 traceValues[
"alpha"] = 1;
995 traceValues[
"phi"] = phiR;
996 if (tracer) tracer->
substep(*
this, 0);
1001 if (phiR > phi0 + eta * alpha * dphi0) {
1004 if (try2ndOrderCorr) {
1006 gsl_blas_dcopy(vecxnew, vecw);
1007 add(vecxnew, vecxnew, 1, vecdxhat);
1008 updateParams(vecxnew);
1011 #ifndef FIT_TRACEOFF
1012 traceValues[
"alpha"] = 1.5;
1013 traceValues[
"phi"] = phi2ndOrder;
1014 if (tracer) tracer->
substep(*
this, 2);
1017 B2DEBUG(12,
"calcLimitedDx: tried 2nd order correction, phi2ndOrder = "
1018 << phi2ndOrder <<
", threshold = " << phi0 + eta * dphi0);
1020 debug_print(vecdxhat,
"dxhat");
1021 debug_print(xnew,
"xnew");
1023 if (phi2ndOrder <= phi0 + eta * alpha * dphi0) {
1024 B2DEBUG(12,
" -> 2nd order correction successfull!");
1027 B2DEBUG(12,
" -> 2nd order correction failed, do linesearch!");
1028 gsl_blas_dcopy(vecw, vecxnew);
1029 updateParams(vecxnew);
1030 #ifndef FIT_TRACEOFF
1032 traceValues[
"alpha"] = 1;
1033 traceValues[
"phi"] = phiR;
1034 if (tracer) tracer->
substep(*
this, 2);
1040 doLineSearch(alpha, vecxnew, imode, phi0, dphi0, eta, zeta, mu,
1041 vecx, vecdx, vece, vecw);
1049 double phi0,
double dphi0,
1050 double eta,
double zeta,
1052 const gsl_vector* vecx,
const gsl_vector* vecdx,
1053 const gsl_vector* vece, gsl_vector* vecw)
1056 assert(vecxnew->size == idim);
1058 assert(vecx->size == idim);
1060 assert(vecdx->size == idim);
1062 assert(vece->size == idim);
1064 assert(vecw->size == idim);
1067 if (imode < 0)
return -1;
1069 assert((imode == 0) || eta < zeta);
1075 B2DEBUG(12,
"NewFitterGSL::doLineSearch: dphi0 > 0!");
1079 add(vecxnew, vecx, alpha, vecdx);
1080 updateParams(vecxnew);
1084 #ifndef FIT_TRACEOFF
1085 traceValues[
"alpha"] = alpha;
1086 traceValues[
"phi"] = phi;
1087 if (tracer) tracer->
substep(*
this, 1);
1093 double alphaR = alpha;
1095 add(vecxnew, vecx, alpha, vecdx);
1096 updateParams(vecxnew);
1108 alpha = 0.5 * (alphaL + alphaR);
1110 add(vecxnew, vecx, alpha, vecdx);
1111 updateParams(vecxnew);
1115 #ifndef FIT_TRACEOFF
1116 traceValues[
"alpha"] = alpha;
1117 traceValues[
"phi"] = phi;
1118 if (tracer) tracer->
substep(*
this, 1);
1122 if (phi >= phi0 + eta * alpha * dphi0) {
1123 B2DEBUG(15,
"NewFitterGSL::doLineSearch, Armijo: phi=" << phi
1124 <<
" >= " << phi0 + eta * alpha * dphi0
1125 <<
" at alpha=" << alpha);
1133 }
else if (imode == 1) {
1135 if (dphi < zeta * dphi0) {
1136 B2DEBUG(15,
"NewFitterGSL::doLineSearch, Wolfe: dphi=" << dphi
1137 <<
" < " << zeta * dphi0
1138 <<
" at alpha=" << alpha);
1146 if (phi < phi0 + zeta * alpha * dphi0) {
1147 B2DEBUG(15,
"NewFitterGSL::doLineSearch, Goldstein: phi=" << phi
1148 <<
" < " << phi0 + zeta * alpha * dphi0
1149 <<
" at alpha=" << alpha);
1156 }
while (nite < 30 && (alphaL == 0 || nite < 6));
1157 if (alphaL > 0) alpha = alphaL;
1163 const gsl_vector* vecdx,
const gsl_vector* vecdxscal,
1164 const gsl_vector* vecxnew,
1165 const gsl_matrix* MatM,
const gsl_matrix* MatMscal,
1169 assert(vecx->size == idim);
1171 assert(vece->size == idim);
1173 assert(vecdx->size == idim);
1175 assert(vecdxscal->size == idim);
1177 assert(MatM->size1 == idim && MatM->size2 == idim);
1179 assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1181 assert(vecw->size == idim);
1193 gsl_vector_set_zero(vecw);
1194 addConstraints(vecw);
1195 gsl_vector_view c(gsl_vector_subvector(vecw,
npar,
ncon));
1197 double cnorm1 = gsl_blas_dasum(&c.vector);
1199 gsl_vector_const_view lambdaerr(gsl_vector_const_subvector(vece,
npar,
ncon));
1200 gsl_vector_mul(&c.vector, &lambdaerr.vector);
1202 double cnorm1scal = gsl_blas_dasum(&c.vector);
1207 assembleChi2Der(vecw);
1208 gsl_vector_view gradf(gsl_vector_subvector(vecw, 0,
npar));
1209 gsl_vector_const_view p(gsl_vector_const_subvector(vecdx, 0,
npar));
1211 gsl_blas_ddot(&gradf.vector, &p.vector, &gradfTp);
1213 B2DEBUG(17,
"NewFitterGSL::calcMu: cnorm1scal=" << cnorm1scal
1214 <<
", gradfTp=" << gradfTp);
1217 if (cnorm1scal <
ncon * eps || gradfTp <= 0) {
1218 for (
int kglobal =
npar; kglobal <
npar +
ncon; ++kglobal) {
1219 double abslambda = std::fabs(gsl_vector_get(vecxnew, kglobal));
1220 if (abslambda > result)
1223 result /= (1 - rho);
1226 double pTLp =
calcpTLp(vecdx, MatM, vecw);
1227 double sigma = (pTLp > 0) ? 1 : 0;
1228 B2DEBUG(17,
" pTLp = " << pTLp);
1230 result = (gradfTp + 0.5 * sigma * pTLp) / ((1 - rho) * cnorm1);
1232 B2DEBUG(17,
" result = " << result);
1237 for (
int kglobal =
npar; kglobal <
npar +
ncon; ++kglobal) {
1238 double abslambdascal = std::fabs(gsl_vector_get(vecx, kglobal) / gsl_vector_get(vece, kglobal));
1239 if (abslambdascal > result)
1240 result = abslambdascal;
1253 assert(vecx->size == idim);
1255 assert(vece->size == idim);
1261 for (
auto c : constraints) {
1263 int kglobal = c->getGlobalNum();
1264 assert(kglobal >= 0 && kglobal < (
int)idim);
1265 result += mu * std::fabs(c->getValue());
1270 for (
auto c : constraints) {
1272 int kglobal = c->getGlobalNum();
1273 assert(kglobal >= 0 && kglobal < (
int)idim);
1275 result += mu * std::fabs(c->getValue() * gsl_vector_get(vece, kglobal));
1286 const gsl_vector* vecdx, gsl_vector* vecw)
1289 assert(vecx->size == idim);
1291 assert(vece->size == idim);
1293 assert(vecdx->size == idim);
1295 assert(vecw->size == idim);
1300 assembleChi2Der(vecw);
1301 for (
int i = 0; i <
npar; ++i) {
1302 result += gsl_vector_get(vecdx, i) * gsl_vector_get(vecw, i);
1311 for (
auto c : constraints) {
1313 int kglobal = c->getGlobalNum();
1314 assert(kglobal >= 0 && kglobal < (
int)idim);
1315 result -= mu * std::fabs(c->getValue());
1319 assembleChi2Der(vecw);
1320 for (
int i = 0; i <
npar; ++i) {
1321 result += gsl_vector_get(vecdx, i) * gsl_vector_get(vecw, i);
1330 for (
auto c : constraints) {
1332 int kglobal = c->getGlobalNum();
1333 assert(kglobal >= 0 && kglobal < (
int)idim);
1334 result -= mu * std::fabs(c->getValue()) * gsl_vector_get(vece, kglobal);
1343 int NewFitterGSL::invertM()
1345 gsl_matrix_memcpy(W, M);
1351 int result = gsl_linalg_LU_decomp(W, permW, &signum);
1352 B2DEBUG(11,
"invertM: gsl_linalg_LU_decomp result=" << result);
1354 ifail = gsl_linalg_LU_invert(W, permW, M);
1355 B2DEBUG(11,
"invertM: gsl_linalg_LU_invert result=" << ifail);
1358 B2ERROR(
"NewtonFitter::invert: ifail from gsl_linalg_LU_invert=" << ifail);
1369 int NewFitterGSL::calcCovMatrix(gsl_matrix* MatW,
1370 gsl_permutation* permw,
1382 gsl_matrix_set_zero(M1);
1383 gsl_matrix_set_zero(M2);
1385 for (
auto fo : fitobjects) {
1387 fo->addToGlobalChi2DerMatrix(M1->block->data, M1->tda);
1388 fo->addToGlobCov(M2->block->data, M2->tda);
1391 gsl_matrix_scale(M1, -1);
1396 gsl_matrix_view dydeta = gsl_matrix_submatrix(M1, 0, 0, idim,
npar);
1397 gsl_matrix_view Cov_eta = gsl_matrix_submatrix(M2, 0, 0,
npar,
npar);
1400 B2INFO(
"NewFitterGSL::calcCovMatrix\n");
1401 debug_print(&dydeta.matrix,
"dydeta");
1402 debug_print(&Cov_eta.matrix,
"Cov_eta");
1408 assembleM(MatW, vecx,
true);
1411 debug_print(MatW,
"MatW");
1418 int result = gsl_linalg_LU_decomp(MatW, permw, &signum);
1421 B2INFO(
"calcCovMatrix: gsl_linalg_LU_decomp result=" << result);
1422 debug_print(MatW,
"M_LU");
1426 gsl_set_error_handler_off();
1427 int ifail = gsl_linalg_LU_invert(MatW, permw, M3);
1429 B2WARNING(
"NewFitterGSL: MatW matrix is singular!");
1434 B2INFO(
"calcCovMatrix: gsl_linalg_LU_invert ifail=" << ifail);
1435 debug_print(M3,
"Minv");
1439 gsl_matrix_set_zero(M4);
1440 gsl_matrix_view dadeta = gsl_matrix_submatrix(M4, 0, 0, idim,
npar);
1443 debug_print(&dadeta.matrix,
"dadeta");
1447 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, M3, &dydeta.matrix, 0, &dadeta.matrix);
1453 gsl_matrix_view M3part = gsl_matrix_submatrix(M3, 0, 0,
npar, idim);
1454 gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, &Cov_eta.matrix, &dadeta.matrix, 0, &M3part.matrix);
1456 gsl_matrix_set_zero(M5);
1457 gsl_matrix_view Cov_a = gsl_matrix_submatrix(M5, 0, 0,
npar,
npar);
1458 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, &dadeta.matrix, &M3part.matrix, 0, M5);
1459 gsl_matrix_memcpy(CCinv, M5);
1462 debug_print(&Cov_a.matrix,
"Cov_a");
1463 debug_print(CCinv,
"full Cov from err prop");
1464 debug_print(M1,
"uncorr Cov from err prop");
1474 for (
int i = 0; i <
covDim; ++i) {
1475 for (
int j = 0; j <
covDim; ++j) {
1476 cov[i *
covDim + j] = gsl_matrix_get(&Cov_a.matrix, i, j);
1484 const gsl_matrix* MatM,
const gsl_vector* vecx,
1485 gsl_matrix* MatW, gsl_vector* vecw,
1489 assert(vecxnew->size == idim);
1491 assert(MatM->size1 == idim && MatM->size2 == idim);
1493 assert(vecx->size == idim);
1495 assert(MatW->size1 == idim && MatW->size2 == idim);
1497 assert(vecw->size == idim);
1498 assert(idim ==
static_cast<unsigned int>(
npar +
ncon));
1500 gsl_matrix_const_view A(gsl_matrix_const_submatrix(MatM, 0,
npar,
npar,
ncon));
1502 gsl_matrix_view Acopy(gsl_matrix_submatrix(MatW, 0,
npar,
npar,
ncon));
1503 gsl_matrix_view& V = ATA;
1505 gsl_vector_view gradf(gsl_vector_subvector(vecw, 0,
npar));
1506 gsl_vector_view ATgradf(gsl_vector_subvector(vecw,
npar,
ncon));
1507 gsl_vector_view& s = ATgradf;
1509 gsl_vector_view lambdanew(gsl_vector_subvector(vecxnew,
npar,
ncon));
1514 gsl_vector_fprintf(stdout, &lambdanew.vector,
"%f");
1518 gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &A.matrix, &A.matrix, 0, &ATA.matrix);
1521 int isfail = assembleChi2Der(vecw);
1522 if (isfail)
return isfail;
1526 gsl_blas_dgemv(CblasTrans, -1, &A.matrix, &gradf.vector, 0, &ATgradf.vector);
1530 gsl_matrix_fprintf(stdout, &A.matrix,
"%f");
1532 gsl_matrix_fprintf(stdout, &ATA.matrix,
"%f");
1534 gsl_vector_fprintf(stdout, &gradf.vector,
"%f");
1535 B2INFO(
"ATgradf: ");
1536 gsl_vector_fprintf(stdout, &ATgradf.vector,
"%f");
1540 gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
1541 int cholesky_result = gsl_linalg_cholesky_decomp(&ATA.matrix);
1542 gsl_set_error_handler(old_handler);
1543 if (cholesky_result) {
1544 B2INFO(
"NewFitterGSL::determineLambdas: resorting to SVD");
1549 gsl_matrix_memcpy(&Acopy.matrix, &A.matrix);
1552 gsl_linalg_SV_decomp_jacobi(&Acopy.matrix, &V.matrix, &s.vector);
1554 double mins = eps * std::fabs(gsl_vector_get(&s.vector, 0));
1555 for (
int i = 0; i <
ncon; ++i) {
1556 if (std::fabs(gsl_vector_get(&s.vector, i)) <= mins)
1557 gsl_vector_set(&s.vector, i, 0);
1559 gsl_linalg_SV_solve(&Acopy.matrix, &V.matrix, &s.vector, &gradf.vector, &lambdanew.vector);
1561 gsl_linalg_cholesky_solve(&ATA.matrix, &ATgradf.vector, &lambdanew.vector);
1564 B2INFO(
"lambdanew: ");
1565 gsl_vector_fprintf(stdout, &lambdanew.vector,
"%f");
1571 gsl_matrix* Wm, gsl_vector* w,
1577 assert(Ainv->size1 == A->size2 && Ainv->size2 == A->size1);
1578 assert(A->size1 >= A->size2);
1580 assert(Wm->size1 >= A->size1 && Wm->size2 >= A->size2);
1582 assert(w->size >= A->size2);
1588 gsl_linalg_SV_decomp_jacobi(A, Wm, w);
1590 double mins = eps * std::fabs(gsl_vector_get(w, 0));
1593 for (
int i = 0; i < m; ++i) {
1594 double singval = gsl_vector_get(w, i);
1595 if (std::fabs(singval) > mins)
1596 gsl_vector_set(w, i, 1 / singval);
1598 gsl_vector_set(w, i, 0);
1604 for (
int j = 0; j < n; ++j) {
1605 double wval = gsl_vector_get(w, j);
1606 for (
int i = 0; i < m; ++i)
1607 gsl_matrix_set(Wm, i, j, wval * gsl_matrix_get(Wm, i, j));
1610 gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, Wm, A, 0, Ainv);
1618 assert(vecdx->size == idim);
1620 assert(MatM->size1 == idim && MatM->size2 == idim);
1622 assert(vecw->size == idim);
1624 gsl_vector_const_view p(gsl_vector_const_subvector(vecdx, 0,
npar));
1625 gsl_vector_view Lp(gsl_vector_subvector(vecw, 0,
npar));
1626 gsl_matrix_const_view L(gsl_matrix_const_submatrix(MatM, 0, 0,
npar,
npar));
1627 gsl_blas_dsymv(CblasUpper, 1, &L.matrix, &p.vector, 0, &Lp.vector);
1629 gsl_blas_ddot(&p.vector, &Lp.vector, &result);
1635 const gsl_vector* vecxnew,
1636 const gsl_matrix* MatM,
1642 assert(vecdxhat->size == idim);
1644 assert(vecxnew->size == idim);
1646 assert(MatM->size1 == idim && MatM->size2 == idim);
1648 assert(MatW->size1 == idim && MatW->size2 == idim);
1650 assert(vecw->size == idim);
1651 assert(idim ==
static_cast<unsigned int>(
npar +
ncon));
1656 gsl_matrix_const_view AT(gsl_matrix_const_submatrix(MatM, 0,
npar,
npar,
ncon));
1658 gsl_matrix_view ATcopy(gsl_matrix_submatrix(MatW, 0,
npar,
npar,
ncon));
1659 gsl_matrix_view& V = AAT;
1661 gsl_vector_set_zero(vecdxhat);
1662 addConstraints(vecdxhat);
1663 gsl_vector_view c(gsl_vector_subvector(vecdxhat,
npar,
ncon));
1664 gsl_vector_view phat = (gsl_vector_subvector(vecdxhat, 0,
npar));
1666 gsl_vector_set_zero(vecw);
1667 gsl_vector_view AATinvc(gsl_vector_subvector(vecw,
npar,
ncon));
1668 gsl_vector_view& s = AATinvc;
1672 gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &AT.matrix, &AT.matrix, 0, &AAT.matrix);
1675 gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
1676 int cholesky_result = gsl_linalg_cholesky_decomp(&AAT.matrix);
1677 gsl_set_error_handler(old_handler);
1678 if (cholesky_result) {
1679 B2INFO(
"NewFitterGSL::calc2ndOrderCorr: resorting to SVD");
1684 gsl_matrix_memcpy(&ATcopy.matrix, &AT.matrix);
1687 gsl_linalg_SV_decomp_jacobi(&ATcopy.matrix, &V.matrix, &s.vector);
1689 double mins = eps * std::fabs(gsl_vector_get(&s.vector, 0));
1690 for (
int i = 0; i <
ncon; ++i) {
1691 if (std::fabs(gsl_vector_get(&s.vector, i)) <= mins)
1692 gsl_vector_set(&s.vector, i, 0);
1694 gsl_linalg_SV_solve(&ATcopy.matrix, &V.matrix, &s.vector, &c.vector, &AATinvc.vector);
1696 gsl_linalg_cholesky_solve(&AAT.matrix, &c.vector, &AATinvc.vector);
1700 gsl_blas_dgemv(CblasNoTrans, -1, &AT.matrix, &AATinvc.vector, 0, &phat.vector);
1701 gsl_vector_set_zero(&c.vector);
1707 const gsl_vector* vecyscal,
1708 const gsl_matrix* MatMscal,
1716 assert(vecdxscal->size == idim);
1718 assert(vecyscal->size == idim);
1720 assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1722 assert(MatW->size1 == idim && MatW->size2 == idim);
1724 assert(MatW2->size1 == idim && MatW2->size2 == idim);
1726 assert(vecw->size == idim);
1730 int iLU =
solveSystemLU(vecdxscal, detW, vecyscal, MatMscal, MatW, vecw, epsLU);
1731 if (iLU == 0)
return result;
1734 int iSVD =
solveSystemSVD(vecdxscal, vecyscal, MatMscal, MatW, MatW2, vecw, epsSV);
1735 if (iSVD == 0)
return result;
1742 const gsl_vector* vecyscal,
1743 const gsl_matrix* MatMscal,
1749 assert(vecdxscal->size == idim);
1751 assert(vecyscal->size == idim);
1753 assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1755 assert(MatW->size1 == idim && MatW->size2 == idim);
1757 assert(vecw->size == idim);
1759 gsl_matrix_memcpy(MatW, MatMscal);
1765 int result = gsl_linalg_LU_decomp(MatW, permW, &signum);
1766 B2DEBUG(14,
"NewFitterGSL::solveSystem: gsl_linalg_LU_decomp result=" << result);
1767 if (result != 0)
return 1;
1769 detW = gsl_linalg_LU_det(MatW, signum);
1770 B2DEBUG(14,
"NewFitterGSL::solveSystem: determinant of W=" << detW);
1771 if (std::fabs(detW) < eps)
return 2;
1772 if (!std::isfinite(detW)) {
1773 B2DEBUG(10,
"NewFitterGSL::solveSystem: infinite determinant of W=" << detW);
1777 B2INFO(
"NewFitterGSL::solveSystem: after LU decomposition: \n");
1778 debug_print(MatW,
"W");
1781 ifail = gsl_linalg_LU_solve(MatW, permW, vecyscal, vecdxscal);
1782 B2DEBUG(14,
"NewFitterGSL::solveSystem: gsl_linalg_LU_solve result=" << ifail);
1785 B2ERROR(
"NewFitterGSL::solveSystem: ifail from gsl_linalg_LU_solve=" << ifail);
1796 const gsl_vector* vecyscal,
1797 const gsl_matrix* MatMscal,
1804 assert(vecdxscal->size == idim);
1806 assert(vecyscal->size == idim);
1808 assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1810 assert(MatW->size1 == idim && MatW->size2 == idim);
1812 assert(MatW2->size1 == idim && MatW2->size2 == idim);
1814 assert(vecw->size == idim);
1816 B2DEBUG(10,
"solveSystemSVD called");
1818 gsl_matrix_memcpy(MatW, MatMscal);
1821 gsl_linalg_SV_decomp_jacobi(MatW, MatW2, vecw);
1823 double mins = eps * std::fabs(gsl_vector_get(vecw, 0));
1824 B2DEBUG(15,
"SV 0 = " << gsl_vector_get(vecw, 0));
1825 for (
unsigned int i = 0; i < idim; ++i) {
1826 if (std::fabs(gsl_vector_get(vecw, i)) <= mins) {
1827 B2DEBUG(15,
"Setting SV" << i <<
" = " << gsl_vector_get(vecw, i) <<
" to zero!");
1828 gsl_vector_set(vecw, i, 0);
1831 gsl_linalg_SV_solve(MatW, MatW2, vecw, vecyscal, vecdxscal);
1836 gsl_vector* vecw1, gsl_vector* vecw2,
1837 gsl_permutation* permw,
double eps)
1840 assert(MatW1->size1 == idim && MatW1->size2 == idim);
1842 assert(MatW2->size1 == idim && MatW2->size2 == idim);
1844 assert(vecw1->size == idim);
1846 assert(vecw2->size == idim);
1848 assert(permw->size == idim);
1851 assembleConstDer(MatW2);
1853 gsl_matrix_const_view AT(gsl_matrix_const_submatrix(MatW2, 0, 0,
npar,
npar));
1855 gsl_matrix_view Q(gsl_matrix_submatrix(MatW1, 0, 0,
npar,
npar));
1856 gsl_matrix_view R(gsl_matrix_submatrix(MatW1,
npar, 0,
ncon,
npar));
1861 gsl_linalg_QRPT_decomp2(&AT.matrix, &Q.matrix, &R.matrix, vecw1, permw, &signum, vecw2);
1864 for (
int i = 0; i <
ncon; ++i) {
1865 if (fabs(gsl_matrix_get(&R.matrix, i, i)) > eps) rankA++;
1867 auto result = gsl_matrix_view(gsl_matrix_submatrix(MatW1, 0, rankA,
ncon - rankA,
npar));
1872 const gsl_vector* vecx, gsl_matrix* MatW2,
1874 gsl_vector* vecw1, gsl_vector* vecw2,
1875 gsl_permutation* permw,
double eps)
1878 assert(MatW1->size1 == idim && MatW1->size2 == idim);
1880 assert(vecx->size == idim);
1882 assert(MatW2->size1 == idim && MatW2->size2 == idim);
1884 assert(MatW2->size1 == idim && MatW2->size2 == idim);
1886 assert(vecw1->size == idim);
1888 assert(vecw2->size == idim);
1890 assert(permw->size == idim);
1894 gsl_matrix_view Z =
calcZ(rankA, MatW2, MatW1, vecw1, vecw2, permw, eps);
1897 assembleG(MatW1, vecx);
1899 rankH =
npar - rankA;
1901 gsl_matrix_view G(gsl_matrix_submatrix(MatW1, 0, 0,
npar,
npar));
1902 gsl_matrix_view GZ(gsl_matrix_submatrix(MatW3, 0, 0,
npar, rankH));
1903 gsl_matrix_view ZGZ(gsl_matrix_submatrix(MatW1, 0, 0, rankH, rankH));
1908 gsl_blas_dsymm(CblasLeft, CblasUpper, 1, &G.matrix, &Z.matrix, 0, &GZ.matrix);
1910 gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &Z.matrix, &GZ.matrix, 0, &ZGZ.matrix);
1916 const gsl_vector* vecx, gsl_matrix* MatW2,
1918 gsl_vector* vecw1, gsl_vector* vecw2,
1919 gsl_permutation* permw,
1920 gsl_eigen_symm_workspace* eigensws,
1924 assert(MatW1->size1 == idim && MatW1->size2 == idim);
1926 assert(vecx->size == idim);
1928 assert(MatW2->size1 == idim && MatW2->size2 == idim);
1930 assert(MatW2->size1 == idim && MatW2->size2 == idim);
1932 assert(vecw1->size == idim);
1934 assert(vecw2->size == idim);
1936 assert(permw->size == idim);
1939 gsl_matrix_view Hred =
calcReducedHessian(rankH, MatW1, vecx, MatW2, MatW3, vecw1, vecw2, permw, eps);
1941 gsl_matrix_view Hredcopy(gsl_matrix_submatrix(MatW3, 0, 0, rankH, rankH));
1943 gsl_matrix_memcpy(&Hredcopy.matrix, &Hred.matrix);
1945 gsl_vector_view
eval(gsl_vector_subvector(vecw1, 0, rankH));
1946 gsl_eigen_symm(&Hredcopy.matrix, &
eval.vector, eigensws);