<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.16"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>RcdMathLib_doc: levenberg_marquardt.c Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&dn=gpl-2.0.txt GPL-v2 */
$(document).ready(initResizable);
/* @license-end */</script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr style="height: 56px;">
<td id="projectalign" style="padding-left: 0.5em;">
<div id="projectname">RcdMathLib_doc
</div>
<div id="projectbrief">Open Source Library for Linear and Non-linear Algebra</div>
</td>
</tr>
</tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.16 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&dn=gpl-2.0.txt GPL-v2 */
var searchBox = new SearchBox("searchBox", "search",false,'Search');
/* @license-end */
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&dn=gpl-2.0.txt GPL-v2 */
$(function() {
initMenu('',true,false,'search.php','Search');
$(document).ready(function() { init_search(); });
});
/* @license-end */</script>
<div id="main-nav"></div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
<div id="nav-tree">
<div id="nav-tree-contents">
<div id="nav-sync" class="sync"></div>
</div>
</div>
<div id="splitbar" style="-moz-user-select:none;"
class="ui-resizable-handle">
</div>
</div>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&dn=gpl-2.0.txt GPL-v2 */
$(document).ready(function(){initNavTree('levenberg__marquardt_8c_source.html','');});
/* @license-end */
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>
<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0"
name="MSearchResults" id="MSearchResults">
</iframe>
</div>
<div class="header">
<div class="headertitle">
<div class="title">levenberg_marquardt.c</div> </div>
</div><!--header-->
<div class="contents">
<a href="levenberg__marquardt_8c.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> <span class="comment">/*</span></div>
<div class="line"><a name="l00002"></a><span class="lineno"> 2</span> <span class="comment"> * Copyright (C) 2020 Zakaria Kasmi <zkasmi@inf.fu-berlin.de></span></div>
<div class="line"><a name="l00003"></a><span class="lineno"> 3</span> <span class="comment"> * 2020 Freie Universität Berlin</span></div>
<div class="line"><a name="l00004"></a><span class="lineno"> 4</span> <span class="comment"> *</span></div>
<div class="line"><a name="l00005"></a><span class="lineno"> 5</span> <span class="comment"> * This file is subject to the terms and conditions of the GNU Lesser General</span></div>
<div class="line"><a name="l00006"></a><span class="lineno"> 6</span> <span class="comment"> * Public License v2.1. See the file LICENSE in the top level directory for more</span></div>
<div class="line"><a name="l00007"></a><span class="lineno"> 7</span> <span class="comment"> * details.</span></div>
<div class="line"><a name="l00008"></a><span class="lineno"> 8</span> <span class="comment"> */</span></div>
<div class="line"><a name="l00009"></a><span class="lineno"> 9</span>  </div>
<div class="line"><a name="l00023"></a><span class="lineno"> 23</span> <span class="preprocessor">#include <stdio.h></span></div>
<div class="line"><a name="l00024"></a><span class="lineno"> 24</span> <span class="preprocessor">#include <math.h></span></div>
<div class="line"><a name="l00025"></a><span class="lineno"> 25</span> <span class="preprocessor">#include <float.h></span></div>
<div class="line"><a name="l00026"></a><span class="lineno"> 26</span> <span class="preprocessor">#include <inttypes.h></span></div>
<div class="line"><a name="l00027"></a><span class="lineno"> 27</span>  </div>
<div class="line"><a name="l00028"></a><span class="lineno"> 28</span> <span class="preprocessor">#include "<a class="code" href="levenberg__marquardt_8h.html">levenberg_marquardt.h</a>"</span></div>
<div class="line"><a name="l00029"></a><span class="lineno"> 29</span> <span class="preprocessor">#include "<a class="code" href="matrix_8h.html">matrix.h</a>"</span></div>
<div class="line"><a name="l00030"></a><span class="lineno"> 30</span> <span class="preprocessor">#include "<a class="code" href="vector_8h.html">vector.h</a>"</span></div>
<div class="line"><a name="l00031"></a><span class="lineno"> 31</span> <span class="preprocessor">#include "<a class="code" href="solve_8h.html">solve.h</a>"</span></div>
<div class="line"><a name="l00032"></a><span class="lineno"> 32</span> <span class="preprocessor">#include "<a class="code" href="utils_8h.html">utils.h</a>"</span></div>
<div class="line"><a name="l00033"></a><span class="lineno"> 33</span>  </div>
<div class="line"><a name="l00053"></a><span class="lineno"><a class="line" href="levenberg__marquardt_8c.html#a741182c4bc201af44ac010bc4474b826"> 53</a></span> <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> <a class="code" href="levenberg__marquardt_8c.html#a741182c4bc201af44ac010bc4474b826">opt_levenberg_marquardt_correction</a>(uint8_t f_length, uint8_t n,</div>
<div class="line"><a name="l00054"></a><span class="lineno"> 54</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> x_vec[n],</div>
<div class="line"><a name="l00055"></a><span class="lineno"> 55</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> data_vec[f_length],</div>
<div class="line"><a name="l00056"></a><span class="lineno"> 56</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> mu,</div>
<div class="line"><a name="l00057"></a><span class="lineno"> 57</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> s[n],</div>
<div class="line"><a name="l00058"></a><span class="lineno"> 58</span>  <span class="keywordtype">void</span> (*get_f_error)(</div>
<div class="line"><a name="l00059"></a><span class="lineno"> 59</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> x_vec[],</div>
<div class="line"><a name="l00060"></a><span class="lineno"> 60</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> data_vec[],</div>
<div class="line"><a name="l00061"></a><span class="lineno"> 61</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> f_vec[]),</div>
<div class="line"><a name="l00062"></a><span class="lineno"> 62</span>  <span class="keywordtype">void</span> (*get_jacobian)(</div>
<div class="line"><a name="l00063"></a><span class="lineno"> 63</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> x_vec[],</div>
<div class="line"><a name="l00064"></a><span class="lineno"> 64</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> J[][n])</div>
<div class="line"><a name="l00065"></a><span class="lineno"> 65</span>  )</div>
<div class="line"><a name="l00066"></a><span class="lineno"> 66</span> {</div>
<div class="line"><a name="l00067"></a><span class="lineno"> 67</span>  </div>
<div class="line"><a name="l00068"></a><span class="lineno"> 68</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> Fx[f_length];</div>
<div class="line"><a name="l00069"></a><span class="lineno"> 69</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> J[f_length][n];</div>
<div class="line"><a name="l00070"></a><span class="lineno"> 70</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> f_vec[f_length];</div>
<div class="line"><a name="l00071"></a><span class="lineno"> 71</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> JTJ_mu2_I[n][n];</div>
<div class="line"><a name="l00072"></a><span class="lineno"> 72</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> JTF[n];</div>
<div class="line"><a name="l00073"></a><span class="lineno"> 73</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> ro_mu = 0;</div>
<div class="line"><a name="l00074"></a><span class="lineno"> 74</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> Fx_square = 0;</div>
<div class="line"><a name="l00075"></a><span class="lineno"> 75</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> Fx_plus_s_square = 0;</div>
<div class="line"><a name="l00076"></a><span class="lineno"> 76</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> Fx_plus_J_mul_s_square = 0;</div>
<div class="line"><a name="l00077"></a><span class="lineno"> 77</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> Fx_plus_J_mul_s[f_length];</div>
<div class="line"><a name="l00078"></a><span class="lineno"> 78</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> F_x_plus_s[f_length];</div>
<div class="line"><a name="l00079"></a><span class="lineno"> 79</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> x_plus_s[n];</div>
<div class="line"><a name="l00080"></a><span class="lineno"> 80</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> denom;</div>
<div class="line"><a name="l00081"></a><span class="lineno"> 81</span>  </div>
<div class="line"><a name="l00082"></a><span class="lineno"> 82</span>  <span class="comment">/*</span></div>
<div class="line"><a name="l00083"></a><span class="lineno"> 83</span> <span class="comment"> * compute: -(J'J + mu^2*I)</span></div>
<div class="line"><a name="l00084"></a><span class="lineno"> 84</span> <span class="comment"> */</span></div>
<div class="line"><a name="l00085"></a><span class="lineno"> 85</span>  <span class="comment">//JT_J = J'*J</span></div>
<div class="line"><a name="l00086"></a><span class="lineno"> 86</span>  get_jacobian(x_vec, J);</div>
<div class="line"><a name="l00087"></a><span class="lineno"> 87</span>  </div>
<div class="line"><a name="l00088"></a><span class="lineno"> 88</span>  <span class="comment">//store J'J in JTJ_mu2_I</span></div>
<div class="line"><a name="l00089"></a><span class="lineno"> 89</span>  <a class="code" href="matrix_8h.html#a0107887a6c40980fa639750be8262f01">matrix_trans_mul_itself</a>(f_length, n, J, JTJ_mu2_I);</div>
<div class="line"><a name="l00090"></a><span class="lineno"> 90</span>  </div>
<div class="line"><a name="l00091"></a><span class="lineno"> 91</span>  <span class="comment">//compute: J'J + mu^2*I</span></div>
<div class="line"><a name="l00092"></a><span class="lineno"> 92</span>  <a class="code" href="matrix_8h.html#aa8acafd4181978f536976f8151e51516">matrix_add_to_diag</a>(n, JTJ_mu2_I, n, pow(mu, 2));</div>
<div class="line"><a name="l00093"></a><span class="lineno"> 93</span>  </div>
<div class="line"><a name="l00094"></a><span class="lineno"> 94</span>  <span class="comment">// -(J'*J+mu^2*eye(n)), is an (nxn) matrix</span></div>
<div class="line"><a name="l00095"></a><span class="lineno"> 95</span>  <a class="code" href="matrix_8h.html#a51139e6c87b602e5e0ebfbc406fda35d">matrix_mul_scalar</a>(n, n, JTJ_mu2_I, -1, JTJ_mu2_I);</div>
<div class="line"><a name="l00096"></a><span class="lineno"> 96</span>  </div>
<div class="line"><a name="l00097"></a><span class="lineno"> 97</span>  <span class="comment">//JT_f = J'*f</span></div>
<div class="line"><a name="l00098"></a><span class="lineno"> 98</span>  get_f_error(x_vec, data_vec, f_vec);</div>
<div class="line"><a name="l00099"></a><span class="lineno"> 99</span>  <a class="code" href="matrix_8h.html#a53b0a1829b2085414fb66a9e9bd6c7b7">matrix_trans_mul_vec</a>(f_length, n, J, f_length, f_vec, JTF);</div>
<div class="line"><a name="l00100"></a><span class="lineno"> 100</span>  </div>
<div class="line"><a name="l00101"></a><span class="lineno"> 101</span>  <span class="comment">//solve the equation: s = -(J'*J+mu^2*I)\(J'*Fx);</span></div>
<div class="line"><a name="l00102"></a><span class="lineno"> 102</span>  <a class="code" href="solve_8h.html#ab6336aff7fbff116a6118b39a0d1e2cd">solve_householder</a>(n, n, JTJ_mu2_I, JTF, s);</div>
<div class="line"><a name="l00103"></a><span class="lineno"> 103</span>  </div>
<div class="line"><a name="l00104"></a><span class="lineno"> 104</span>  <span class="comment">//f(x)</span></div>
<div class="line"><a name="l00105"></a><span class="lineno"> 105</span>  get_f_error(x_vec, data_vec, Fx);</div>
<div class="line"><a name="l00106"></a><span class="lineno"> 106</span>  </div>
<div class="line"><a name="l00107"></a><span class="lineno"> 107</span>  <span class="comment">//f(x)^2</span></div>
<div class="line"><a name="l00108"></a><span class="lineno"> 108</span>  Fx_square = <a class="code" href="vector_8h.html#a5f4b706c07b9b0f7a4983d80fba99e4e">vector_get_scalar_product</a>(f_length, Fx, Fx);</div>
<div class="line"><a name="l00109"></a><span class="lineno"> 109</span>  </div>
<div class="line"><a name="l00110"></a><span class="lineno"> 110</span>  <span class="comment">//(x+s)</span></div>
<div class="line"><a name="l00111"></a><span class="lineno"> 111</span>  <a class="code" href="vector_8h.html#a92917951880002814392c2367896d7a0">vector_add</a>(n, x_vec, s, x_plus_s);</div>
<div class="line"><a name="l00112"></a><span class="lineno"> 112</span>  </div>
<div class="line"><a name="l00113"></a><span class="lineno"> 113</span>  <span class="comment">//f(x+s)</span></div>
<div class="line"><a name="l00114"></a><span class="lineno"> 114</span>  get_f_error(x_plus_s, data_vec, F_x_plus_s);</div>
<div class="line"><a name="l00115"></a><span class="lineno"> 115</span>  </div>
<div class="line"><a name="l00116"></a><span class="lineno"> 116</span>  <span class="comment">//f(x+s)^2</span></div>
<div class="line"><a name="l00117"></a><span class="lineno"> 117</span>  Fx_plus_s_square = <a class="code" href="vector_8h.html#a5f4b706c07b9b0f7a4983d80fba99e4e">vector_get_scalar_product</a>(f_length, F_x_plus_s,</div>
<div class="line"><a name="l00118"></a><span class="lineno"> 118</span>  F_x_plus_s);</div>
<div class="line"><a name="l00119"></a><span class="lineno"> 119</span>  </div>
<div class="line"><a name="l00120"></a><span class="lineno"> 120</span>  <span class="comment">//compute: J(x)*s</span></div>
<div class="line"><a name="l00121"></a><span class="lineno"> 121</span>  <a class="code" href="matrix_8h.html#ad610bcce232c69ca302550f57b1ad9fb">matrix_mul_vec</a>(f_length, n, J, s, Fx_plus_J_mul_s);</div>
<div class="line"><a name="l00122"></a><span class="lineno"> 122</span>  </div>
<div class="line"><a name="l00123"></a><span class="lineno"> 123</span>  <span class="comment">//compute: f(x) + J(x)*s</span></div>
<div class="line"><a name="l00124"></a><span class="lineno"> 124</span>  <a class="code" href="vector_8h.html#a92917951880002814392c2367896d7a0">vector_add</a>(f_length, Fx, Fx_plus_J_mul_s, Fx_plus_J_mul_s);</div>
<div class="line"><a name="l00125"></a><span class="lineno"> 125</span>  </div>
<div class="line"><a name="l00126"></a><span class="lineno"> 126</span>  <span class="comment">//compute: ||f(x) + J(x)*s||^2</span></div>
<div class="line"><a name="l00127"></a><span class="lineno"> 127</span>  Fx_plus_J_mul_s_square = <a class="code" href="vector_8h.html#a5f4b706c07b9b0f7a4983d80fba99e4e">vector_get_scalar_product</a>(f_length,</div>
<div class="line"><a name="l00128"></a><span class="lineno"> 128</span>  Fx_plus_J_mul_s,</div>
<div class="line"><a name="l00129"></a><span class="lineno"> 129</span>  Fx_plus_J_mul_s);</div>
<div class="line"><a name="l00130"></a><span class="lineno"> 130</span>  </div>
<div class="line"><a name="l00131"></a><span class="lineno"> 131</span>  denom = Fx_square - Fx_plus_J_mul_s_square;</div>
<div class="line"><a name="l00132"></a><span class="lineno"> 132</span>  <span class="keywordflow">if</span> (denom != 0) {</div>
<div class="line"><a name="l00133"></a><span class="lineno"> 133</span>  ro_mu = (Fx_square - Fx_plus_s_square) / denom;</div>
<div class="line"><a name="l00134"></a><span class="lineno"> 134</span>  }</div>
<div class="line"><a name="l00135"></a><span class="lineno"> 135</span>  <span class="keywordflow">else</span> {</div>
<div class="line"><a name="l00136"></a><span class="lineno"> 136</span>  puts(<span class="stringliteral">"ro_mu is infinite !!!"</span>);</div>
<div class="line"><a name="l00137"></a><span class="lineno"> 137</span>  ro_mu = FLT_MAX;</div>
<div class="line"><a name="l00138"></a><span class="lineno"> 138</span>  }</div>
<div class="line"><a name="l00139"></a><span class="lineno"> 139</span>  </div>
<div class="line"><a name="l00140"></a><span class="lineno"> 140</span>  <span class="keywordflow">return</span> ro_mu;</div>
<div class="line"><a name="l00141"></a><span class="lineno"> 141</span> }</div>
<div class="line"><a name="l00142"></a><span class="lineno"> 142</span>  </div>
<div class="line"><a name="l00143"></a><span class="lineno"><a class="line" href="levenberg__marquardt_8c.html#a2a72b2b6be15a9998b61a9c0097e3d5f"> 143</a></span> uint8_t <a class="code" href="levenberg__marquardt_8c.html#a2a72b2b6be15a9998b61a9c0097e3d5f">opt_levenberg_marquardt</a>(uint8_t f_length, uint8_t n,</div>
<div class="line"><a name="l00144"></a><span class="lineno"> 144</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> x0_vec[n],</div>
<div class="line"><a name="l00145"></a><span class="lineno"> 145</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> data_vec[f_length],</div>
<div class="line"><a name="l00146"></a><span class="lineno"> 146</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> eps, <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> tau, <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> beta0,</div>
<div class="line"><a name="l00147"></a><span class="lineno"> 147</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> beta1,</div>
<div class="line"><a name="l00148"></a><span class="lineno"> 148</span>  uint8_t max_iter_num,</div>
<div class="line"><a name="l00149"></a><span class="lineno"> 149</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> est_x_vec[n],</div>
<div class="line"><a name="l00150"></a><span class="lineno"> 150</span>  <span class="keywordtype">void</span> (*get_f_error)(<a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> x0_vec[],</div>
<div class="line"><a name="l00151"></a><span class="lineno"> 151</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> data_vec[],</div>
<div class="line"><a name="l00152"></a><span class="lineno"> 152</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> f_vec[]),</div>
<div class="line"><a name="l00153"></a><span class="lineno"> 153</span>  <span class="keywordtype">void</span> (*get_jacobian)(<a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> x0_vec[],</div>
<div class="line"><a name="l00154"></a><span class="lineno"> 154</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> J[][n])</div>
<div class="line"><a name="l00155"></a><span class="lineno"> 155</span>  )</div>
<div class="line"><a name="l00156"></a><span class="lineno"> 156</span> {</div>
<div class="line"><a name="l00157"></a><span class="lineno"> 157</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> J[f_length][n];</div>
<div class="line"><a name="l00158"></a><span class="lineno"> 158</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> JT_f[n];</div>
<div class="line"><a name="l00159"></a><span class="lineno"> 159</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> JTJ_mu2_I[n][n];</div>
<div class="line"><a name="l00160"></a><span class="lineno"> 160</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> s[n];</div>
<div class="line"><a name="l00161"></a><span class="lineno"> 161</span>  <a class="code" href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a> f_vec[f_length];</div>
<div class="line"><a name="l00162"></a><span class="lineno"> 162</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> mu;</div>
<div class="line"><a name="l00163"></a><span class="lineno"> 163</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> ro_mu;</div>
<div class="line"><a name="l00164"></a><span class="lineno"> 164</span>  uint8_t it;</div>
<div class="line"><a name="l00165"></a><span class="lineno"> 165</span>  </div>
<div class="line"><a name="l00166"></a><span class="lineno"> 166</span>  <span class="comment">/*</span></div>
<div class="line"><a name="l00167"></a><span class="lineno"> 167</span> <span class="comment"> * compute mu0 and -(J'J + mu0^2*I)</span></div>
<div class="line"><a name="l00168"></a><span class="lineno"> 168</span> <span class="comment"> */</span></div>
<div class="line"><a name="l00169"></a><span class="lineno"> 169</span>  <span class="comment">//JT_J = J'*J</span></div>
<div class="line"><a name="l00170"></a><span class="lineno"> 170</span>  get_jacobian(x0_vec, J);</div>
<div class="line"><a name="l00171"></a><span class="lineno"> 171</span>  </div>
<div class="line"><a name="l00172"></a><span class="lineno"> 172</span>  <span class="comment">//store J'J in JTJ_mu2_I</span></div>
<div class="line"><a name="l00173"></a><span class="lineno"> 173</span>  <a class="code" href="matrix_8h.html#a0107887a6c40980fa639750be8262f01">matrix_trans_mul_itself</a>(f_length, n, J, JTJ_mu2_I);</div>
<div class="line"><a name="l00174"></a><span class="lineno"> 174</span>  </div>
<div class="line"><a name="l00175"></a><span class="lineno"> 175</span>  <span class="comment">// compute mu_0: mu = mu_0</span></div>
<div class="line"><a name="l00176"></a><span class="lineno"> 176</span>  mu = <a class="code" href="levenberg__marquardt_8c.html#a46fb5a9d6d185ff7f98c9cf34b56e34f">opt_levenberg_marquardt_get_mu0</a>(n, tau, JTJ_mu2_I);</div>
<div class="line"><a name="l00177"></a><span class="lineno"> 177</span>  </div>
<div class="line"><a name="l00178"></a><span class="lineno"> 178</span>  <span class="comment">//compute: J'J + mu0^2*I</span></div>
<div class="line"><a name="l00179"></a><span class="lineno"> 179</span>  <a class="code" href="matrix_8h.html#aa8acafd4181978f536976f8151e51516">matrix_add_to_diag</a>(n, JTJ_mu2_I, n, pow(mu, 2));</div>
<div class="line"><a name="l00180"></a><span class="lineno"> 180</span>  </div>
<div class="line"><a name="l00181"></a><span class="lineno"> 181</span>  <span class="comment">//-(J'*J+mu0^2*eye(n)) is a (n,n) matrix</span></div>
<div class="line"><a name="l00182"></a><span class="lineno"> 182</span>  <a class="code" href="matrix_8h.html#a51139e6c87b602e5e0ebfbc406fda35d">matrix_mul_scalar</a>(n, n, JTJ_mu2_I, -1, JTJ_mu2_I);</div>
<div class="line"><a name="l00183"></a><span class="lineno"> 183</span>  </div>
<div class="line"><a name="l00184"></a><span class="lineno"> 184</span>  <span class="comment">//compute JTF: JT_f = J'*f</span></div>
<div class="line"><a name="l00185"></a><span class="lineno"> 185</span>  get_f_error(x0_vec, data_vec, f_vec);</div>
<div class="line"><a name="l00186"></a><span class="lineno"> 186</span>  <a class="code" href="matrix_8h.html#a53b0a1829b2085414fb66a9e9bd6c7b7">matrix_trans_mul_vec</a>(f_length, n, J, f_length, f_vec, JT_f);</div>
<div class="line"><a name="l00187"></a><span class="lineno"> 187</span>  </div>
<div class="line"><a name="l00188"></a><span class="lineno"> 188</span>  <span class="comment">//solve the equation: s=-(J'*J+mu^2*eye(n))\(J'*F);</span></div>
<div class="line"><a name="l00189"></a><span class="lineno"> 189</span>  <a class="code" href="solve_8h.html#ab6336aff7fbff116a6118b39a0d1e2cd">solve_householder</a>(n, n, JTJ_mu2_I, JT_f, s);</div>
<div class="line"><a name="l00190"></a><span class="lineno"> 190</span>  </div>
<div class="line"><a name="l00191"></a><span class="lineno"> 191</span>  <a class="code" href="vector_8h.html#ae9d3378f23ba835c727245b434b8a7f6">vector_copy</a>(n, x0_vec, est_x_vec);</div>
<div class="line"><a name="l00192"></a><span class="lineno"> 192</span>  </div>
<div class="line"><a name="l00193"></a><span class="lineno"> 193</span>  it = 0;</div>
<div class="line"><a name="l00194"></a><span class="lineno"> 194</span>  <span class="keywordflow">while</span> ((<a class="code" href="vector_8h.html#a4cfc452fdff3e32a601ccff140cd9ae6">vector_get_norm2</a>(n, s)</div>
<div class="line"><a name="l00195"></a><span class="lineno"> 195</span>  > eps * (1 + <a class="code" href="vector_8h.html#a4cfc452fdff3e32a601ccff140cd9ae6">vector_get_norm2</a>(n, est_x_vec)))</div>
<div class="line"><a name="l00196"></a><span class="lineno"> 196</span>  && (it < max_iter_num)) { <span class="comment">//norm(s,2)>eps*(1+norm(x,2))</span></div>
<div class="line"><a name="l00197"></a><span class="lineno"> 197</span>  </div>
<div class="line"><a name="l00198"></a><span class="lineno"> 198</span>  ro_mu = <a class="code" href="levenberg__marquardt_8c.html#a741182c4bc201af44ac010bc4474b826">opt_levenberg_marquardt_correction</a>(f_length, n,</div>
<div class="line"><a name="l00199"></a><span class="lineno"> 199</span>  est_x_vec, data_vec,</div>
<div class="line"><a name="l00200"></a><span class="lineno"> 200</span>  mu, s, get_f_error,</div>
<div class="line"><a name="l00201"></a><span class="lineno"> 201</span>  get_jacobian);</div>
<div class="line"><a name="l00202"></a><span class="lineno"> 202</span>  </div>
<div class="line"><a name="l00203"></a><span class="lineno"> 203</span>  <span class="keywordflow">while</span> (1) {</div>
<div class="line"><a name="l00204"></a><span class="lineno"> 204</span>  <span class="keywordflow">if</span> (ro_mu <= beta0) {</div>
<div class="line"><a name="l00205"></a><span class="lineno"> 205</span>  mu = 2.0 * mu;</div>
<div class="line"><a name="l00206"></a><span class="lineno"> 206</span>  ro_mu = <a class="code" href="levenberg__marquardt_8c.html#a741182c4bc201af44ac010bc4474b826">opt_levenberg_marquardt_correction</a>(</div>
<div class="line"><a name="l00207"></a><span class="lineno"> 207</span>  f_length, n,</div>
<div class="line"><a name="l00208"></a><span class="lineno"> 208</span>  est_x_vec,</div>
<div class="line"><a name="l00209"></a><span class="lineno"> 209</span>  data_vec, mu, s, get_f_error,</div>
<div class="line"><a name="l00210"></a><span class="lineno"> 210</span>  get_jacobian);</div>
<div class="line"><a name="l00211"></a><span class="lineno"> 211</span>  }</div>
<div class="line"><a name="l00212"></a><span class="lineno"> 212</span>  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (ro_mu >= beta1) {</div>
<div class="line"><a name="l00213"></a><span class="lineno"> 213</span>  mu = mu / 2.0;</div>
<div class="line"><a name="l00214"></a><span class="lineno"> 214</span>  <span class="keywordflow">break</span>;</div>
<div class="line"><a name="l00215"></a><span class="lineno"> 215</span>  }</div>
<div class="line"><a name="l00216"></a><span class="lineno"> 216</span>  <span class="keywordflow">else</span> {</div>
<div class="line"><a name="l00217"></a><span class="lineno"> 217</span>  <span class="keywordflow">break</span>;</div>
<div class="line"><a name="l00218"></a><span class="lineno"> 218</span>  }</div>
<div class="line"><a name="l00219"></a><span class="lineno"> 219</span>  }</div>
<div class="line"><a name="l00220"></a><span class="lineno"> 220</span>  <a class="code" href="vector_8h.html#a92917951880002814392c2367896d7a0">vector_add</a>(n, est_x_vec, s, est_x_vec);</div>
<div class="line"><a name="l00221"></a><span class="lineno"> 221</span>  it = it + 1;</div>
<div class="line"><a name="l00222"></a><span class="lineno"> 222</span>  } <span class="comment">//while</span></div>
<div class="line"><a name="l00223"></a><span class="lineno"> 223</span>  </div>
<div class="line"><a name="l00224"></a><span class="lineno"> 224</span>  <span class="keywordflow">return</span> it;</div>
<div class="line"><a name="l00225"></a><span class="lineno"> 225</span> }</div>
<div class="line"><a name="l00226"></a><span class="lineno"> 226</span>  </div>
<div class="line"><a name="l00227"></a><span class="lineno"><a class="line" href="levenberg__marquardt_8c.html#a46fb5a9d6d185ff7f98c9cf34b56e34f"> 227</a></span> <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> <a class="code" href="levenberg__marquardt_8c.html#a46fb5a9d6d185ff7f98c9cf34b56e34f">opt_levenberg_marquardt_get_mu0</a>(uint8_t n, <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> tau,</div>
<div class="line"><a name="l00228"></a><span class="lineno"> 228</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> JTJ[][n])</div>
<div class="line"><a name="l00229"></a><span class="lineno"> 229</span> {</div>
<div class="line"><a name="l00230"></a><span class="lineno"> 230</span>  <a class="code" href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a> max_diag_JTJ = JTJ[0][0];</div>
<div class="line"><a name="l00231"></a><span class="lineno"> 231</span>  </div>
<div class="line"><a name="l00232"></a><span class="lineno"> 232</span>  <span class="keywordflow">for</span> (uint8_t i = 1; i < n; i++) {</div>
<div class="line"><a name="l00233"></a><span class="lineno"> 233</span>  <span class="keywordflow">if</span> (JTJ[i][i] > max_diag_JTJ) {</div>
<div class="line"><a name="l00234"></a><span class="lineno"> 234</span>  max_diag_JTJ = JTJ[i][i];</div>
<div class="line"><a name="l00235"></a><span class="lineno"> 235</span>  }</div>
<div class="line"><a name="l00236"></a><span class="lineno"> 236</span>  }</div>
<div class="line"><a name="l00237"></a><span class="lineno"> 237</span>  </div>
<div class="line"><a name="l00238"></a><span class="lineno"> 238</span>  <span class="keywordflow">return</span> tau * max_diag_JTJ;</div>
<div class="line"><a name="l00239"></a><span class="lineno"> 239</span> }</div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<div class="ttc" id="avector_8h_html_a4cfc452fdff3e32a601ccff140cd9ae6"><div class="ttname"><a href="vector_8h.html#a4cfc452fdff3e32a601ccff140cd9ae6">vector_get_norm2</a></div><div class="ttdeci">vector_t vector_get_norm2(uint8_t length, vector_t arr[])</div><div class="ttdoc">Compute the 2-norm norm of a vector.</div><div class="ttdef"><b>Definition:</b> <a href="vector_8c_source.html#l00042">vector.c:42</a></div></div>
<div class="ttc" id="amatrix_8h_html_ad610bcce232c69ca302550f57b1ad9fb"><div class="ttname"><a href="matrix_8h.html#ad610bcce232c69ca302550f57b1ad9fb">matrix_mul_vec</a></div><div class="ttdeci">void matrix_mul_vec(uint8_t m, uint8_t n, matrix_t matrix[m][n], matrix_t vec[n], matrix_t dst_arr[m])</div><div class="ttdoc">Compute the multiplication of a matrix with a column vector.</div><div class="ttdef"><b>Definition:</b> <a href="matrix_8c_source.html#l00434">matrix.c:434</a></div></div>
<div class="ttc" id="alevenberg__marquardt_8c_html_a2a72b2b6be15a9998b61a9c0097e3d5f"><div class="ttname"><a href="levenberg__marquardt_8c.html#a2a72b2b6be15a9998b61a9c0097e3d5f">opt_levenberg_marquardt</a></div><div class="ttdeci">uint8_t opt_levenberg_marquardt(uint8_t f_length, uint8_t n, vector_t x0_vec[n], vector_t data_vec[f_length], matrix_t eps, matrix_t tau, matrix_t beta0, matrix_t beta1, uint8_t max_iter_num, vector_t est_x_vec[n], void(*get_f_error)(vector_t x0_vec[], vector_t data_vec[], vector_t f_vec[]), void(*get_jacobian)(vector_t x0_vec[], matrix_t J[][n]))</div><div class="ttdoc">Implements the Levenberg–Marquardt (LVM) algorithm.</div><div class="ttdef"><b>Definition:</b> <a href="levenberg__marquardt_8c_source.html#l00143">levenberg_marquardt.c:143</a></div></div>
<div class="ttc" id="avector_8h_html_a5f4b706c07b9b0f7a4983d80fba99e4e"><div class="ttname"><a href="vector_8h.html#a5f4b706c07b9b0f7a4983d80fba99e4e">vector_get_scalar_product</a></div><div class="ttdeci">vector_t vector_get_scalar_product(uint8_t n, vector_t vec1[n], vector_t vec2[n])</div><div class="ttdoc">Compute the dot product of two vectors.</div><div class="ttdef"><b>Definition:</b> <a href="vector_8c_source.html#l00190">vector.c:190</a></div></div>
<div class="ttc" id="asolve_8h_html_ab6336aff7fbff116a6118b39a0d1e2cd"><div class="ttname"><a href="solve_8h.html#ab6336aff7fbff116a6118b39a0d1e2cd">solve_householder</a></div><div class="ttdeci">int8_t solve_householder(uint8_t m, uint8_t n, matrix_t A[][n], matrix_t b[m], matrix_t x_sol[n])</div><div class="ttdoc">Solve an (m n) linear system Ax = b, using the Householder algorithm.</div><div class="ttdef"><b>Definition:</b> <a href="solve_8c_source.html#l00077">solve.c:77</a></div></div>
<div class="ttc" id="avector_8h_html_acb41430bc5720dda7d1c45d91a0b0221"><div class="ttname"><a href="vector_8h.html#acb41430bc5720dda7d1c45d91a0b0221">vector_t</a></div><div class="ttdeci">#define vector_t</div><div class="ttdoc">Define the data type of the vector elements.</div><div class="ttdef"><b>Definition:</b> <a href="vector_8h_source.html#l00033">vector.h:33</a></div></div>
<div class="ttc" id="amatrix_8h_html_a0107887a6c40980fa639750be8262f01"><div class="ttname"><a href="matrix_8h.html#a0107887a6c40980fa639750be8262f01">matrix_trans_mul_itself</a></div><div class="ttdeci">void matrix_trans_mul_itself(uint8_t m, uint8_t n, matrix_t A[m][n], matrix_t AT_mul_A[n][n])</div><div class="ttdoc">Compute the multiplication of the transpose of a matrix with itself.</div><div class="ttdef"><b>Definition:</b> <a href="matrix_8c_source.html#l00545">matrix.c:545</a></div></div>
<div class="ttc" id="amatrix_8h_html_aa8acafd4181978f536976f8151e51516"><div class="ttname"><a href="matrix_8h.html#aa8acafd4181978f536976f8151e51516">matrix_add_to_diag</a></div><div class="ttdeci">void matrix_add_to_diag(uint8_t n, matrix_t A[][n], uint8_t diag_el_num, matrix_t value)</div><div class="ttdoc">Add a number to diagonal elements of a matrix.</div><div class="ttdef"><b>Definition:</b> <a href="matrix_8c_source.html#l00353">matrix.c:353</a></div></div>
<div class="ttc" id="amatrix_8h_html_a53b0a1829b2085414fb66a9e9bd6c7b7"><div class="ttname"><a href="matrix_8h.html#a53b0a1829b2085414fb66a9e9bd6c7b7">matrix_trans_mul_vec</a></div><div class="ttdeci">void matrix_trans_mul_vec(uint8_t m, uint8_t n, matrix_t A[m][n], uint8_t b_size, matrix_t b_vec[m], matrix_t c_vec[n])</div><div class="ttdoc">Compute the multiplication of transposed matrix with column vector.</div><div class="ttdef"><b>Definition:</b> <a href="matrix_8c_source.html#l00511">matrix.c:511</a></div></div>
<div class="ttc" id="asolve_8h_html"><div class="ttname"><a href="solve_8h.html">solve.h</a></div><div class="ttdoc">Enables to solve systems of linear equations Ax = b for x.</div></div>
<div class="ttc" id="amatrix_8h_html"><div class="ttname"><a href="matrix_8h.html">matrix.h</a></div><div class="ttdoc">Matrix computations.</div></div>
<div class="ttc" id="autils_8h_html"><div class="ttname"><a href="utils_8h.html">utils.h</a></div><div class="ttdoc">Utilities for linear algebra.</div></div>
<div class="ttc" id="alevenberg__marquardt_8c_html_a741182c4bc201af44ac010bc4474b826"><div class="ttname"><a href="levenberg__marquardt_8c.html#a741182c4bc201af44ac010bc4474b826">opt_levenberg_marquardt_correction</a></div><div class="ttdeci">matrix_t opt_levenberg_marquardt_correction(uint8_t f_length, uint8_t n, matrix_t x_vec[n], matrix_t data_vec[f_length], matrix_t mu, matrix_t s[n], void(*get_f_error)(vector_t x_vec[], vector_t data_vec[], vector_t f_vec[]), void(*get_jacobian)(vector_t x_vec[], matrix_t J[][n]))</div><div class="ttdoc">Implements the correction-function of the Levenberg–Marquardt (LVM) algorithm.</div><div class="ttdef"><b>Definition:</b> <a href="levenberg__marquardt_8c_source.html#l00053">levenberg_marquardt.c:53</a></div></div>
<div class="ttc" id="amatrix_8h_html_a51139e6c87b602e5e0ebfbc406fda35d"><div class="ttname"><a href="matrix_8h.html#a51139e6c87b602e5e0ebfbc406fda35d">matrix_mul_scalar</a></div><div class="ttdeci">void matrix_mul_scalar(uint8_t m, uint8_t n, matrix_t mat_src[m][n], matrix_t value, matrix_t mat_dest[m][n])</div><div class="ttdoc">Multiply all elements of a matrix with a specified value.</div><div class="ttdef"><b>Definition:</b> <a href="matrix_8c_source.html#l00602">matrix.c:602</a></div></div>
<div class="ttc" id="alevenberg__marquardt_8h_html"><div class="ttname"><a href="levenberg__marquardt_8h.html">levenberg_marquardt.h</a></div><div class="ttdoc">Implement the Levenberg–Marquardt (LVM) algorithm.</div></div>
<div class="ttc" id="avector_8h_html_ae9d3378f23ba835c727245b434b8a7f6"><div class="ttname"><a href="vector_8h.html#ae9d3378f23ba835c727245b434b8a7f6">vector_copy</a></div><div class="ttdeci">void vector_copy(uint8_t size, vector_t src_arr[], vector_t dest_arr[])</div><div class="ttdoc">Copy the elements of the source vector to the destination vector.</div><div class="ttdef"><b>Definition:</b> <a href="vector_8c_source.html#l00037">vector.c:37</a></div></div>
<div class="ttc" id="alevenberg__marquardt_8c_html_a46fb5a9d6d185ff7f98c9cf34b56e34f"><div class="ttname"><a href="levenberg__marquardt_8c.html#a46fb5a9d6d185ff7f98c9cf34b56e34f">opt_levenberg_marquardt_get_mu0</a></div><div class="ttdeci">matrix_t opt_levenberg_marquardt_get_mu0(uint8_t n, matrix_t tau, matrix_t JTJ[][n])</div><div class="ttdoc">Compute the initial value of the Levenberg–Marquardt (LVM) algorithm.</div><div class="ttdef"><b>Definition:</b> <a href="levenberg__marquardt_8c_source.html#l00227">levenberg_marquardt.c:227</a></div></div>
<div class="ttc" id="amatrix_8h_html_af38ac6b76d645fea9abd6caeb4d9dd31"><div class="ttname"><a href="matrix_8h.html#af38ac6b76d645fea9abd6caeb4d9dd31">matrix_t</a></div><div class="ttdeci">#define matrix_t</div><div class="ttdoc">Define the data type of the matrix elements.</div><div class="ttdef"><b>Definition:</b> <a href="matrix_8h_source.html#l00038">matrix.h:38</a></div></div>
<div class="ttc" id="avector_8h_html_a92917951880002814392c2367896d7a0"><div class="ttname"><a href="vector_8h.html#a92917951880002814392c2367896d7a0">vector_add</a></div><div class="ttdeci">void vector_add(uint8_t size, vector_t a_vec[size], vector_t b_vec[size], vector_t a_plus_b_vec[size])</div><div class="ttdoc">Compute the addition of two vectors.</div><div class="ttdef"><b>Definition:</b> <a href="vector_8c_source.html#l00104">vector.c:104</a></div></div>
<div class="ttc" id="avector_8h_html"><div class="ttname"><a href="vector_8h.html">vector.h</a></div><div class="ttdoc">Vector computations.</div></div>
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
<ul>
<li class="navelem"><a class="el" href="dir_943f05c9b1835bace55f73b49add4eb5.html">non_linear_algebra</a></li><li class="navelem"><a class="el" href="dir_a849c678c2f9059c847e621780a4cff4.html">optimization</a></li><li class="navelem"><a class="el" href="levenberg__marquardt_8c.html">levenberg_marquardt.c</a></li>
<li class="footer">Generated by
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.16 </li>
</ul>
</div>
</body>
</html>