Show
Ignore:
Timestamp:
02/22/08 16:40:12 (18 years ago)
Author:
smidl
Message:

prelozitelna verze

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • doc/html/libKF_8h-source.html

    r22 r28  
    1111    <li><a href="annotated.html"><span>Classes</span></a></li> 
    1212    <li class="current"><a href="files.html"><span>Files</span></a></li> 
     13    <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li> 
    1314  </ul> 
    1415</div> 
    … …  
    3738<a name="l00039"></a>00039 <span class="keyword">public</span>: 
    3839<a name="l00041"></a>00041         <a class="code" href="classKalmanFull.html#7197ab6e7380790006394eabd3b97043" title="Full constructor.">KalmanFull</a> ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0); 
    39 <a name="l00043"></a>00043         <span class="keywordtype">void</span> <a class="code" href="classKalmanFull.html#048b13739b94c331cda08249b278552b" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt, <span class="keywordtype">bool</span> evalll=<span class="keyword">true</span>);  
     40<a name="l00043"></a>00043         <span class="keywordtype">void</span> <a class="code" href="classKalmanFull.html#048b13739b94c331cda08249b278552b" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt, <span class="keywordtype">bool</span> <a class="code" href="classBM.html#bf6fb59b30141074f8ee1e2f43d03129" title="If true, the filter will compute likelihood of the data record and store it in ll...">evalll</a>=<span class="keyword">true</span>);  
    4041<a name="l00044"></a>00044  
    4142<a name="l00045"></a>00045         <span class="keyword">friend</span> std::ostream &amp;operator&lt;&lt; ( std::ostream &amp;os, <span class="keyword">const</span> <a class="code" href="classKalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> &amp;kf ); 
    … …  
    6364<a name="l00073"></a>00073         <a class="code" href="classKalman.html#96958a5ebfa966d892137987f265083a" title="Default constructor.">Kalman</a> (<span class="keywordtype">int</span> dimx, <span class="keywordtype">int</span> dimu, <span class="keywordtype">int</span> dimy); 
    6465<a name="l00075"></a>00075         <a class="code" href="classKalman.html#96958a5ebfa966d892137987f265083a" title="Default constructor.">Kalman</a> ( mat A0, mat B0, mat C0, mat D0, sq_T R0, sq_T Q0, sq_T P0, vec mu0 ); 
    65 <a name="l00077"></a>00077         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#e945d9205ca14acbd83ba80ea6f72b8e" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt, <span class="keywordtype">bool</span> evalll=<span class="keyword">true</span>);  
     66<a name="l00077"></a>00077         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#e945d9205ca14acbd83ba80ea6f72b8e" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt, <span class="keywordtype">bool</span> <a class="code" href="classBM.html#bf6fb59b30141074f8ee1e2f43d03129" title="If true, the filter will compute likelihood of the data record and store it in ll...">evalll</a>=<span class="keyword">true</span>);  
    6667<a name="l00078"></a>00078  
    6768<a name="l00079"></a>00079         <span class="keyword">friend</span> std::ostream &amp;operator&lt;&lt; ( std::ostream &amp;os, <span class="keyword">const</span> <a class="code" href="classKalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> &amp;kf ); 
    … …  
    7172<a name="l00088"></a>00088 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    7273<a name="l00089"></a><a class="code" href="classEKF.html">00089</a> <span class="keyword">class </span><a class="code" href="classEKF.html" title="Extended Kalman Filter.">EKF</a> : <span class="keyword">public</span> <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;fsqmat&gt; { 
    73 <a name="l00091"></a>00091         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a> fxu; 
    74 <a name="l00093"></a>00093         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a> hxu; 
     74<a name="l00091"></a>00091         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* pfxu; 
     75<a name="l00093"></a>00093         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* phxu; 
    7576<a name="l00094"></a>00094 <span class="keyword">public</span>:  
    76 <a name="l00096"></a>00096         <a class="code" href="classEKF.html#ec441d41529eeae4a1309426386b4a10" title="Default constructor.">EKF</a> (<span class="keyword">const</span> <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a> fxu, <span class="keyword">const</span> <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a> hxu); 
    77 <a name="l00098"></a>00098         <span class="keywordtype">void</span> <a class="code" href="classEKF.html#fb0a08463f14e5584344ea2df99fe747" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt, <span class="keywordtype">bool</span> evalll=<span class="keyword">true</span>);     
     77<a name="l00096"></a>00096         <a class="code" href="classEKF.html#003687c6cf2a01be90a00e2c99e3863e" title="Default constructor.">EKF</a> (<a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* pfxu, <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* phxu, sq_T Q0, sq_T R0, vec mu0, mat P0); 
     78<a name="l00098"></a>00098         <span class="keywordtype">void</span> <a class="code" href="classEKF.html#fb0a08463f14e5584344ea2df99fe747" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt, <span class="keywordtype">bool</span> <a class="code" href="classBM.html#bf6fb59b30141074f8ee1e2f43d03129" title="If true, the filter will compute likelihood of the data record and store it in ll...">evalll</a>=<span class="keyword">true</span>);     
    7879<a name="l00099"></a>00099 }; 
    7980<a name="l00100"></a>00100  
    … …  
    114115<a name="l00136"></a>00136 <span class="comment">//Fixme should we assign cache??</span> 
    115116<a name="l00137"></a>00137         _iRy = eye(dimy); <span class="comment">// needed in inv(_iRy)</span> 
    116 <a name="l00138"></a>00138 } 
    117 <a name="l00139"></a>00139  
    118 <a name="l00140"></a>00140 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    119 <a name="l00141"></a><a class="code" href="classKalman.html#e945d9205ca14acbd83ba80ea6f72b8e">00141</a> <span class="keywordtype">void</span> <a class="code" href="classKalman.html#e945d9205ca14acbd83ba80ea6f72b8e" title="Here dt = [yt;ut] of appropriate dimensions.">Kalman&lt;sq_T&gt;::bayes</a>( <span class="keyword">const</span> vec &amp;dt , <span class="keywordtype">bool</span> evalll) { 
    120 <a name="l00142"></a>00142         it_assert_debug( dt.length()==( dimy+dimu ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
    121 <a name="l00143"></a>00143  
    122 <a name="l00144"></a>00144         vec u = dt.get( dimy,dimy+dimu-1 ); 
    123 <a name="l00145"></a>00145         vec y = dt.get( 0,dimy-1 ); 
    124 <a name="l00146"></a>00146         <span class="comment">//Time update</span> 
    125 <a name="l00147"></a>00147         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = A*<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> + B*u; 
    126 <a name="l00148"></a>00148         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
    127 <a name="l00149"></a>00149         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.mult_sym( A ); 
    128 <a name="l00150"></a>00150         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>+=Q; 
    129 <a name="l00151"></a>00151  
    130 <a name="l00152"></a>00152         <span class="comment">//Data update</span> 
    131 <a name="l00153"></a>00153         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
    132 <a name="l00154"></a>00154         _Ry.mult_sym( C, <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>); 
    133 <a name="l00155"></a>00155         _Ry+=R; 
    134 <a name="l00156"></a>00156  
    135 <a name="l00157"></a>00157         mat Pfull = <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.to_mat(); 
    136 <a name="l00158"></a>00158          
    137 <a name="l00159"></a>00159         _Ry.inv( _iRy ); <span class="comment">// result is in _iRy;</span> 
    138 <a name="l00160"></a>00160         _K = Pfull*C.transpose()*(_iRy.to_mat()); 
    139 <a name="l00161"></a>00161         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> -= _K*C*Pfull; <span class="comment">// P = P -KCP;</span> 
    140 <a name="l00162"></a>00162         _yp = y-C*<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>-D*u; <span class="comment">//y prediction</span> 
    141 <a name="l00163"></a>00163         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> += _K*( _yp ); 
    142 <a name="l00164"></a>00164          
    143 <a name="l00165"></a>00165         <span class="keywordflow">if</span> (evalll==<span class="keyword">true</span>) { 
    144 <a name="l00166"></a>00166         <a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a>+= -0.5*(_Ry.cols()*0.79817986835811504957 \ 
    145 <a name="l00167"></a>00167         +_Ry.logdet() +_iRy.qform(_yp)); 
    146 <a name="l00168"></a>00168         } 
    147 <a name="l00169"></a>00169 }; 
    148 <a name="l00170"></a>00170  
     117<a name="l00138"></a>00138         _Ry = eye(dimy); <span class="comment">// needed in inv(_iRy)</span> 
     118<a name="l00139"></a>00139 } 
     119<a name="l00140"></a>00140  
     120<a name="l00141"></a>00141 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     121<a name="l00142"></a><a class="code" href="classKalman.html#e945d9205ca14acbd83ba80ea6f72b8e">00142</a> <span class="keywordtype">void</span> <a class="code" href="classKalman.html#e945d9205ca14acbd83ba80ea6f72b8e" title="Here dt = [yt;ut] of appropriate dimensions.">Kalman&lt;sq_T&gt;::bayes</a>( <span class="keyword">const</span> vec &amp;dt , <span class="keywordtype">bool</span> <a class="code" href="classBM.html#bf6fb59b30141074f8ee1e2f43d03129" title="If true, the filter will compute likelihood of the data record and store it in ll...">evalll</a>) { 
     122<a name="l00143"></a>00143         it_assert_debug( dt.length()==( dimy+dimu ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
     123<a name="l00144"></a>00144  
     124<a name="l00145"></a>00145         vec u = dt.get( dimy,dimy+dimu-1 ); 
     125<a name="l00146"></a>00146         vec y = dt.get( 0,dimy-1 ); 
     126<a name="l00147"></a>00147         <span class="comment">//Time update</span> 
     127<a name="l00148"></a>00148         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = A*<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> + B*u; 
     128<a name="l00149"></a>00149         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
     129<a name="l00150"></a>00150         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.mult_sym( A ); 
     130<a name="l00151"></a>00151         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>+=Q; 
     131<a name="l00152"></a>00152  
     132<a name="l00153"></a>00153         <span class="comment">//Data update</span> 
     133<a name="l00154"></a>00154         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
     134<a name="l00155"></a>00155         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.mult_sym( C, _Ry); 
     135<a name="l00156"></a>00156         _Ry+=R; 
     136<a name="l00157"></a>00157  
     137<a name="l00158"></a>00158         mat Pfull = <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.to_mat(); 
     138<a name="l00159"></a>00159          
     139<a name="l00160"></a>00160         _Ry.inv( _iRy ); <span class="comment">// result is in _iRy;</span> 
     140<a name="l00161"></a>00161         _K = Pfull*C.transpose()*(_iRy.to_mat()); 
     141<a name="l00162"></a>00162         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> -= _K*C*Pfull; <span class="comment">// P = P -KCP;</span> 
     142<a name="l00163"></a>00163         _yp = y-C*<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>-D*u; <span class="comment">//y prediction</span> 
     143<a name="l00164"></a>00164         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> += _K*( _yp ); 
     144<a name="l00165"></a>00165          
     145<a name="l00166"></a>00166         <span class="keywordflow">if</span> (evalll==<span class="keyword">true</span>) { 
     146<a name="l00167"></a>00167         <a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a>+= -0.5*(_Ry.cols()*0.79817986835811504957 \ 
     147<a name="l00168"></a>00168         +_Ry.logdet() +_iRy.qform(_yp)); 
     148<a name="l00169"></a>00169         } 
     149<a name="l00170"></a>00170 }; 
    149150<a name="l00171"></a>00171  
    150 <a name="l00172"></a>00172 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    151 <a name="l00173"></a><a class="code" href="classEKF.html#ec441d41529eeae4a1309426386b4a10">00173</a> <a class="code" href="classEKF.html#ec441d41529eeae4a1309426386b4a10" title="Default constructor.">EKF&lt;sq_T&gt;::EKF</a>(<span class="keyword">const</span> <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a> fxu0, <span class="keyword">const</span> <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a> hxu0): fxu(fxu0), hxu(hxu0),<a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;<a class="code" href="classfsqmat.html" title="Fake sqmat. This class maps sqmat operations to operations on full matrix.">fsqmat</a>&gt;(fxu0._dimx(),fxu0._dimu(),hxu0._dimy()) { 
    152 <a name="l00174"></a>00174                  
    153 <a name="l00175"></a>00175                 <span class="comment">//initialize matrices A C, later, these will be only updated!</span> 
    154 <a name="l00176"></a>00176                 fxu.<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,zeros(dimu),A,<span class="keyword">true</span>); 
    155 <a name="l00177"></a>00177                 hxu.<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,zeros(dimu),C,<span class="keyword">true</span>); 
    156 <a name="l00178"></a>00178 } 
    157 <a name="l00179"></a>00179  
    158 <a name="l00180"></a>00180 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    159 <a name="l00181"></a><a class="code" href="classEKF.html#fb0a08463f14e5584344ea2df99fe747">00181</a> <span class="keywordtype">void</span> <a class="code" href="classEKF.html#fb0a08463f14e5584344ea2df99fe747" title="Here dt = [yt;ut] of appropriate dimensions.">EKF&lt;sq_T&gt;::bayes</a>( <span class="keyword">const</span> vec &amp;dt , <span class="keywordtype">bool</span> evalll) { 
    160 <a name="l00182"></a>00182         it_assert_debug( dt.length()==( dimy+dimu ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
     151<a name="l00172"></a>00172 <span class="comment">//TODO why not const pointer??</span> 
     152<a name="l00173"></a>00173  
     153<a name="l00174"></a>00174 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     154<a name="l00175"></a><a class="code" href="classEKF.html#003687c6cf2a01be90a00e2c99e3863e">00175</a> <a class="code" href="classEKF.html#003687c6cf2a01be90a00e2c99e3863e" title="Default constructor.">EKF&lt;sq_T&gt;::EKF</a>( <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* pfxu0,  <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* phxu0, sq_T Q0, sq_T R0, vec mu0, mat P0): pfxu(pfxu0), phxu(phxu0), <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;<a class="code" href="classfsqmat.html" title="Fake sqmat. This class maps sqmat operations to operations on full matrix.">fsqmat</a>&gt;(pfxu0-&gt;_dimx(),pfxu0-&gt;_dimu(),phxu0-&gt;_dimy()) { 
     155<a name="l00176"></a>00176                  
     156<a name="l00177"></a>00177                 <span class="comment">//initialize matrices A C, later, these will be only updated!</span> 
     157<a name="l00178"></a>00178                 pfxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,zeros(dimu),A,<span class="keyword">true</span>); 
     158<a name="l00179"></a>00179                 pfxu-&gt;<a class="code" href="classdiffbifn.html#1978bafd7909d15c139a08c495c24aa0" title="Evaluates  and writes result into A .">dfdu_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,zeros(dimu),B,<span class="keyword">true</span>); 
     159<a name="l00180"></a>00180                 phxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,zeros(dimu),C,<span class="keyword">true</span>); 
     160<a name="l00181"></a>00181                 phxu-&gt;<a class="code" href="classdiffbifn.html#1978bafd7909d15c139a08c495c24aa0" title="Evaluates  and writes result into A .">dfdu_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,zeros(dimu),D,<span class="keyword">true</span>); 
     161<a name="l00182"></a>00182                  
    161162<a name="l00183"></a>00183  
    162 <a name="l00184"></a>00184         vec u = dt.get( dimy,dimy+dimu-1 ); 
    163 <a name="l00185"></a>00185         vec y = dt.get( 0,dimy-1 ); 
    164 <a name="l00186"></a>00186         <span class="comment">//Time update</span> 
    165 <a name="l00187"></a>00187         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = fxu.<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates $f(x0,u0)$ (VS: Do we really need common eval? ).">eval</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>, u); 
    166 <a name="l00188"></a>00188         fxu.<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,u,A,<span class="keyword">false</span>); <span class="comment">//update A by a derivative of fx</span> 
    167 <a name="l00189"></a>00189          
    168 <a name="l00190"></a>00190         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
    169 <a name="l00191"></a>00191         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.<a class="code" href="classfsqmat.html#acc5d2d0a243f1de6d0106065f01f518" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a>( A ); 
    170 <a name="l00192"></a>00192         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>+=Q; 
    171 <a name="l00193"></a>00193  
    172 <a name="l00194"></a>00194         <span class="comment">//Data update</span> 
    173 <a name="l00195"></a>00195         hxu.<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,u,C,<span class="keyword">false</span>); <span class="comment">//update C by a derivative hx</span> 
    174 <a name="l00196"></a>00196         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
    175 <a name="l00197"></a>00197         _Ry.<a class="code" href="classfsqmat.html#acc5d2d0a243f1de6d0106065f01f518" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a>( C, <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>); 
    176 <a name="l00198"></a>00198         _Ry+=R; 
    177 <a name="l00199"></a>00199  
    178 <a name="l00200"></a>00200         mat Pfull = <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.<a class="code" href="classfsqmat.html#cedf4f048309056f4262c930914dfda8" title="Conversion to full matrix.">to_mat</a>(); 
    179 <a name="l00201"></a>00201          
    180 <a name="l00202"></a>00202         _Ry.<a class="code" href="classfsqmat.html#9fa853e1ca28f2a1a1c43377e798ecb1">inv</a>( _iRy ); <span class="comment">// result is in _iRy;</span> 
    181 <a name="l00203"></a>00203         _K = Pfull*C.transpose()*(_iRy.<a class="code" href="classfsqmat.html#cedf4f048309056f4262c930914dfda8" title="Conversion to full matrix.">to_mat</a>()); 
    182 <a name="l00204"></a>00204         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> -= _K*C*Pfull; <span class="comment">// P = P -KCP;</span> 
    183 <a name="l00205"></a>00205         _yp = y-hxu.<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates $f(x0,u0)$ (VS: Do we really need common eval? ).">eval</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,u); <span class="comment">//y prediction</span> 
    184 <a name="l00206"></a>00206         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> += _K*( _yp ); 
    185 <a name="l00207"></a>00207          
    186 <a name="l00208"></a>00208         <span class="keywordflow">if</span> (evalll==<span class="keyword">true</span>) { 
    187 <a name="l00209"></a>00209         <a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a>+= -0.5*(_Ry.<a class="code" href="classsqmat.html#ecc2e2540f95a04f4449842588170f5b" title="Reimplementing common functions of mat: cols().">cols</a>()*0.79817986835811504957 \ 
    188 <a name="l00210"></a>00210         +_Ry.<a class="code" href="classfsqmat.html#bf212272ec195ad2706e2bf4d8e7c9b3" title="Logarithm of a determinant.">logdet</a>() +_iRy.<a class="code" href="classfsqmat.html#6d047b9f7a27dfc093303a13cc9b1fba" title="Evaluates quadratic form $x= v&amp;#39;*V*v$;.">qform</a>(_yp)); 
    189 <a name="l00211"></a>00211         } 
    190 <a name="l00212"></a>00212 }; 
    191 <a name="l00213"></a>00213  
    192 <a name="l00214"></a>00214  
    193 <a name="l00215"></a>00215 <span class="preprocessor">#endif // KF_H</span> 
    194 <a name="l00216"></a>00216 <span class="preprocessor"></span> 
    195 <a name="l00217"></a>00217  
    196 </pre></div><hr size="1"><address style="text-align: right;"><small>Generated on Sun Feb 17 16:14:14 2008 for mixpp by&nbsp; 
     163<a name="l00184"></a>00184                 R = R0; 
     164<a name="l00185"></a>00185                 Q = Q0; 
     165<a name="l00186"></a>00186                 <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = mu0; 
     166<a name="l00187"></a>00187                 <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> = P0; 
     167<a name="l00188"></a>00188  
     168<a name="l00189"></a>00189         <span class="keyword">using</span> std::cout; 
     169<a name="l00190"></a>00190         cout&lt;&lt;A&lt;&lt;std::endl; 
     170<a name="l00191"></a>00191         cout&lt;&lt;B&lt;&lt;std::endl; 
     171<a name="l00192"></a>00192         cout&lt;&lt;C&lt;&lt;std::endl; 
     172<a name="l00193"></a>00193         cout&lt;&lt;D&lt;&lt;std::endl; 
     173<a name="l00194"></a>00194  
     174<a name="l00195"></a>00195 } 
     175<a name="l00196"></a>00196  
     176<a name="l00197"></a>00197 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     177<a name="l00198"></a><a class="code" href="classEKF.html#fb0a08463f14e5584344ea2df99fe747">00198</a> <span class="keywordtype">void</span> <a class="code" href="classEKF.html#fb0a08463f14e5584344ea2df99fe747" title="Here dt = [yt;ut] of appropriate dimensions.">EKF&lt;sq_T&gt;::bayes</a>( <span class="keyword">const</span> vec &amp;dt , <span class="keywordtype">bool</span> <a class="code" href="classBM.html#bf6fb59b30141074f8ee1e2f43d03129" title="If true, the filter will compute likelihood of the data record and store it in ll...">evalll</a>) { 
     178<a name="l00199"></a>00199         it_assert_debug( dt.length()==( dimy+dimu ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
     179<a name="l00200"></a>00200  
     180<a name="l00201"></a>00201         vec u = dt.get( dimy,dimy+dimu-1 ); 
     181<a name="l00202"></a>00202         vec y = dt.get( 0,dimy-1 ); 
     182<a name="l00203"></a>00203         <span class="comment">//Time update</span> 
     183<a name="l00204"></a>00204         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = pfxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates $f(x0,u0)$ (VS: Do we really need common eval? ).">eval</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>, u); 
     184<a name="l00205"></a>00205         pfxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,u,A,<span class="keyword">false</span>); <span class="comment">//update A by a derivative of fx</span> 
     185<a name="l00206"></a>00206          
     186<a name="l00207"></a>00207         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
     187<a name="l00208"></a>00208         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.<a class="code" href="classfsqmat.html#acc5d2d0a243f1de6d0106065f01f518" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a>( A ); 
     188<a name="l00209"></a>00209         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>+=Q; 
     189<a name="l00210"></a>00210  
     190<a name="l00211"></a>00211         <span class="comment">//Data update</span> 
     191<a name="l00212"></a>00212         phxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,u,C,<span class="keyword">false</span>); <span class="comment">//update C by a derivative hx</span> 
     192<a name="l00213"></a>00213         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
     193<a name="l00214"></a>00214         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.<a class="code" href="classfsqmat.html#acc5d2d0a243f1de6d0106065f01f518" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a>( C, _Ry); 
     194<a name="l00215"></a>00215         _Ry+=R; 
     195<a name="l00216"></a>00216  
     196<a name="l00217"></a>00217         mat Pfull = <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.<a class="code" href="classfsqmat.html#cedf4f048309056f4262c930914dfda8" title="Conversion to full matrix.">to_mat</a>(); 
     197<a name="l00218"></a>00218          
     198<a name="l00219"></a>00219         _Ry.<a class="code" href="classfsqmat.html#9fa853e1ca28f2a1a1c43377e798ecb1" title="Matrix inversion preserving the chosen form.">inv</a>( _iRy ); <span class="comment">// result is in _iRy;</span> 
     199<a name="l00220"></a>00220         _K = Pfull*C.transpose()*(_iRy.<a class="code" href="classfsqmat.html#cedf4f048309056f4262c930914dfda8" title="Conversion to full matrix.">to_mat</a>()); 
     200<a name="l00221"></a>00221         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> -= _K*C*Pfull; <span class="comment">// P = P -KCP;</span> 
     201<a name="l00222"></a>00222         _yp = y-phxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates $f(x0,u0)$ (VS: Do we really need common eval? ).">eval</a>(<a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>,u); <span class="comment">//y prediction</span> 
     202<a name="l00223"></a>00223         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> += _K*( _yp ); 
     203<a name="l00224"></a>00224          
     204<a name="l00225"></a>00225         <span class="keywordflow">if</span> (evalll==<span class="keyword">true</span>) { 
     205<a name="l00226"></a>00226         <a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a>+= -0.5*(_Ry.<a class="code" href="classsqmat.html#ecc2e2540f95a04f4449842588170f5b" title="Reimplementing common functions of mat: cols().">cols</a>()*0.79817986835811504957 \ 
     206<a name="l00227"></a>00227         +_Ry.<a class="code" href="classfsqmat.html#bf212272ec195ad2706e2bf4d8e7c9b3" title="Logarithm of a determinant.">logdet</a>() +_iRy.<a class="code" href="classfsqmat.html#6d047b9f7a27dfc093303a13cc9b1fba" title="Evaluates quadratic form $x= v&amp;#39;*V*v$;.">qform</a>(_yp)); 
     207<a name="l00228"></a>00228         } 
     208<a name="l00229"></a>00229 }; 
     209<a name="l00230"></a>00230  
     210<a name="l00231"></a>00231  
     211<a name="l00232"></a>00232 <span class="preprocessor">#endif // KF_H</span> 
     212<a name="l00233"></a>00233 <span class="preprocessor"></span> 
     213<a name="l00234"></a>00234  
     214</pre></div><hr size="1"><address style="text-align: right;"><small>Generated on Mon Feb 18 21:48:39 2008 for mixpp by&nbsp; 
    197215<a href="http://www.doxygen.org/index.html"> 
    198216<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.3 </small></address>