Show
Ignore:
Timestamp:
03/14/08 18:11:21 (17 years ago)
Author:
smidl
Message:

Matrix in Cholesky decomposition, Square-root Kalman and many bug fixes

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • doc/html/libKF_8h-source.html

    r33 r37  
    2121<a name="l00017"></a>00017 <span class="preprocessor">#include "../stat/libFN.h"</span> 
    2222<a name="l00018"></a>00018 <span class="preprocessor">#include "../stat/libEF.h"</span> 
    23 <a name="l00019"></a>00019  
     23<a name="l00019"></a>00019 <span class="preprocessor">#include "../math/chmat.h"</span> 
    2424<a name="l00020"></a>00020  
    2525<a name="l00021"></a>00021 <span class="keyword">using namespace </span>itpp; 
    … …  
    8686<a name="l00124"></a>00124 }; 
    8787<a name="l00125"></a>00125  
    88 <a name="l00131"></a>00131 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    89 <a name="l00132"></a>00132  
    90 <a name="l00133"></a><a class="code" href="classEKF.html">00133</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; { 
    91 <a name="l00135"></a>00135         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* pfxu; 
    92 <a name="l00137"></a>00137         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* phxu; 
    93 <a name="l00138"></a>00138 <span class="keyword">public</span>: 
    94 <a name="l00140"></a>00140         <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> <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a> ); 
    95 <a name="l00142"></a>00142         <span class="keywordtype">void</span> <a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</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, <span class="keyword">const</span> sq_T Q0, <span class="keyword">const</span> sq_T R0 ); 
    96 <a name="l00144"></a>00144         <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 ); 
    97 <a name="l00145"></a>00145 }; 
    98 <a name="l00146"></a>00146  
    99 <a name="l00151"></a><a class="code" href="classKFcondQR.html">00151</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> { 
    100 <a name="l00152"></a>00152 <span class="comment">//protected:</span> 
    101 <a name="l00153"></a>00153 <span class="keyword">public</span>: 
    102 <a name="l00155"></a><a class="code" href="classKFcondQR.html#3f3968f92c7bbe4b0902d5e14ecc1cb4">00155</a>         <a class="code" href="classKFcondQR.html#3f3968f92c7bbe4b0902d5e14ecc1cb4" title="Default constructor.">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> <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvRQ ) : <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;<a class="code" href="classldmat.html" title="Matrix stored in LD form, (typically known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvRQ ) {}; 
    103 <a name="l00156"></a>00156  
    104 <a name="l00157"></a>00157         <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 ); 
    105 <a name="l00158"></a>00158 }; 
    106 <a name="l00159"></a>00159  
    107 <a name="l00164"></a><a class="code" href="classKFcondR.html">00164</a> <span class="keyword">class </span><a class="code" href="classKFcondR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondR</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> { 
    108 <a name="l00165"></a>00165 <span class="comment">//protected:</span> 
    109 <a name="l00166"></a>00166 <span class="keyword">public</span>: 
    110 <a name="l00168"></a><a class="code" href="classKFcondR.html#d2acbb8e66c7ee592b1a9da5b429a69e">00168</a>         <a class="code" href="classKFcondR.html#d2acbb8e66c7ee592b1a9da5b429a69e" title="Default constructor.">KFcondR</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> <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvR ) : <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;<a class="code" href="classldmat.html" title="Matrix stored in LD form, (typically known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvR ) {}; 
    111 <a name="l00169"></a>00169  
    112 <a name="l00170"></a>00170         <span class="keywordtype">void</span> <a class="code" href="classKFcondR.html#8c0721e47879bb8840d829db7a174a7f" title="Substitute val for rvc.">condition</a> ( <span class="keyword">const</span> vec &amp;<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> ); 
    113 <a name="l00171"></a>00171 }; 
    114 <a name="l00172"></a>00172  
    115 <a name="l00174"></a>00174  
    116 <a name="l00175"></a>00175 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    117 <a name="l00176"></a><a class="code" href="classKalman.html#ce38e31810aea4db45a83ad05eaba009">00176</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 ), 
    118 <a name="l00177"></a>00177                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ), 
    119 <a name="l00178"></a>00178                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),est ( rv ), fy ( rvy ) { 
     88<a name="l00128"></a><a class="code" href="classKalmanCh.html">00128</a> <span class="keyword">class </span><a class="code" href="classKalmanCh.html" title="Kalman filter in square root form.">KalmanCh</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>{ 
     89<a name="l00129"></a>00129 <span class="keyword">protected</span>: 
     90<a name="l00131"></a><a class="code" href="classKalmanCh.html#e1b7369670041b75231242f7293c4f90">00131</a>         <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalmanCh.html#e1b7369670041b75231242f7293c4f90" title="Indetifier of output rv.">rvy</a>; 
     91<a name="l00133"></a><a class="code" href="classKalmanCh.html#1cebd72aaff9f146b9b2400cc69e1b1a">00133</a>         <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalmanCh.html#1cebd72aaff9f146b9b2400cc69e1b1a" title="Indetifier of exogeneous rv.">rvu</a>; 
     92<a name="l00135"></a><a class="code" href="classKalmanCh.html#5c34f0f5cc4e1dd1be4b4489d2fc83d0">00135</a>         <span class="keywordtype">int</span> <a class="code" href="classKalmanCh.html#5c34f0f5cc4e1dd1be4b4489d2fc83d0" title="cache of rv.count()">dimx</a>; 
     93<a name="l00137"></a><a class="code" href="classKalmanCh.html#c8302d61db0512d8a0d4f34715117445">00137</a>         <span class="keywordtype">int</span> <a class="code" href="classKalmanCh.html#c8302d61db0512d8a0d4f34715117445" title="cache of rvy.count()">dimy</a>; 
     94<a name="l00139"></a><a class="code" href="classKalmanCh.html#bac7284585a271e28f928acabd990146">00139</a>         <span class="keywordtype">int</span> <a class="code" href="classKalmanCh.html#bac7284585a271e28f928acabd990146" title="cache of rvu.count()">dimu</a>; 
     95<a name="l00141"></a><a class="code" href="classKalmanCh.html#2ca9afe2042fbda38af3da8e7d8d9d3c">00141</a>         mat <a class="code" href="classKalmanCh.html#2ca9afe2042fbda38af3da8e7d8d9d3c" title="Matrix A.">A</a>; 
     96<a name="l00143"></a><a class="code" href="classKalmanCh.html#5a5ee93d6d32f45b5d15e87764efd529">00143</a>         mat <a class="code" href="classKalmanCh.html#5a5ee93d6d32f45b5d15e87764efd529" title="Matrix B.">B</a>;  
     97<a name="l00145"></a><a class="code" href="classKalmanCh.html#3c1ce2fe15dae5c0882a99c34b19cc40">00145</a>         mat <a class="code" href="classKalmanCh.html#3c1ce2fe15dae5c0882a99c34b19cc40" title="Matrix C.">C</a>; 
     98<a name="l00147"></a><a class="code" href="classKalmanCh.html#50d3e4c9e411f8c96b87a1883b7e7154">00147</a>         mat <a class="code" href="classKalmanCh.html#50d3e4c9e411f8c96b87a1883b7e7154" title="Matrix D.">D</a>; 
     99<a name="l00149"></a><a class="code" href="classKalmanCh.html#eb21fe7ab03feb24005c331070d28040">00149</a>         <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> <a class="code" href="classKalmanCh.html#eb21fe7ab03feb24005c331070d28040" title="Matrix Q in square-root form.">Q</a>; 
     100<a name="l00151"></a><a class="code" href="classKalmanCh.html#6716e9f9208fc7191b38fd801810b30a">00151</a>         <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> <a class="code" href="classKalmanCh.html#6716e9f9208fc7191b38fd801810b30a" title="Matrix R in square-root form.">R</a>; 
     101<a name="l00152"></a>00152  
     102<a name="l00154"></a><a class="code" href="classKalmanCh.html#94ee9da75b0e0f632e4a354988ca3798">00154</a> mat <a class="code" href="classKalmanCh.html#94ee9da75b0e0f632e4a354988ca3798" title="pre array (triangular matrix)">preA</a>; 
     103<a name="l00156"></a><a class="code" href="classKalmanCh.html#0d31a26dc72b5846cfe5af3ccb63ac87">00156</a> mat <a class="code" href="classKalmanCh.html#0d31a26dc72b5846cfe5af3ccb63ac87" title="post array (triangular matrix)">postA</a>; 
     104<a name="l00157"></a>00157  
     105<a name="l00159"></a><a class="code" href="classKalmanCh.html#c0ffeeb2ca028ae2c1b1e166d4015804">00159</a>         <a class="code" href="classenorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm&lt;chmat&gt;</a> <a class="code" href="classKalmanCh.html#c0ffeeb2ca028ae2c1b1e166d4015804" title="predictive density on $x_t$">pred</a>; 
     106<a name="l00161"></a><a class="code" href="classKalmanCh.html#9086baae040b9c4e5cc42a445853862a">00161</a>         <a class="code" href="classenorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm&lt;chmat&gt;</a> <a class="code" href="classKalmanCh.html#9086baae040b9c4e5cc42a445853862a" title="predictive density on $y_t$">fy</a>; 
     107<a name="l00162"></a>00162  
     108<a name="l00163"></a>00163 vec* _mu; 
     109<a name="l00164"></a>00164 <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a>* _P; 
     110<a name="l00165"></a>00165 vec* _yp; 
     111<a name="l00166"></a>00166 <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a>* _Ry; 
     112<a name="l00167"></a>00167  
     113<a name="l00168"></a>00168 <span class="keyword">public</span>: 
     114<a name="l00170"></a>00170         <a class="code" href="classKalmanCh.html#d11f110cccaa66177514632d37b086bb" title="Default constructor.">KalmanCh</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 ); 
     115<a name="l00172"></a>00172         <a class="code" href="classKalmanCh.html#d11f110cccaa66177514632d37b086bb" title="Default constructor.">KalmanCh</a> ( <span class="keyword">const</span> <a class="code" href="classKalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a> &amp;K0 ); 
     116<a name="l00174"></a>00174         <span class="keywordtype">void</span> <a class="code" href="classKalmanCh.html#92fb227287af05c9f0078d523c7c9793" 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> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &amp;R0,<span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &amp;Q0 ); 
     117<a name="l00176"></a><a class="code" href="classKalmanCh.html#7fe9b65d626971542e35213d11b4b98d">00176</a>         <span class="keywordtype">void</span> <a class="code" href="classKalmanCh.html#7fe9b65d626971542e35213d11b4b98d" title="Set estimate values, used e.g. in initialization.">set_pred</a> ( <span class="keyword">const</span> vec &amp;mu0, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &amp;P0 ) { 
     118<a name="l00177"></a>00177                 <a class="code" href="classKalmanCh.html#c0ffeeb2ca028ae2c1b1e166d4015804" title="predictive density on $x_t$">pred</a>.<a class="code" href="classenorm.html#1394a65caa6e00d42e00cc99b12227af" title="Set mean value mu and covariance R.">set_parameters</a> ( mu0,P0 ); 
     119<a name="l00178"></a>00178         }; 
    120120<a name="l00179"></a>00179  
    121 <a name="l00180"></a>00180         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" title="Matrix A.">A</a>, K0.<a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>, K0.<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, K0.<a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>, K0.<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>, K0.<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> ); 
    122 <a name="l00181"></a>00181  
    123 <a name="l00182"></a>00182 <span class="comment">//establish pointers</span> 
    124 <a name="l00183"></a>00183         <a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._mu(); 
    125 <a name="l00184"></a>00184         <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._R ( <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>,<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a> ); 
    126 <a name="l00185"></a>00185  
    127 <a name="l00186"></a>00186 <span class="comment">//fy</span> 
    128 <a name="l00187"></a>00187         <a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._mu(); 
    129 <a name="l00188"></a>00188         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._R ( <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>,<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); 
    130 <a name="l00189"></a>00189  
    131 <a name="l00190"></a>00190 <span class="comment">//reset copy values in pointers</span> 
    132 <a name="l00191"></a>00191         *<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = *K0.<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>; 
    133 <a name="l00192"></a>00192         *<a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a> = *K0.<a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>; 
    134 <a name="l00193"></a>00193         *<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a> = *K0.<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a>; 
    135 <a name="l00194"></a>00194         *<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = *K0.<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a>; 
    136 <a name="l00195"></a>00195         *<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> = *K0.<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>; 
    137 <a name="l00196"></a>00196         *<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a> = *K0.<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>; 
    138 <a name="l00197"></a>00197  
    139 <a name="l00198"></a>00198 } 
    140 <a name="l00199"></a>00199  
    141 <a name="l00200"></a>00200 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    142 <a name="l00201"></a><a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3">00201</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 ),<a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a> ( rvy0 ),<a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a> ( rvu0 ), 
    143 <a name="l00202"></a>00202                 <a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ( rvx.count() ), <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ( <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>.count() ),<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ( <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a>.count() ), 
    144 <a name="l00203"></a>00203                 <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> ( <a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>,<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ), <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a> ( <a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>,<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ), <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a> ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ), <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a> ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ), 
    145 <a name="l00204"></a>00204                 <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>(<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>), <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> (<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>), 
    146 <a name="l00205"></a>00205                 <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> ( <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a> ) { 
    147 <a name="l00206"></a>00206 <span class="comment">//assign cache</span> 
    148 <a name="l00207"></a>00207 <span class="comment">//est</span> 
    149 <a name="l00208"></a>00208         <a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._mu(); 
    150 <a name="l00209"></a>00209         <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._R ( <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>,<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a> ); 
    151 <a name="l00210"></a>00210  
    152 <a name="l00211"></a>00211 <span class="comment">//fy</span> 
    153 <a name="l00212"></a>00212         <a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._mu(); 
    154 <a name="l00213"></a>00213         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._R ( <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>,<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); 
    155 <a name="l00214"></a>00214 }; 
    156 <a name="l00215"></a>00215  
    157 <a name="l00216"></a>00216 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    158 <a name="l00217"></a><a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a">00217</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 ) { 
    159 <a name="l00218"></a>00218         it_assert_debug ( A0.cols() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: A is not square"</span> ); 
    160 <a name="l00219"></a>00219         it_assert_debug ( B0.rows() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: B is not compatible"</span> ); 
    161 <a name="l00220"></a>00220         it_assert_debug ( C0.cols() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: C is not square"</span> ); 
    162 <a name="l00221"></a>00221         it_assert_debug ( ( D0.rows() ==<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ) || ( D0.cols() ==<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ), <span class="stringliteral">"Kalman: D is not compatible"</span> ); 
    163 <a name="l00222"></a>00222         it_assert_debug ( ( R0.cols() ==<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ) || ( R0.rows() ==<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); 
    164 <a name="l00223"></a>00223         it_assert_debug ( ( Q0.cols() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ) || ( Q0.rows() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); 
    165 <a name="l00224"></a>00224  
    166 <a name="l00225"></a>00225         <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> = A0; 
    167 <a name="l00226"></a>00226         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a> = B0; 
    168 <a name="l00227"></a>00227         <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a> = C0; 
    169 <a name="l00228"></a>00228         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a> = D0; 
    170 <a name="l00229"></a>00229         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0; 
    171 <a name="l00230"></a>00230         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0; 
    172 <a name="l00231"></a>00231 } 
    173 <a name="l00232"></a>00232  
    174 <a name="l00233"></a>00233 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    175 <a name="l00234"></a><a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c">00234</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 ) { 
    176 <a name="l00235"></a>00235         it_assert_debug ( dt.length() == ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
    177 <a name="l00236"></a>00236  
    178 <a name="l00237"></a>00237         vec u = dt.get ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a>-1 ); 
    179 <a name="l00238"></a>00238         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 ); 
    180 <a name="l00239"></a>00239         <span class="comment">//Time update</span> 
    181 <a name="l00240"></a>00240         *<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>* ( *_mu ) + <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>*u; 
    182 <a name="l00241"></a>00241         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
    183 <a name="l00242"></a>00242         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;mult_sym ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> ); 
    184 <a name="l00243"></a>00243         ( *_P ) +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>; 
     121<a name="l00190"></a>00190         <span class="keywordtype">void</span> <a class="code" href="classKalmanCh.html#cca758192846940409822b9bd778d4e1" title="Here dt = [yt;ut] of appropriate dimensions The following equality hold::.">bayes</a> ( <span class="keyword">const</span> vec &amp;dt ); 
     122<a name="l00192"></a><a class="code" href="classKalmanCh.html#221892e744e7020ac5735978803d357d">00192</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="classKalmanCh.html#221892e744e7020ac5735978803d357d" title="access function">_epdf</a>() {it_warning(<span class="stringliteral">"this is predictor, not estimator"</span>);<span class="keywordflow">return</span> <a class="code" href="classKalmanCh.html#c0ffeeb2ca028ae2c1b1e166d4015804" title="predictive density on $x_t$">pred</a>;} 
     123<a name="l00194"></a><a class="code" href="classKalmanCh.html#1d62f16738a29f25c468acd3b1289ec7">00194</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="classKalmanCh.html#1d62f16738a29f25c468acd3b1289ec7" title="access function">_pred</a>() {<span class="keywordflow">return</span> <a class="code" href="classKalmanCh.html#c0ffeeb2ca028ae2c1b1e166d4015804" title="predictive density on $x_t$">pred</a>;} 
     124<a name="l00195"></a>00195 }; 
     125<a name="l00196"></a>00196  
     126<a name="l00202"></a>00202 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     127<a name="l00203"></a><a class="code" href="classEKF.html">00203</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; { 
     128<a name="l00205"></a>00205         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* pfxu; 
     129<a name="l00207"></a>00207         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables $f(x,u)$.">diffbifn</a>* phxu; 
     130<a name="l00208"></a>00208 <span class="keyword">public</span>: 
     131<a name="l00210"></a>00210         <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> <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a> ); 
     132<a name="l00212"></a>00212         <span class="keywordtype">void</span> <a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</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, <span class="keyword">const</span> sq_T Q0, <span class="keyword">const</span> sq_T R0 ); 
     133<a name="l00214"></a>00214         <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 ); 
     134<a name="l00215"></a>00215 }; 
     135<a name="l00216"></a>00216  
     136<a name="l00221"></a><a class="code" href="classKFcondQR.html">00221</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> { 
     137<a name="l00222"></a>00222 <span class="comment">//protected:</span> 
     138<a name="l00223"></a>00223 <span class="keyword">public</span>: 
     139<a name="l00225"></a><a class="code" href="classKFcondQR.html#3f3968f92c7bbe4b0902d5e14ecc1cb4">00225</a>         <a class="code" href="classKFcondQR.html#3f3968f92c7bbe4b0902d5e14ecc1cb4" title="Default constructor.">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> <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvRQ ) : <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;<a class="code" href="classldmat.html" title="Matrix stored in LD form, (typically known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvRQ ) {}; 
     140<a name="l00226"></a>00226  
     141<a name="l00227"></a>00227         <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 ); 
     142<a name="l00228"></a>00228 }; 
     143<a name="l00229"></a>00229  
     144<a name="l00234"></a><a class="code" href="classKFcondR.html">00234</a> <span class="keyword">class </span><a class="code" href="classKFcondR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondR</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> { 
     145<a name="l00235"></a>00235 <span class="comment">//protected:</span> 
     146<a name="l00236"></a>00236 <span class="keyword">public</span>: 
     147<a name="l00238"></a><a class="code" href="classKFcondR.html#d2acbb8e66c7ee592b1a9da5b429a69e">00238</a>         <a class="code" href="classKFcondR.html#d2acbb8e66c7ee592b1a9da5b429a69e" title="Default constructor.">KFcondR</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> <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a>, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvR ) : <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a>&lt;<a class="code" href="classldmat.html" title="Matrix stored in LD form, (typically known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvR ) {}; 
     148<a name="l00239"></a>00239  
     149<a name="l00240"></a>00240         <span class="keywordtype">void</span> <a class="code" href="classKFcondR.html#8c0721e47879bb8840d829db7a174a7f" title="Substitute val for rvc.">condition</a> ( <span class="keyword">const</span> vec &amp;<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> ); 
     150<a name="l00241"></a>00241 }; 
     151<a name="l00242"></a>00242  
    185152<a name="l00244"></a>00244  
    186 <a name="l00245"></a>00245         <span class="comment">//Data update</span> 
    187 <a name="l00246"></a>00246         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
    188 <a name="l00247"></a>00247         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;mult_sym ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, *<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a> ); 
    189 <a name="l00248"></a>00248         ( *_Ry ) +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>; 
     153<a name="l00245"></a>00245 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     154<a name="l00246"></a><a class="code" href="classKalman.html#ce38e31810aea4db45a83ad05eaba009">00246</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 ), 
     155<a name="l00247"></a>00247                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ), 
     156<a name="l00248"></a>00248                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),est ( rv ), fy ( rvy ) { 
    190157<a name="l00249"></a>00249  
    191 <a name="l00250"></a>00250         mat Pfull = <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;to_mat(); 
     158<a name="l00250"></a>00250         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" title="Matrix A.">A</a>, K0.<a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>, K0.<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, K0.<a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>, K0.<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>, K0.<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> ); 
    192159<a name="l00251"></a>00251  
    193 <a name="l00252"></a>00252         <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>-&gt;inv ( *<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); <span class="comment">// result is in _iRy;</span> 
    194 <a name="l00253"></a>00253         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._cached ( <span class="keyword">true</span> ); 
    195 <a name="l00254"></a>00254         <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a> = Pfull*<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>.transpose() * ( <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;to_mat() ); 
     160<a name="l00252"></a>00252 <span class="comment">//establish pointers</span> 
     161<a name="l00253"></a>00253         <a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._mu(); 
     162<a name="l00254"></a>00254         <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._R ( <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>,<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a> ); 
    196163<a name="l00255"></a>00255  
    197 <a name="l00256"></a>00256         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() ); 
    198 <a name="l00257"></a>00257         <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;mult_sym_t ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom ); 
    199 <a name="l00258"></a>00258         ( *_P ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span> 
    200 <a name="l00259"></a>00259         ( *_yp ) = <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>* ( *_mu ) +<a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>*u; <span class="comment">//y prediction</span> 
    201 <a name="l00260"></a>00260         ( *_mu ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y- ( *_yp ) ); 
    202 <a name="l00261"></a>00261  
    203 <a name="l00262"></a>00262  
    204 <a name="l00263"></a>00263         <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> ) { <span class="comment">//likelihood of observation y</span> 
    205 <a name="l00264"></a>00264                 <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 ( y ); 
    206 <a name="l00265"></a>00265         } 
    207 <a name="l00266"></a>00266  
    208 <a name="l00267"></a>00267 <span class="comment">//cout &lt;&lt; "y: " &lt;&lt; y-(*_yp) &lt;&lt;" R: " &lt;&lt; _Ry-&gt;to_mat() &lt;&lt; " iR: " &lt;&lt; _iRy-&gt;to_mat() &lt;&lt; " ll: " &lt;&lt; ll &lt;&lt;endl;</span> 
    209 <a name="l00268"></a>00268  
    210 <a name="l00269"></a>00269 }; 
    211 <a name="l00270"></a>00270  
    212 <a name="l00271"></a>00271 <span class="comment">//TODO why not const pointer??</span> 
    213 <a name="l00272"></a>00272  
    214 <a name="l00273"></a>00273 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    215 <a name="l00274"></a><a class="code" href="classEKF.html#ea4f3254cacf0a92d2a820b1201d049e">00274</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;<a class="code" href="classldmat.html" title="Matrix stored in LD form, (typically known as UD).">ldmat</a>&gt; ( rvx0,rvy0,rvu0 ) {} 
    216 <a name="l00275"></a>00275  
    217 <a name="l00276"></a>00276 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    218 <a name="l00277"></a><a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66">00277</a> <span class="keywordtype">void</span> <a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66" title="Set nonlinear functions for mean values and covariance matrices.">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 ) { 
    219 <a name="l00278"></a>00278         pfxu = pfxu0; 
    220 <a name="l00279"></a>00279         phxu = phxu0; 
     164<a name="l00256"></a>00256 <span class="comment">//fy</span> 
     165<a name="l00257"></a>00257         <a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._mu(); 
     166<a name="l00258"></a>00258         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._R ( <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>,<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); 
     167<a name="l00259"></a>00259  
     168<a name="l00260"></a>00260 <span class="comment">// copy values in pointers</span> 
     169<a name="l00261"></a>00261         *<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = *K0.<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>; 
     170<a name="l00262"></a>00262         *<a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a> = *K0.<a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>; 
     171<a name="l00263"></a>00263         *<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a> = *K0.<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a>; 
     172<a name="l00264"></a>00264         *<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = *K0.<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a>; 
     173<a name="l00265"></a>00265         *<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> = *K0.<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>; 
     174<a name="l00266"></a>00266         *<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a> = *K0.<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>; 
     175<a name="l00267"></a>00267  
     176<a name="l00268"></a>00268 } 
     177<a name="l00269"></a>00269  
     178<a name="l00270"></a>00270 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     179<a name="l00271"></a><a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3">00271</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 ),<a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a> ( rvy0 ),<a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a> ( rvu0 ), 
     180<a name="l00272"></a>00272                 <a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ( rvx.count() ), <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ( <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a>.count() ),<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ( <a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498" title="Indetifier of exogeneous rv.">rvu</a>.count() ), 
     181<a name="l00273"></a>00273                 <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> ( <a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>,<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ), <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a> ( <a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>,<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ), <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a> ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ), <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a> ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ), 
     182<a name="l00274"></a>00274                 <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>(<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>), <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> (<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>), 
     183<a name="l00275"></a>00275                 <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> ( <a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c" title="Indetifier of output rv.">rvy</a> ) { 
     184<a name="l00276"></a>00276 <span class="comment">//assign cache</span> 
     185<a name="l00277"></a>00277 <span class="comment">//est</span> 
     186<a name="l00278"></a>00278         <a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._mu(); 
     187<a name="l00279"></a>00279         <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._R ( <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>,<a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a> ); 
    221188<a name="l00280"></a>00280  
    222 <a name="l00281"></a>00281         <span class="comment">//initialize matrices A C, later, these will be only updated!</span> 
    223 <a name="l00282"></a>00282         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,zeros ( <a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>,<span class="keyword">true</span> ); 
    224 <a name="l00283"></a>00283 <span class="comment">//      pfxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),B,true );</span> 
    225 <a name="l00284"></a>00284         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>.clear(); 
    226 <a name="l00285"></a>00285         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,zeros ( <a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>,<span class="keyword">true</span> ); 
    227 <a name="l00286"></a>00286 <span class="comment">//      phxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),D,true );</span> 
    228 <a name="l00287"></a>00287         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>.clear(); 
    229 <a name="l00288"></a>00288  
    230 <a name="l00289"></a>00289         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0; 
    231 <a name="l00290"></a>00290         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0; 
    232 <a name="l00291"></a>00291 } 
    233 <a name="l00292"></a>00292  
    234 <a name="l00293"></a>00293 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
    235 <a name="l00294"></a><a class="code" href="classEKF.html#c79c62c9b3e0b56b3aaa1b6f1d9a7af7">00294</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 ) { 
    236 <a name="l00295"></a>00295         it_assert_debug ( dt.length() == ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
    237 <a name="l00296"></a>00296  
    238 <a name="l00297"></a>00297         vec u = dt.get ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a>-1 ); 
    239 <a name="l00298"></a>00298         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 ); 
    240 <a name="l00299"></a>00299         <span class="comment">//Time update</span> 
    241 <a name="l00300"></a>00300         *<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>, u ); 
    242 <a name="l00301"></a>00301         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,u,<a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>,<span class="keyword">false</span> ); <span class="comment">//update A by a derivative of fx</span> 
     189<a name="l00281"></a>00281 <span class="comment">//fy</span> 
     190<a name="l00282"></a>00282         <a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._mu(); 
     191<a name="l00283"></a>00283         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._R ( <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>,<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); 
     192<a name="l00284"></a>00284 }; 
     193<a name="l00285"></a>00285  
     194<a name="l00286"></a>00286 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     195<a name="l00287"></a><a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a">00287</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 ) { 
     196<a name="l00288"></a>00288         it_assert_debug ( A0.cols() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: A is not square"</span> ); 
     197<a name="l00289"></a>00289         it_assert_debug ( B0.rows() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: B is not compatible"</span> ); 
     198<a name="l00290"></a>00290         it_assert_debug ( C0.cols() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: C is not square"</span> ); 
     199<a name="l00291"></a>00291         it_assert_debug ( ( D0.rows() ==<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ) || ( D0.cols() ==<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ), <span class="stringliteral">"Kalman: D is not compatible"</span> ); 
     200<a name="l00292"></a>00292         it_assert_debug ( ( R0.cols() ==<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ) || ( R0.rows() ==<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a> ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); 
     201<a name="l00293"></a>00293         it_assert_debug ( ( Q0.cols() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ) || ( Q0.rows() ==<a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a> ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); 
     202<a name="l00294"></a>00294  
     203<a name="l00295"></a>00295         <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> = A0; 
     204<a name="l00296"></a>00296         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a> = B0; 
     205<a name="l00297"></a>00297         <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a> = C0; 
     206<a name="l00298"></a>00298         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a> = D0; 
     207<a name="l00299"></a>00299         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0; 
     208<a name="l00300"></a>00300         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0; 
     209<a name="l00301"></a>00301 } 
    243210<a name="l00302"></a>00302  
    244 <a name="l00303"></a>00303         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
    245 <a name="l00304"></a>00304         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;<a class="code" href="classldmat.html#e967b9425007f0cb6cd59b845f9756d8" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a> ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> ); 
    246 <a name="l00305"></a>00305         ( *_P ) +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>; 
     211<a name="l00303"></a>00303 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     212<a name="l00304"></a><a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c">00304</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 ) { 
     213<a name="l00305"></a>00305         it_assert_debug ( dt.length() == ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
    247214<a name="l00306"></a>00306  
    248 <a name="l00307"></a>00307         <span class="comment">//Data update</span> 
    249 <a name="l00308"></a>00308         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,u,<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>,<span class="keyword">false</span> ); <span class="comment">//update C by a derivative hx</span> 
    250 <a name="l00309"></a>00309         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
    251 <a name="l00310"></a>00310         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;<a class="code" href="classldmat.html#e967b9425007f0cb6cd59b845f9756d8" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a> ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, *<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a> ); 
    252 <a name="l00311"></a>00311         ( *_Ry ) +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>; 
    253 <a name="l00312"></a>00312  
    254 <a name="l00313"></a>00313         mat Pfull = <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;<a class="code" href="classldmat.html#5b0515da8dc2293d9e4360b74cc26c9e" title="Conversion to full matrix.">to_mat</a>(); 
     215<a name="l00307"></a>00307         vec u = dt.get ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a>-1 ); 
     216<a name="l00308"></a>00308         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 ); 
     217<a name="l00309"></a>00309         <span class="comment">//Time update</span> 
     218<a name="l00310"></a>00310         *<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>* ( *_mu ) + <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>*u; 
     219<a name="l00311"></a>00311         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
     220<a name="l00312"></a>00312         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;mult_sym ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> ); 
     221<a name="l00313"></a>00313         ( *_P ) +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>; 
    255222<a name="l00314"></a>00314  
    256 <a name="l00315"></a>00315         <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>-&gt;<a class="code" href="classldmat.html#2c160cb123c1102face7a50ec566a031" title="Matrix inversion preserving the chosen form.">inv</a> ( *<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); <span class="comment">// result is in _iRy;</span> 
    257 <a name="l00316"></a>00316         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.<a class="code" href="classenorm.html#c9ca4f2ca42568e40ca146168e7f3247" title="set cache as inconsistent">_cached</a> ( <span class="keyword">true</span> ); 
    258 <a name="l00317"></a>00317         <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a> = Pfull*<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>.transpose() * ( <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;<a class="code" href="classldmat.html#5b0515da8dc2293d9e4360b74cc26c9e" title="Conversion to full matrix.">to_mat</a>() ); 
    259 <a name="l00318"></a>00318  
    260 <a name="l00319"></a>00319         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() ); 
    261 <a name="l00320"></a>00320         <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;<a class="code" href="classldmat.html#4fd155f38eb6dd5af4bdf9c98a7999a9" title="Inplace symmetric multiplication by a SQUARE transpose of matrix $C$, i.e. $V = C&amp;#39;*V*C$...">mult_sym_t</a> ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom ); 
    262 <a name="l00321"></a>00321         ( *_P ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span> 
    263 <a name="l00322"></a>00322         *<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = 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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,u ); <span class="comment">//y prediction</span> 
    264 <a name="l00323"></a>00323         ( *_mu ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y-*<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> ); 
    265 <a name="l00324"></a>00324  
    266 <a name="l00325"></a>00325         <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#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.<a class="code" href="classenorm.html#609a7c33dbb4fdfab050f3bdd1122401" title="Compute log-probability of argument val.">evalpdflog</a> ( y );} 
    267 <a name="l00326"></a>00326 }; 
    268 <a name="l00327"></a>00327  
    269 <a name="l00328"></a>00328  
    270 <a name="l00329"></a>00329 <span class="preprocessor">#endif // KF_H</span> 
    271 <a name="l00330"></a>00330 <span class="preprocessor"></span> 
     223<a name="l00315"></a>00315         <span class="comment">//Data update</span> 
     224<a name="l00316"></a>00316         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
     225<a name="l00317"></a>00317         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;mult_sym ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, *<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a> ); 
     226<a name="l00318"></a>00318         ( *_Ry ) +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>; 
     227<a name="l00319"></a>00319  
     228<a name="l00320"></a>00320         mat Pfull = <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;to_mat(); 
     229<a name="l00321"></a>00321  
     230<a name="l00322"></a>00322         <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>-&gt;inv ( *<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); <span class="comment">// result is in _iRy;</span> 
     231<a name="l00323"></a>00323         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._cached ( <span class="keyword">true</span> ); 
     232<a name="l00324"></a>00324         <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a> = Pfull*<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>.transpose() * ( <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;to_mat() ); 
     233<a name="l00325"></a>00325  
     234<a name="l00326"></a>00326         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() ); 
     235<a name="l00327"></a>00327         <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;mult_sym_t ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom ); 
     236<a name="l00328"></a>00328         ( *_P ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span> 
     237<a name="l00329"></a>00329         ( *_yp ) = <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>* ( *_mu ) +<a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>*u; <span class="comment">//y prediction</span> 
     238<a name="l00330"></a>00330         ( *_mu ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y- ( *_yp ) ); 
    272239<a name="l00331"></a>00331  
    273 </pre></div><hr size="1"><address style="text-align: right;"><small>Generated on Wed Mar 5 15:40:00 2008 for mixpp by&nbsp; 
     240<a name="l00332"></a>00332  
     241<a name="l00333"></a>00333         <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> ) { <span class="comment">//likelihood of observation y</span> 
     242<a name="l00334"></a>00334                 <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 ( y ); 
     243<a name="l00335"></a>00335         } 
     244<a name="l00336"></a>00336  
     245<a name="l00337"></a>00337 <span class="comment">//cout &lt;&lt; "y: " &lt;&lt; y-(*_yp) &lt;&lt;" R: " &lt;&lt; _Ry-&gt;to_mat() &lt;&lt; " iR: " &lt;&lt; _iRy-&gt;to_mat() &lt;&lt; " ll: " &lt;&lt; ll &lt;&lt;endl;</span> 
     246<a name="l00338"></a>00338  
     247<a name="l00339"></a>00339 }; 
     248<a name="l00340"></a>00340  
     249<a name="l00341"></a>00341 <span class="comment">//TODO why not const pointer??</span> 
     250<a name="l00342"></a>00342  
     251<a name="l00343"></a>00343 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     252<a name="l00344"></a><a class="code" href="classEKF.html#ea4f3254cacf0a92d2a820b1201d049e">00344</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;<a class="code" href="classldmat.html" title="Matrix stored in LD form, (typically known as UD).">ldmat</a>&gt; ( rvx0,rvy0,rvu0 ) {} 
     253<a name="l00345"></a>00345  
     254<a name="l00346"></a>00346 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     255<a name="l00347"></a><a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66">00347</a> <span class="keywordtype">void</span> <a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66" title="Set nonlinear functions for mean values and covariance matrices.">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 ) { 
     256<a name="l00348"></a>00348         pfxu = pfxu0; 
     257<a name="l00349"></a>00349         phxu = phxu0; 
     258<a name="l00350"></a>00350  
     259<a name="l00351"></a>00351         <span class="comment">//initialize matrices A C, later, these will be only updated!</span> 
     260<a name="l00352"></a>00352         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,zeros ( <a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>,<span class="keyword">true</span> ); 
     261<a name="l00353"></a>00353 <span class="comment">//      pfxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),B,true );</span> 
     262<a name="l00354"></a>00354         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>.clear(); 
     263<a name="l00355"></a>00355         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,zeros ( <a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>,<span class="keyword">true</span> ); 
     264<a name="l00356"></a>00356 <span class="comment">//      phxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),D,true );</span> 
     265<a name="l00357"></a>00357         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>.clear(); 
     266<a name="l00358"></a>00358  
     267<a name="l00359"></a>00359         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0; 
     268<a name="l00360"></a>00360         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0; 
     269<a name="l00361"></a>00361 } 
     270<a name="l00362"></a>00362  
     271<a name="l00363"></a>00363 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt; 
     272<a name="l00364"></a><a class="code" href="classEKF.html#c79c62c9b3e0b56b3aaa1b6f1d9a7af7">00364</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 ) { 
     273<a name="l00365"></a>00365         it_assert_debug ( dt.length() == ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a> ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); 
     274<a name="l00366"></a>00366  
     275<a name="l00367"></a>00367         vec u = dt.get ( <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>+<a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a>-1 ); 
     276<a name="l00368"></a>00368         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 ); 
     277<a name="l00369"></a>00369         <span class="comment">//Time update</span> 
     278<a name="l00370"></a>00370         *<a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>, u ); 
     279<a name="l00371"></a>00371         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,u,<a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>,<span class="keyword">false</span> ); <span class="comment">//update A by a derivative of fx</span> 
     280<a name="l00372"></a>00372  
     281<a name="l00373"></a>00373         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span> 
     282<a name="l00374"></a>00374         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;<a class="code" href="classldmat.html#e967b9425007f0cb6cd59b845f9756d8" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a> ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> ); 
     283<a name="l00375"></a>00375         ( *_P ) +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>; 
     284<a name="l00376"></a>00376  
     285<a name="l00377"></a>00377         <span class="comment">//Data update</span> 
     286<a name="l00378"></a>00378         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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,u,<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>,<span class="keyword">false</span> ); <span class="comment">//update C by a derivative hx</span> 
     287<a name="l00379"></a>00379         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> 
     288<a name="l00380"></a>00380         <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;<a class="code" href="classldmat.html#e967b9425007f0cb6cd59b845f9756d8" title="Inplace symmetric multiplication by a SQUARE matrix $C$, i.e. $V = C*V*C&amp;#39;$.">mult_sym</a> ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, *<a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a> ); 
     289<a name="l00381"></a>00381         ( *_Ry ) +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>; 
     290<a name="l00382"></a>00382  
     291<a name="l00383"></a>00383         mat Pfull = <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>-&gt;<a class="code" href="classldmat.html#5b0515da8dc2293d9e4360b74cc26c9e" title="Conversion to full matrix.">to_mat</a>(); 
     292<a name="l00384"></a>00384  
     293<a name="l00385"></a>00385         <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>-&gt;<a class="code" href="classldmat.html#2c160cb123c1102face7a50ec566a031" title="Matrix inversion preserving the chosen form.">inv</a> ( *<a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a> ); <span class="comment">// result is in _iRy;</span> 
     294<a name="l00386"></a>00386         <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.<a class="code" href="classenorm.html#c9ca4f2ca42568e40ca146168e7f3247" title="set cache as inconsistent">_cached</a> ( <span class="keyword">true</span> ); 
     295<a name="l00387"></a>00387         <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a> = Pfull*<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>.transpose() * ( <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;<a class="code" href="classldmat.html#5b0515da8dc2293d9e4360b74cc26c9e" title="Conversion to full matrix.">to_mat</a>() ); 
     296<a name="l00388"></a>00388  
     297<a name="l00389"></a>00389         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() ); 
     298<a name="l00390"></a>00390         <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>-&gt;<a class="code" href="classldmat.html#4fd155f38eb6dd5af4bdf9c98a7999a9" title="Inplace symmetric multiplication by a SQUARE transpose of matrix $C$, i.e. $V = C&amp;#39;*V*C$...">mult_sym_t</a> ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom ); 
     299<a name="l00391"></a>00391         ( *_P ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span> 
     300<a name="l00392"></a>00392         *<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> = 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#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>,u ); <span class="comment">//y prediction</span> 
     301<a name="l00393"></a>00393         ( *_mu ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y-*<a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a> ); 
     302<a name="l00394"></a>00394  
     303<a name="l00395"></a>00395         <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#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.<a class="code" href="classenorm.html#609a7c33dbb4fdfab050f3bdd1122401" title="Compute log-probability of argument val.">evalpdflog</a> ( y );} 
     304<a name="l00396"></a>00396 }; 
     305<a name="l00397"></a>00397  
     306<a name="l00398"></a>00398  
     307<a name="l00399"></a>00399 <span class="preprocessor">#endif // KF_H</span> 
     308<a name="l00400"></a>00400 <span class="preprocessor"></span> 
     309<a name="l00401"></a>00401  
     310</pre></div><hr size="1"><address style="text-align: right;"><small>Generated on Wed Mar 12 16:15:44 2008 for mixpp by&nbsp; 
    274311<a href="http://www.doxygen.org/index.html"> 
    275312<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.3 </small></address>