Show
Ignore:
Timestamp:
03/03/08 13:00:32 (18 years ago)
Author:
smidl
Message:

test KF : estimation of R in KF is not possible! Likelihood of y_t is growing when R -> 0

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • doc/html/libKF_8h-source.html

    r28 r32  
    2020<a name="l00016"></a>00016 <span class="preprocessor">#include &lt;itpp/itbase.h&gt;</span> 
    2121<a name="l00017"></a>00017 <span class="preprocessor">#include "../stat/libFN.h"</span> 
    22 <a name="l00018"></a>00018 <span class="preprocessor">#include "../math/libDC.h"</span> 
     22<a name="l00018"></a>00018 <span class="preprocessor">#include "../stat/libEF.h"</span> 
    2323<a name="l00019"></a>00019  
    2424<a name="l00020"></a>00020  
    2525<a name="l00021"></a>00021 <span class="keyword">using namespace </span>itpp; 
    2626<a name="l00022"></a>00022  
    27 <a name="l00026"></a><a class="code" href="classKalmanFull.html">00026</a> <span class="keyword">class </span><a class="code" href="classKalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</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> {  
     27<a name="l00026"></a><a class="code" href="classKalmanFull.html">00026</a> <span class="keyword">class </span><a class="code" href="classKalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> {  
    2828<a name="l00027"></a>00027         <span class="keywordtype">int</span> dimx, dimy, dimu; 
    2929<a name="l00028"></a>00028         mat A, B, C, D, R, Q; 
    … …  
    3838<a name="l00039"></a>00039 <span class="keyword">public</span>: 
    3939<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); 
    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>);  
     40<a name="l00043"></a>00043         <span class="keywordtype">void</span> <a class="code" href="classKalmanFull.html#13a041cd98ff157703766be275a657bb" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt);  
    4141<a name="l00044"></a>00044  
    4242<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 ); 
    … …  
    4848<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> {  
    4949<a name="l00055"></a>00055 <span class="keyword">protected</span>: 
    50 <a name="l00056"></a>00056         <span class="keywordtype">int</span> dimx, dimy, dimu; 
    51 <a name="l00057"></a>00057         mat A, B, C, D; 
    52 <a name="l00058"></a>00058         sq_T R, Q; 
    53 <a name="l00059"></a>00059          
    54 <a name="l00060"></a>00060         <span class="comment">//cache</span> 
    55 <a name="l00061"></a>00061         mat _K; 
    56 <a name="l00062"></a>00062         vec _yp; 
    57 <a name="l00063"></a>00063         sq_T _Ry,_iRy; 
    58 <a name="l00064"></a>00064 <span class="keyword">public</span>: 
    59 <a name="l00065"></a>00065         <span class="comment">//posterior </span> 
    60 <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>; 
    61 <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>; 
    62 <a name="l00070"></a>00070  
    63 <a name="l00071"></a>00071 <span class="keyword">public</span>: 
    64 <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); 
    65 <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 ); 
    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>);  
    67 <a name="l00078"></a>00078  
    68 <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 ); 
    69 <a name="l00080"></a>00080  
    70 <a name="l00081"></a>00081 }; 
    71 <a name="l00082"></a>00082  
    72 <a name="l00088"></a>00088 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    73 <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; { 
    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; 
    76 <a name="l00094"></a>00094 <span class="keyword">public</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>);     
    79 <a name="l00099"></a>00099 }; 
    80 <a name="l00100"></a>00100  
    81 <a name="l00102"></a>00102  
    82 <a name="l00103"></a>00103 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    83 <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){ 
    84 <a name="l00105"></a>00105         A = mat(dimx,dimx); 
    85 <a name="l00106"></a>00106         B = mat(dimx,dimu); 
    86 <a name="l00107"></a>00107         C = mat(dimy,dimx); 
    87 <a name="l00108"></a>00108         D = mat(dimy,dimu); 
    88 <a name="l00109"></a>00109  
    89 <a name="l00110"></a>00110         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = vec(dimx); 
    90 <a name="l00111"></a>00111         <span class="comment">//TODO Initialize the rest?</span> 
    91 <a name="l00112"></a>00112 }; 
    92 <a name="l00113"></a>00113  
    93 <a name="l00114"></a>00114 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    94 <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>() { 
    95 <a name="l00116"></a>00116         dimx = A0.rows(); 
    96 <a name="l00117"></a>00117         dimu = B0.cols(); 
    97 <a name="l00118"></a>00118         dimy = C0.rows(); 
    98 <a name="l00119"></a>00119  
    99 <a name="l00120"></a>00120         it_assert_debug( A0.cols()==dimx, <span class="stringliteral">"Kalman: A is not square"</span> ); 
    100 <a name="l00121"></a>00121         it_assert_debug( B0.rows()==dimx, <span class="stringliteral">"Kalman: B is not compatible"</span> ); 
    101 <a name="l00122"></a>00122         it_assert_debug( C0.cols()==dimx, <span class="stringliteral">"Kalman: C is not square"</span> ); 
    102 <a name="l00123"></a>00123         it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     <span class="stringliteral">"Kalman: D is not compatible"</span> ); 
    103 <a name="l00124"></a>00124         it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); 
    104 <a name="l00125"></a>00125         it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); 
    105 <a name="l00126"></a>00126  
    106 <a name="l00127"></a>00127         A = A0; 
    107 <a name="l00128"></a>00128         B = B0; 
    108 <a name="l00129"></a>00129         C = C0; 
    109 <a name="l00130"></a>00130         D = D0; 
    110 <a name="l00131"></a>00131         R = R0; 
    111 <a name="l00132"></a>00132         Q = Q0; 
    112 <a name="l00133"></a>00133         <a class="code" href="classKalman.html#3063a3f58a74cea672ae889971012eed" title="Mean value of the posterior density.">mu</a> = mu0; 
    113 <a name="l00134"></a>00134         <a class="code" href="classKalman.html#188cd5ac1c9e496b1a371eb7c57c97d3" title="Mean value of the posterior density.">P</a> = P0; 
     50<a name="l00056"></a>00056         <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvy; 
     51<a name="l00057"></a>00057         <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvu; 
     52<a name="l00058"></a>00058         <span class="keywordtype">int</span> dimx, dimy, dimu; 
     53<a name="l00059"></a>00059         mat A, B, C, D; 
     54<a name="l00060"></a>00060         sq_T R, Q; 
     55<a name="l00061"></a>00061          
     56<a name="l00063"></a><a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424">00063</a>         <a class="code" href="classenorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm&lt;sq_T&gt;</a> <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>; 
     57<a name="l00065"></a><a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f">00065</a>         <a class="code" href="classenorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm&lt;sq_T&gt;</a> <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>; 
     58<a name="l00066"></a>00066          
     59<a name="l00067"></a>00067         mat _K; 
     60<a name="l00068"></a>00068         <span class="comment">//cache of fy</span> 
     61<a name="l00069"></a>00069         vec* _yp;  
     62<a name="l00070"></a>00070         sq_T* _Ry,*_iRy; 
     63<a name="l00071"></a>00071         <span class="comment">//cache of est</span> 
     64<a name="l00072"></a>00072         vec* _mu; 
     65<a name="l00073"></a>00073         sq_T* _P, *_iP; 
     66<a name="l00074"></a>00074          
     67<a name="l00075"></a>00075 <span class="keyword">public</span>: 
     68<a name="l00077"></a>00077         <a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3" title="Default constructor.">Kalman</a> (<a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvx0, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvy0, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvu0); 
     69<a name="l00079"></a>00079         <a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3" title="Default constructor.">Kalman</a> (<span class="keyword">const</span> <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman&lt;sq_T&gt;</a> &amp;K0); 
     70<a name="l00081"></a>00081         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a" title="Set parameters with check of relevance.">set_parameters</a> (<span class="keyword">const</span> mat &amp;A0,<span class="keyword">const</span> mat &amp;B0,<span class="keyword">const</span> mat &amp;C0,<span class="keyword">const</span> mat &amp;D0,<span class="keyword">const</span> sq_T &amp;R0,<span class="keyword">const</span> sq_T &amp;Q0); 
     71<a name="l00083"></a><a class="code" href="classKalman.html#80bcf29466d9a9dd2b8f74699807d0c0">00083</a>         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#80bcf29466d9a9dd2b8f74699807d0c0" title="Set estimate values, used e.g. in initialization.">set_est</a>(<span class="keyword">const</span> vec &amp;mu0, <span class="keyword">const</span> sq_T &amp;P0 ){<a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>.set_parameters(mu0,P0);}; 
     72<a name="l00085"></a>00085         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt);  
     73<a name="l00086"></a><a class="code" href="classKalman.html#a213c57aef55b2645e550bed81cfc0d4">00086</a>         <a class="code" href="classepdf.html" title="Probability density function with numerical statistics, e.g. posterior density.">epdf</a>&amp; <a class="code" href="classKalman.html#a213c57aef55b2645e550bed81cfc0d4" title="Returns a pointer to the epdf representing posterior density on parameters. Use with...">_epdf</a>(){<span class="keywordflow">return</span> <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>;} 
     74<a name="l00087"></a>00087 <span class="comment">//      friend std::ostream &amp;operator&lt;&lt; ( std::ostream &amp;os, const Kalman&lt;sq_T&gt; &amp;kf );</span> 
     75<a name="l00088"></a>00088  
     76<a name="l00090"></a>00090 }; 
     77<a name="l00091"></a>00091  
     78<a name="l00097"></a>00097 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     79<a name="l00098"></a><a class="code" href="classEKF.html">00098</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;ldmat&gt; { 
     80<a name="l00100"></a>00100         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* pfxu; 
     81<a name="l00102"></a>00102         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* phxu; 
     82<a name="l00103"></a>00103 <span class="keyword">public</span>:  
     83<a name="l00105"></a>00105         <a class="code" href="classEKF.html#ea4f3254cacf0a92d2a820b1201d049e" title="Default constructor.">EKF</a> (<a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvx, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvy, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvu); 
     84<a name="l00106"></a>00106         <span class="keywordtype">void</span> set_parameters(<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, <span class="keyword">const</span> sq_T Q0, <span class="keyword">const</span> sq_T R0); 
     85<a name="l00108"></a>00108         <span class="keywordtype">void</span> <a class="code" href="classEKF.html#c79c62c9b3e0b56b3aaa1b6f1d9a7af7" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a>(<span class="keyword">const</span> vec &amp;dt);       
     86<a name="l00109"></a>00109 }; 
     87<a name="l00110"></a>00110  
     88<a name="l00114"></a><a class="code" href="classKFcondQR.html">00114</a> <span class="keyword">class </span><a class="code" href="classKFcondQR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondQR</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;ldmat&gt;, <span class="keyword">public</span> <a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> { 
     89<a name="l00115"></a>00115 <span class="comment">//protected:</span> 
     90<a name="l00116"></a>00116 <span class="keyword">public</span>: 
     91<a name="l00117"></a>00117 <a class="code" href="classKFcondQR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondQR</a>(<a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvx, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvy, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvu): <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman&lt;ldmat&gt;</a>(rvx, rvy,rvu){}; 
     92<a name="l00118"></a>00118 <span class="keywordtype">void</span> <a class="code" href="classKFcondQR.html#c9ecf292a85327aa6309c9fd70ceb606" title="Substitute val for rvc.">condition</a>(<span class="keyword">const</span> vec &amp;RQ); 
     93<a name="l00119"></a>00119 }; 
     94<a name="l00120"></a>00120  
     95<a name="l00122"></a>00122  
     96<a name="l00123"></a>00123 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     97<a name="l00124"></a><a class="code" href="classKalman.html#ce38e31810aea4db45a83ad05eaba009">00124</a> <a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3" title="Default constructor.">Kalman&lt;sq_T&gt;::Kalman</a>(<span class="keyword">const</span> <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman&lt;sq_T&gt;</a> &amp;K0): <a class="code" href="classBM.html" title="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</a>(K0.rv),rvy(K0.rvy),rvu(K0.rvu), 
     98<a name="l00125"></a>00125 dimx(rv.count()), dimy(rvy.count()),dimu(rvu.count()),  
     99<a name="l00126"></a>00126  A(dimx,dimx), B(dimx,dimu), C(dimy,dimx), D(dimy,dimu),est(rv), fy(rvy){ 
     100<a name="l00127"></a>00127 this-&gt;<a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a" title="Set parameters with check of relevance.">set_parameters</a>(K0.<a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9">A</a>, K0.<a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a">B</a>, K0.<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13">C</a>, K0.<a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1">D</a>, K0.<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec">R</a>, K0.<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9">Q</a>); 
     101<a name="l00128"></a>00128 <span class="comment">//reset copy values in pointers</span> 
     102<a name="l00129"></a>00129 *_mu = *K0.<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734">_mu</a>; 
     103<a name="l00130"></a>00130 *_P = *K0.<a class="code" href="classKalman.html#b3388218567128a797e69b109138271d">_P</a>; 
     104<a name="l00131"></a>00131 *_iP = *K0.<a class="code" href="classKalman.html#b8bb7f870d69993493ba67ce40e7c3e9">_iP</a>; 
     105<a name="l00132"></a>00132 *_yp = *K0.<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8">_yp</a>; 
     106<a name="l00133"></a>00133 *_iRy = *K0.<a class="code" href="classKalman.html#fbbdf31365f5a5674099599200ea193b">_iRy</a>; 
     107<a name="l00134"></a>00134 *_Ry = *K0.<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674">_Ry</a>; 
    114108<a name="l00135"></a>00135  
    115 <a name="l00136"></a>00136 <span class="comment">//Fixme should we assign cache??</span> 
    116 <a name="l00137"></a>00137         _iRy = eye(dimy); <span class="comment">// needed in inv(_iRy)</span> 
    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 }; 
    150 <a name="l00171"></a>00171  
    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                  
    162 <a name="l00183"></a>00183  
    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; 
     109<a name="l00136"></a>00136 } 
     110<a name="l00137"></a>00137  
     111<a name="l00138"></a>00138 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     112<a name="l00139"></a><a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3">00139</a> <a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3" title="Default constructor.">Kalman&lt;sq_T&gt;::Kalman</a>(<a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvx, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvy0, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvu0): <a class="code" href="classBM.html" title="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</a>(rvx),rvy(rvy0),rvu(rvu0), 
     113<a name="l00140"></a>00140 dimx(rvx.count()), dimy(rvy.count()),dimu(rvu.count()),  
     114<a name="l00141"></a>00141  A(dimx,dimx), B(dimx,dimu), C(dimy,dimx), D(dimy,dimu),<a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>(rvx), <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>(rvy){ 
     115<a name="l00142"></a>00142 <span class="comment">//assign cache</span> 
     116<a name="l00143"></a>00143 <span class="comment">//est</span> 
     117<a name="l00144"></a>00144 _mu = <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._mu(); 
     118<a name="l00145"></a>00145 <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._R(_P,_iP); 
     119<a name="l00146"></a>00146  
     120<a name="l00147"></a>00147 <span class="comment">//fy</span> 
     121<a name="l00148"></a>00148 _yp = <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._mu(); 
     122<a name="l00149"></a>00149 <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._R(_Ry,_iRy); 
     123<a name="l00150"></a>00150 }; 
     124<a name="l00151"></a>00151  
     125<a name="l00152"></a>00152 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     126<a name="l00153"></a><a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a">00153</a> <span class="keywordtype">void</span> <a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a" title="Set parameters with check of relevance.">Kalman&lt;sq_T&gt;::set_parameters</a>(<span class="keyword">const</span> mat &amp;A0,<span class="keyword">const</span>  mat &amp;B0, <span class="keyword">const</span> mat &amp;C0, <span class="keyword">const</span> mat &amp;D0, <span class="keyword">const</span> sq_T &amp;R0, <span class="keyword">const</span> sq_T &amp;Q0) { 
     127<a name="l00154"></a>00154         it_assert_debug( A0.cols()==dimx, <span class="stringliteral">"Kalman: A is not square"</span> ); 
     128<a name="l00155"></a>00155         it_assert_debug( B0.rows()==dimx, <span class="stringliteral">"Kalman: B is not compatible"</span> ); 
     129<a name="l00156"></a>00156         it_assert_debug( C0.cols()==dimx, <span class="stringliteral">"Kalman: C is not square"</span> ); 
     130<a name="l00157"></a>00157         it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     <span class="stringliteral">"Kalman: D is not compatible"</span> ); 
     131<a name="l00158"></a>00158         it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); 
     132<a name="l00159"></a>00159         it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); 
     133<a name="l00160"></a>00160  
     134<a name="l00161"></a>00161         A = A0; 
     135<a name="l00162"></a>00162         B = B0; 
     136<a name="l00163"></a>00163         C = C0; 
     137<a name="l00164"></a>00164         D = D0; 
     138<a name="l00165"></a>00165         R = R0; 
     139<a name="l00166"></a>00166         Q = Q0;  
     140<a name="l00167"></a>00167 } 
     141<a name="l00168"></a>00168  
     142<a name="l00169"></a>00169 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     143<a name="l00170"></a><a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c">00170</a> <span class="keywordtype">void</span> <a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c" title="Here dt = [yt;ut] of appropriate dimensions.">Kalman&lt;sq_T&gt;::bayes</a>( <span class="keyword">const</span> vec &amp;dt) { 
     144<a name="l00171"></a>00171         it_assert_debug( dt.length()==( dimy+dimu ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
     145<a name="l00172"></a>00172  
     146<a name="l00173"></a>00173         vec u = dt.get( dimy,dimy+dimu-1 ); 
     147<a name="l00174"></a>00174         vec y = dt.get( 0,dimy-1 ); 
     148<a name="l00175"></a>00175         <span class="comment">//Time update</span> 
     149<a name="l00176"></a>00176         *_mu = A*(*_mu) + B*u; 
     150<a name="l00177"></a>00177         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
     151<a name="l00178"></a>00178         _P-&gt;mult_sym( A ); 
     152<a name="l00179"></a>00179         (*_P)+=Q; 
     153<a name="l00180"></a>00180  
     154<a name="l00181"></a>00181         <span class="comment">//Data update</span> 
     155<a name="l00182"></a>00182         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
     156<a name="l00183"></a>00183         _P-&gt;mult_sym( C, *_Ry); 
     157<a name="l00184"></a>00184         (*_Ry)+=R; 
     158<a name="l00185"></a>00185  
     159<a name="l00186"></a>00186         mat Pfull = _P-&gt;to_mat(); 
     160<a name="l00187"></a>00187          
     161<a name="l00188"></a>00188         _Ry-&gt;inv( *_iRy ); <span class="comment">// result is in _iRy;</span> 
     162<a name="l00189"></a>00189         _K = Pfull*C.transpose()*(_iRy-&gt;to_mat()); 
     163<a name="l00190"></a>00190          
     164<a name="l00191"></a>00191         sq_T pom((<span class="keywordtype">int</span>)Pfull.rows()); 
     165<a name="l00192"></a>00192         _iRy-&gt;mult_sym_t(C*Pfull,pom); 
     166<a name="l00193"></a>00193         (*_P) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span> 
     167<a name="l00194"></a>00194         (*_yp) = C*(*_mu)+D*u; <span class="comment">//y prediction</span> 
     168<a name="l00195"></a>00195         (*_mu) += _K*( y-(*_yp) ); 
     169<a name="l00196"></a>00196          
     170<a name="l00197"></a>00197         <span class="keywordflow">if</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>) { 
     171<a name="l00198"></a>00198                 <a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a>+=<a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.evalpdflog(*_yp); 
     172<a name="l00199"></a>00199         } 
     173<a name="l00200"></a>00200 }; 
     174<a name="l00201"></a>00201  
     175<a name="l00202"></a>00202 <span class="comment">//TODO why not const pointer??</span> 
     176<a name="l00203"></a>00203  
     177<a name="l00204"></a>00204 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     178<a name="l00205"></a><a class="code" href="classEKF.html#ea4f3254cacf0a92d2a820b1201d049e">00205</a> <a class="code" href="classEKF.html#ea4f3254cacf0a92d2a820b1201d049e" title="Default constructor.">EKF&lt;sq_T&gt;::EKF</a>(<a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvx0, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvy0, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvu0): <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;ldmat&gt;(rvx0,rvy0,rvu0){} 
     179<a name="l00206"></a>00206  
     180<a name="l00207"></a>00207 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     181<a name="l00208"></a>00208 <span class="keywordtype">void</span> <a class="code" href="classEKF.html" title="Extended Kalman Filter.">EKF&lt;sq_T&gt;::set_parameters</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,<span class="keyword">const</span> sq_T Q0,<span class="keyword">const</span> sq_T R0) { 
     182<a name="l00209"></a>00209                 pfxu = pfxu0; 
     183<a name="l00210"></a>00210                 phxu = phxu0; 
     184<a name="l00211"></a>00211                  
     185<a name="l00212"></a>00212                 <span class="comment">//initialize matrices A C, later, these will be only updated!</span> 
     186<a name="l00213"></a>00213                 pfxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(*_mu,zeros(dimu),A,<span class="keyword">true</span>); 
     187<a name="l00214"></a>00214                 pfxu-&gt;<a class="code" href="classdiffbifn.html#1978bafd7909d15c139a08c495c24aa0" title="Evaluates  and writes result into A .">dfdu_cond</a>(*_mu,zeros(dimu),B,<span class="keyword">true</span>); 
     188<a name="l00215"></a>00215                 phxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(*_mu,zeros(dimu),C,<span class="keyword">true</span>); 
     189<a name="l00216"></a>00216                 phxu-&gt;<a class="code" href="classdiffbifn.html#1978bafd7909d15c139a08c495c24aa0" title="Evaluates  and writes result into A .">dfdu_cond</a>(*_mu,zeros(dimu),D,<span class="keyword">true</span>); 
     190<a name="l00217"></a>00217  
     191<a name="l00218"></a>00218                 R = R0; 
     192<a name="l00219"></a>00219                 Q = Q0; 
     193<a name="l00220"></a>00220  
     194<a name="l00221"></a>00221         <span class="keyword">using</span> std::cout; 
     195<a name="l00222"></a>00222         cout&lt;&lt;A&lt;&lt;std::endl; 
     196<a name="l00223"></a>00223         cout&lt;&lt;B&lt;&lt;std::endl; 
     197<a name="l00224"></a>00224         cout&lt;&lt;C&lt;&lt;std::endl; 
     198<a name="l00225"></a>00225         cout&lt;&lt;D&lt;&lt;std::endl; 
     199<a name="l00226"></a>00226  
     200<a name="l00227"></a>00227 } 
     201<a name="l00228"></a>00228  
     202<a name="l00229"></a>00229 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     203<a name="l00230"></a><a class="code" href="classEKF.html#c79c62c9b3e0b56b3aaa1b6f1d9a7af7">00230</a> <span class="keywordtype">void</span> <a class="code" href="classEKF.html#c79c62c9b3e0b56b3aaa1b6f1d9a7af7" title="Here dt = [yt;ut] of appropriate dimensions.">EKF&lt;sq_T&gt;::bayes</a>( <span class="keyword">const</span> vec &amp;dt) { 
     204<a name="l00231"></a>00231         it_assert_debug( dt.length()==( dimy+dimu ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
     205<a name="l00232"></a>00232  
     206<a name="l00233"></a>00233         vec u = dt.get( dimy,dimy+dimu-1 ); 
     207<a name="l00234"></a>00234         vec y = dt.get( 0,dimy-1 ); 
     208<a name="l00235"></a>00235         <span class="comment">//Time update</span> 
     209<a name="l00236"></a>00236         *_mu = pfxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates $f(x0,u0)$ (VS: Do we really need common eval? ).">eval</a>(*_mu, u); 
     210<a name="l00237"></a>00237         pfxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(*_mu,u,A,<span class="keyword">false</span>); <span class="comment">//update A by a derivative of fx</span> 
     211<a name="l00238"></a>00238          
     212<a name="l00239"></a>00239         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
     213<a name="l00240"></a>00240         _P-&gt;mult_sym( A ); 
     214<a name="l00241"></a>00241         (*_P)+=Q; 
     215<a name="l00242"></a>00242  
     216<a name="l00243"></a>00243         <span class="comment">//Data update</span> 
     217<a name="l00244"></a>00244         phxu-&gt;<a class="code" href="classdiffbifn.html#6d217a02d4fa13931258d4bebdd0feb4" title="Evaluates  and writes result into A .">dfdx_cond</a>(*_mu,u,C,<span class="keyword">false</span>); <span class="comment">//update C by a derivative hx</span> 
     218<a name="l00245"></a>00245         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
     219<a name="l00246"></a>00246         _P-&gt;mult_sym( C, *_Ry); 
     220<a name="l00247"></a>00247         (*_Ry)+=R; 
     221<a name="l00248"></a>00248  
     222<a name="l00249"></a>00249         mat Pfull = _P-&gt;to_mat(); 
     223<a name="l00250"></a>00250          
     224<a name="l00251"></a>00251         _Ry-&gt;inv( *_iRy ); <span class="comment">// result is in _iRy;</span> 
     225<a name="l00252"></a>00252         _K = Pfull*C.transpose()*(_iRy-&gt;to_mat()); 
     226<a name="l00253"></a>00253          
     227<a name="l00254"></a>00254         sq_T pom((<span class="keywordtype">int</span>)Pfull.rows()); 
     228<a name="l00255"></a>00255         _iRy-&gt;mult_sym_t(C*Pfull,pom); 
     229<a name="l00256"></a>00256         (*_P) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span> 
     230<a name="l00257"></a>00257         *_yp = phxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates $f(x0,u0)$ (VS: Do we really need common eval? ).">eval</a>(*_mu,u); <span class="comment">//y prediction</span> 
     231<a name="l00258"></a>00258         (*_mu) += _K*( y-*_yp ); 
     232<a name="l00259"></a>00259          
     233<a name="l00260"></a>00260         <span class="keywordflow">if</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>) {<a class="code" href="classBM.html#5623fef6572a08c2b53b8c87b82dc979" title="Logarithm of marginalized data likelihood.">ll</a>+=<a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>.<a class="code" href="classenorm.html#9517594915e897584eaebbb057ed8881" title="Compute log-probability of argument val.">evalpdflog</a>(y);} 
     234<a name="l00261"></a>00261 }; 
     235<a name="l00262"></a>00262  
     236<a name="l00263"></a>00263  
     237<a name="l00264"></a>00264 <span class="preprocessor">#endif // KF_H</span> 
     238<a name="l00265"></a>00265 <span class="preprocessor"></span> 
     239<a name="l00266"></a>00266  
     240</pre></div><hr size="1"><address style="text-align: right;"><small>Generated on Thu Feb 28 16:54:39 2008 for mixpp by&nbsp; 
    215241<a href="http://www.doxygen.org/index.html"> 
    216242<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.3 </small></address>