Show
Ignore:
Timestamp:
02/18/08 17:50:37 (17 years ago)
Author:
smidl
Message:

upravy Kalmana

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • doc/html/libKF_8h-source.html

    r19 r22  
    1818<a name="l00015"></a>00015 <span class="preprocessor"></span> 
    1919<a name="l00016"></a>00016 <span class="preprocessor">#include &lt;itpp/itbase.h&gt;</span> 
    20 <a name="l00017"></a>00017 <span class="preprocessor">#include "../stat/libBM.h"</span> 
     20<a name="l00017"></a>00017 <span class="preprocessor">#include "../stat/libFN.h"</span> 
    2121<a name="l00018"></a>00018 <span class="preprocessor">#include "../math/libDC.h"</span> 
    2222<a name="l00019"></a>00019  
    … …  
    4545<a name="l00049"></a>00049  
    4646<a name="l00053"></a>00053 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    47 <a name="l00054"></a><a class="code" href="classKalman.html">00054</a> <span class="keyword">class </span><a class="code" href="classKalman.html" title="Kalman filter with covaraince matrices in square root form.">Kalman</a> : <span class="keyword">public</span> <a class="code" href="classBM.html" title="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</a> {  
    48 <a name="l00055"></a>00055         <span class="keywordtype">int</span> dimx, dimy, dimu; 
    49 <a name="l00056"></a>00056         mat A, B, C, D; 
    50 <a name="l00057"></a>00057         sq_T R, Q; 
    51 <a name="l00058"></a>00058          
    52 <a name="l00059"></a>00059         <span class="comment">//cache</span> 
    53 <a name="l00060"></a>00060         mat _K; 
    54 <a name="l00061"></a>00061         vec _yp; 
    55 <a name="l00062"></a>00062         sq_T _Ry,_iRy; 
    56 <a name="l00063"></a>00063 <span class="keyword">public</span>: 
    57 <a name="l00064"></a>00064         <span class="comment">//posterior </span> 
    58 <a name="l00066"></a><a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed">00066</a> <span class="comment"></span>        vec <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>; 
    59 <a name="l00068"></a><a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3">00068</a>         sq_T <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>; 
    60 <a name="l00069"></a>00069  
    61 <a name="l00070"></a>00070 <span class="keyword">public</span>: 
    62 <a name="l00072"></a>00072         <a class="code" href="classKalman.html#83118f4bd2ecbc70b03cfd573088ed6f" title="Full constructor.">Kalman</a> ( mat A0, mat B0, mat C0, mat D0, sq_T R0, sq_T Q0, sq_T P0, vec mu0 ); 
    63 <a name="l00074"></a>00074         <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>);  
    64 <a name="l00075"></a>00075  
    65 <a name="l00076"></a>00076         <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 ); 
    66 <a name="l00077"></a>00077  
    67 <a name="l00078"></a>00078 }; 
    68 <a name="l00079"></a>00079  
    69 <a name="l00081"></a>00081  
    70 <a name="l00082"></a>00082 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    71 <a name="l00083"></a><a class="code" href="classKalman.html#83118f4bd2ecbc70b03cfd573088ed6f">00083</a> <a class="code" href="classKalman.html#83118f4bd2ecbc70b03cfd573088ed6f" title="Full constructor.">Kalman&lt;sq_T&gt;::Kalman</a>( mat A0, mat B0, mat C0, mat D0, sq_T R0, sq_T Q0, sq_T P0, vec mu0 ) { 
    72 <a name="l00084"></a>00084         dimx = A0.rows(); 
    73 <a name="l00085"></a>00085         dimu = B0.cols(); 
    74 <a name="l00086"></a>00086         dimy = C0.rows(); 
    75 <a name="l00087"></a>00087  
    76 <a name="l00088"></a>00088         it_assert_debug( A0.cols()==dimx, <span class="stringliteral">"Kalman: A is not square"</span> ); 
    77 <a name="l00089"></a>00089         it_assert_debug( B0.rows()==dimx, <span class="stringliteral">"Kalman: B is not compatible"</span> ); 
    78 <a name="l00090"></a>00090         it_assert_debug( C0.cols()==dimx, <span class="stringliteral">"Kalman: C is not square"</span> ); 
    79 <a name="l00091"></a>00091         it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     <span class="stringliteral">"Kalman: D is not compatible"</span> ); 
    80 <a name="l00092"></a>00092         it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); 
    81 <a name="l00093"></a>00093         it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); 
    82 <a name="l00094"></a>00094  
    83 <a name="l00095"></a>00095         A = A0; 
    84 <a name="l00096"></a>00096         B = B0; 
    85 <a name="l00097"></a>00097         C = C0; 
    86 <a name="l00098"></a>00098         D = D0; 
    87 <a name="l00099"></a>00099         R = R0; 
    88 <a name="l00100"></a>00100         Q = Q0; 
    89 <a name="l00101"></a>00101         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = mu0; 
    90 <a name="l00102"></a>00102         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> = P0; 
    91 <a name="l00103"></a>00103  
    92 <a name="l00104"></a>00104         <a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a> = 0; 
    93 <a name="l00105"></a>00105 <span class="comment">//Fixme should we assign cache??</span> 
    94 <a name="l00106"></a>00106         _iRy = eye(dimy); <span class="comment">// needed in inv(_iRy)</span> 
    95 <a name="l00107"></a>00107 } 
    96 <a name="l00108"></a>00108  
    97 <a name="l00109"></a>00109 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    98 <a name="l00110"></a><a class="code" href="classKalman.html#e945d9205ca14acbd83ba80ea6f72b8e">00110</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) { 
    99 <a name="l00111"></a>00111         it_assert_debug( dt.length()==( dimy+dimu ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
    100 <a name="l00112"></a>00112  
    101 <a name="l00113"></a>00113         vec u = dt.get( dimy,dimy+dimu-1 ); 
    102 <a name="l00114"></a>00114         vec y = dt.get( 0,dimy-1 ); 
    103 <a name="l00115"></a>00115         <span class="comment">//Time update</span> 
    104 <a name="l00116"></a>00116         <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; 
    105 <a name="l00117"></a>00117         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
    106 <a name="l00118"></a>00118         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.mult_sym( A ); 
    107 <a name="l00119"></a>00119         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>+=Q; 
    108 <a name="l00120"></a>00120  
    109 <a name="l00121"></a>00121         <span class="comment">//Data update</span> 
    110 <a name="l00122"></a>00122         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
    111 <a name="l00123"></a>00123         _Ry.mult_sym( C, <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>); 
    112 <a name="l00124"></a>00124         _Ry+=R; 
    113 <a name="l00125"></a>00125  
    114 <a name="l00126"></a>00126         mat Pfull = <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>.to_mat(); 
    115 <a name="l00127"></a>00127          
    116 <a name="l00128"></a>00128         _Ry.inv( _iRy ); <span class="comment">// result is in _iRy;</span> 
    117 <a name="l00129"></a>00129         _K = Pfull*C.transpose()*(_iRy.to_mat()); 
    118 <a name="l00130"></a>00130         <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> 
    119 <a name="l00131"></a>00131         _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> 
    120 <a name="l00132"></a>00132         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> += _K*( _yp ); 
    121 <a name="l00133"></a>00133          
    122 <a name="l00134"></a>00134         <span class="keywordflow">if</span> (evalll==<span class="keyword">true</span>) { 
    123 <a name="l00135"></a>00135         <a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a>+= -0.5*(_Ry.cols()*0.79817986835811504957 \ 
    124 <a name="l00136"></a>00136         +_Ry.logdet() +_iRy.qform(_yp)); 
    125 <a name="l00137"></a>00137         } 
    126 <a name="l00138"></a>00138 }; 
     47<a name="l00054"></a><a class="code" href="classKalman.html">00054</a> <span class="keyword">class </span><a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a> : <span class="keyword">public</span> <a class="code" href="classBM.html" title="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</a> {  
     48<a name="l00055"></a>00055 <span class="keyword">protected</span>: 
     49<a name="l00056"></a>00056         <span class="keywordtype">int</span> dimx, dimy, dimu; 
     50<a name="l00057"></a>00057         mat A, B, C, D; 
     51<a name="l00058"></a>00058         sq_T R, Q; 
     52<a name="l00059"></a>00059          
     53<a name="l00060"></a>00060         <span class="comment">//cache</span> 
     54<a name="l00061"></a>00061         mat _K; 
     55<a name="l00062"></a>00062         vec _yp; 
     56<a name="l00063"></a>00063         sq_T _Ry,_iRy; 
     57<a name="l00064"></a>00064 <span class="keyword">public</span>: 
     58<a name="l00065"></a>00065         <span class="comment">//posterior </span> 
     59<a name="l00067"></a><a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed">00067</a> <span class="comment"></span>        vec <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a>; 
     60<a name="l00069"></a><a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3">00069</a>         sq_T <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a>; 
     61<a name="l00070"></a>00070  
     62<a name="l00071"></a>00071 <span class="keyword">public</span>: 
     63<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); 
     64<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="l00078"></a>00078  
     67<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 ); 
     68<a name="l00080"></a>00080  
     69<a name="l00081"></a>00081 }; 
     70<a name="l00082"></a>00082  
     71<a name="l00088"></a>00088 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     72<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; 
     75<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>);     
     78<a name="l00099"></a>00099 }; 
     79<a name="l00100"></a>00100  
     80<a name="l00102"></a>00102  
     81<a name="l00103"></a>00103 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     82<a name="l00104"></a><a class="code" href="classKalman.html#96958a5ebfa966d892137987f265083a">00104</a> <a class="code" href="classKalman.html#96958a5ebfa966d892137987f265083a" title="Default constructor.">Kalman&lt;sq_T&gt;::Kalman</a>( <span class="keywordtype">int</span> dx, <span class="keywordtype">int</span> du, <span class="keywordtype">int</span> dy): <a class="code" href="classBM.html" title="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</a>(), dimx(dx),dimy(dy),dimu(du){ 
     83<a name="l00105"></a>00105         A = mat(dimx,dimx); 
     84<a name="l00106"></a>00106         B = mat(dimx,dimu); 
     85<a name="l00107"></a>00107         C = mat(dimy,dimx); 
     86<a name="l00108"></a>00108         D = mat(dimy,dimu); 
     87<a name="l00109"></a>00109  
     88<a name="l00110"></a>00110         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = vec(dimx); 
     89<a name="l00111"></a>00111         <span class="comment">//TODO Initialize the rest?</span> 
     90<a name="l00112"></a>00112 }; 
     91<a name="l00113"></a>00113  
     92<a name="l00114"></a>00114 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     93<a name="l00115"></a><a class="code" href="classKalman.html#83118f4bd2ecbc70b03cfd573088ed6f">00115</a> <a class="code" href="classKalman.html#96958a5ebfa966d892137987f265083a" title="Default constructor.">Kalman&lt;sq_T&gt;::Kalman</a>(<span class="keyword">const</span>  mat A0,<span class="keyword">const</span>  mat B0, <span class="keyword">const</span> mat C0, <span class="keyword">const</span> mat D0, <span class="keyword">const</span> sq_T R0, <span class="keyword">const</span> sq_T Q0, <span class="keyword">const</span> sq_T P0, <span class="keyword">const</span> vec mu0 ): <a class="code" href="classBM.html" title="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</a>() { 
     94<a name="l00116"></a>00116         dimx = A0.rows(); 
     95<a name="l00117"></a>00117         dimu = B0.cols(); 
     96<a name="l00118"></a>00118         dimy = C0.rows(); 
     97<a name="l00119"></a>00119  
     98<a name="l00120"></a>00120         it_assert_debug( A0.cols()==dimx, <span class="stringliteral">"Kalman: A is not square"</span> ); 
     99<a name="l00121"></a>00121         it_assert_debug( B0.rows()==dimx, <span class="stringliteral">"Kalman: B is not compatible"</span> ); 
     100<a name="l00122"></a>00122         it_assert_debug( C0.cols()==dimx, <span class="stringliteral">"Kalman: C is not square"</span> ); 
     101<a name="l00123"></a>00123         it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     <span class="stringliteral">"Kalman: D is not compatible"</span> ); 
     102<a name="l00124"></a>00124         it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); 
     103<a name="l00125"></a>00125         it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); 
     104<a name="l00126"></a>00126  
     105<a name="l00127"></a>00127         A = A0; 
     106<a name="l00128"></a>00128         B = B0; 
     107<a name="l00129"></a>00129         C = C0; 
     108<a name="l00130"></a>00130         D = D0; 
     109<a name="l00131"></a>00131         R = R0; 
     110<a name="l00132"></a>00132         Q = Q0; 
     111<a name="l00133"></a>00133         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = mu0; 
     112<a name="l00134"></a>00134         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> = P0; 
     113<a name="l00135"></a>00135  
     114<a name="l00136"></a>00136 <span class="comment">//Fixme should we assign cache??</span> 
     115<a name="l00137"></a>00137         _iRy = eye(dimy); <span class="comment">// needed in inv(_iRy)</span> 
     116<a name="l00138"></a>00138 } 
    127117<a name="l00139"></a>00139  
    128 <a name="l00140"></a>00140 <span class="comment">//extern template class Kalman&lt;ldmat&gt;; </span> 
    129 <a name="l00141"></a>00141  
    130 <a name="l00142"></a>00142  
    131 <a name="l00143"></a>00143 <span class="preprocessor">#endif // KF_H</span> 
    132 <a name="l00144"></a>00144 <span class="preprocessor"></span> 
    133 <a name="l00145"></a>00145  
    134 </pre></div><hr size="1"><address style="text-align: right;"><small>Generated on Fri Feb 15 18:57:36 2008 for mixpp by&nbsp; 
     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  
     149<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> ); 
     161<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; 
    135197<a href="http://www.doxygen.org/index.html"> 
    136198<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.3 </small></address>