1/*!
2 * \file   include/TFEL/Math/LevenbergMarquardt/LevenbergMarquardt.ixx
3 * \brief
4 *
5 * \author Thomas Helfer
6 * \date   21 nov 2008
7 * \copyright Copyright (C) 2006-2018 CEA/DEN, EDF R&D. All rights
8 * reserved.
9 * This project is publicly released under either the GNU GPL Licence
10 * or the CECILL-A licence. A copy of thoses licences are delivered
11 * with the sources of TFEL. CEA or EDF may also distribute this
12 * project under specific licensing conditions.
13 */
14
15#ifndef LIB_TFEL_MATH_LEVENBERGMARQUARDTIXX
16#define LIB_TFEL_MATH_LEVENBERGMARQUARDTIXX
17
18#include<algorithm>
19
20#include"TFEL/Math/power.hxx"
21#include"TFEL/Math/MathException.hxx"
22
23namespace tfel
24{
25
26  namespace math
27  {
28
29    template<typename F>
30    LevenbergMarquardt<F>::LevenbergMarquardt(const F f_)
31      : f(f_),
32	p(f_.getNumberOfParameters())
33    {} // end of LevenbergMarquardt::LevenbergMarquardt
34
35    template<typename F>
36    void
37    LevenbergMarquardt<F>::setInitialDampingParameter(const T value)
38    {
39      this->lambda0 = value;
40    } // end of LevenbergMarquardt<F>::setInitialDampingParameter
41
42    template<typename F>
43    void
44    LevenbergMarquardt<F>::setFirstCriterium(const T value)
45    {
46      this->eps1 = value;
47    } // end of LevenbergMarquardt<F>::setFirstCriterium
48
49    template<typename F>
50    void
51    LevenbergMarquardt<F>::setSecondCriterium(const T value)
52    {
53      this->eps2 = value;
54    } // end of LevenbergMarquardt<F>::setSecondCriterium
55
56    template<typename F>
57    void
58    LevenbergMarquardt<F>::setMultiplicationFactor(const T value)
59    {
60      this->factor = value;
61    } // end of LevenbergMarquardt<F>::setMultiplicationFactor
62
63    template<typename F>
64    void
65    LevenbergMarquardt<F>::addData(const typename LevenbergMarquardt<F>::Variable& x,
66				   const T y)
67    {
68      using namespace std;
69      this->data.push_back(pair<Variable,T>(x,y));
70    } // end of LevenbergMarquardt::addData
71
72    template<typename F>
73    void
74    LevenbergMarquardt<F>::setInitialGuess(const typename LevenbergMarquardt<F>::Parameter& p_)
75    {
76      this->p = p_;
77    } // end of LevenbergMarquardt::setInitialGuess
78
79    template<typename F>
80    void
81    LevenbergMarquardt<F>::setMaximumIteration(const T nb)
82    {
83      this->iterMax = nb;
84    } // end of LevenbergMarquardt::setMaximumIteration
85
86    template<typename F>
87    unsigned short
88    LevenbergMarquardt<F>::getNumberOfIterations() const
89    {
90      return this->iter;
91    } // end of LevenbergMarquardt::getNumberOfIterations
92
93    template<typename F>
94    const typename LevenbergMarquardt<F>::Parameter&
95    LevenbergMarquardt<F>::execute()
96    {
97      using namespace std;
98      using tfel::math::stdfunctions::power;
99      typename vector<pair<Variable,T> >::const_iterator it;
100      size_type m = this->f.getNumberOfParameters();
101      // grad is the opposite of the gradient
102      matrix<T> J(m,m,T(0));
103      matrix<T> Jn(m,m,T(0));
104      Parameter g(m,T(0));
105      Parameter gn(m,T(0));
106      Parameter gradient(m,T(0));
107      Parameter h(m);
108      Parameter p_(m);
109      T s(T(0));
110      T v(0);
111      T lambda = this->lambda0;
112      unsigned short i;
113      bool success;
114      s = T(0);
115      for(it=this->data.begin();it!=this->data.end();++it){
116	p_ = this->p+h;
117	(this->f)(v,gradient,it->first,p_);
118	g += (v-it->second)*gradient;
119	J += gradient^gradient;
120	s += power<2>(v-it->second);
121      }
122      lambda *= *(max_element(J.begin(),J.end()));
123      for(i=0;i!=m;++i){
124	J(i,i) += lambda;
125      }
126      this->factor=2;
127      success = false;
128      for(this->iter=0;(this->iter!=this->iterMax)&&(!success);++(this->iter)){
129	Jn=J;
130	fill(gn.begin(),gn.end(),T(0));
131	h = -g;
132	T sn(T(0));
133	LUSolve::exe(Jn,h);
134	fill(Jn.begin(),Jn.end(),T(0));
135	for(it=this->data.begin();it!=this->data.end();++it){
136	  p_ = this->p+h;
137	  (this->f)(v,gradient,it->first,p_);
138	  gn += (v-it->second)*gradient;
139	  Jn += gradient^gradient;
140	  sn += power<2>(v-it->second);
141	}
142	T rho = (s-sn)/(0.5*(h|(lambda*h-g)));
143	if(rho>0){
144	  lambda  *= max(T(0.3333),T(1)-power<3>(2*rho-1));
145	  this->factor = 2;
146	} else {
147	  lambda *= this->factor;
148	  this->factor *= 2;
149	}
150	for(i=0;i!=m;++i){
151	  Jn(i,i) += lambda;
152	}
153	this->p += h;
154	T ng = norm(gn);
155	T nh = norm(h);
156	T np = norm(p);
157	if(nh<this->eps2*(np+this->eps2)){
158	  success = true;
159	} else if (ng<this->eps1){
160	  success = true;
161	} else {
162	  // preparing next iteration
163	  J = Jn;
164	  g = gn;
165	  s = sn;
166	}
167      }
168      if(!success){
169	throw(MaximumNumberOfIterationsReachedException());
170      }
171
172      return this->p;
173    } // end of execute
174
175    template<typename F>
176    LevenbergMarquardt<F>::~LevenbergMarquardt() = default;
177
178  } // end of namespace math
179
180} // end of namespace tfel
181
182#endif /* LIB_TFEL_MATH_LEVENBERGMARQUARDTIXX */
183
184