Index: doc/html/libKF_8h-source.html
===================================================================
--- doc/html/libKF_8h-source.html (revision 219)
+++ doc/html/libKF_8h-source.html (revision 224)
@@ -135,174 +135,175 @@
 <a name="l00213"></a>00213 
 <a name="l00220"></a><a class="code" href="classEKFCh.html">00220</a> <span class="keyword">class </span><a class="code" href="classEKFCh.html" title="Extended Kalman Filter in Square root.">EKFCh</a> : <span class="keyword">public</span> <a class="code" href="classKalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a> {
-<a name="l00222"></a>00222         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* pfxu;
-<a name="l00224"></a>00224         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu;
-<a name="l00225"></a>00225 <span class="keyword">public</span>:
-<a name="l00227"></a>00227         <a class="code" href="classEKFCh.html#e9e39a9204db3dda88d06e47c1e19064" title="Default constructor.">EKFCh</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 name="l00229"></a>00229         <span class="keywordtype">void</span> <a class="code" href="classEKFCh.html#0216bed270df59fe65d0d62d41f8257c" 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 .">diffbifn</a>* pfxu, <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> Q0, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> R0 );
-<a name="l00231"></a>00231         <span class="keywordtype">void</span> <a class="code" href="classEKFCh.html#96f6edda324a0b7ef8b4e86cc7af60c1" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &amp;dt );
-<a name="l00232"></a>00232 };
-<a name="l00233"></a>00233 
-<a name="l00238"></a><a class="code" href="classKFcondQR.html">00238</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> {
-<a name="l00239"></a>00239 <span class="comment">//protected:</span>
-<a name="l00240"></a>00240 <span class="keyword">public</span>:
-<a name="l00242"></a><a class="code" href="classKFcondQR.html#3f3968f92c7bbe4b0902d5e14ecc1cb4">00242</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, (commonly known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvRQ ) {};
-<a name="l00243"></a>00243 
-<a name="l00244"></a>00244         <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 );
-<a name="l00245"></a>00245 };
-<a name="l00246"></a>00246 
-<a name="l00251"></a><a class="code" href="classKFcondR.html">00251</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> {
-<a name="l00252"></a>00252 <span class="comment">//protected:</span>
-<a name="l00253"></a>00253 <span class="keyword">public</span>:
-<a name="l00255"></a><a class="code" href="classKFcondR.html#d2acbb8e66c7ee592b1a9da5b429a69e">00255</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, (commonly known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvR ) {};
-<a name="l00256"></a>00256 
-<a name="l00257"></a>00257         <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> );
-<a name="l00258"></a>00258 };
-<a name="l00259"></a>00259 
-<a name="l00261"></a>00261 
-<a name="l00262"></a>00262 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
-<a name="l00263"></a><a class="code" href="classKalman.html#ce38e31810aea4db45a83ad05eaba009">00263</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 ),
-<a name="l00264"></a>00264                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
-<a name="l00265"></a>00265                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
-<a name="l00266"></a>00266                 Q(dimx), R(dimy),
-<a name="l00267"></a>00267                 est ( rv ), fy ( rvy ), _yp(fy._mu()),_Ry(fy._R()), _mu(est._mu()), _P(est._R()) {
-<a name="l00268"></a>00268 
-<a name="l00269"></a>00269         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> );
-<a name="l00270"></a>00270 
-<a name="l00271"></a>00271 <span class="comment">// copy values in pointers</span>
-<a name="l00272"></a>00272         <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> = K0.<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>;
-<a name="l00273"></a>00273         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> = K0.<a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>;
-<a name="l00274"></a>00274         <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> = K0.<a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a>;
-<a name="l00275"></a>00275         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> = K0.<a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>;
-<a name="l00276"></a>00276 
-<a name="l00277"></a>00277 }
-<a name="l00278"></a>00278 
-<a name="l00279"></a>00279 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
-<a name="l00280"></a><a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3">00280</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 ),
-<a name="l00281"></a>00281                 <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() ),
-<a name="l00282"></a>00282                 <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> ),
-<a name="l00283"></a>00283                 <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>),
-<a name="l00284"></a>00284                 <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> ),  <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a>(<a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>()),<a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>(<a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._R()),<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>(<a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>.<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>()), <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>(<a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._R()) {
-<a name="l00285"></a>00285 };
-<a name="l00286"></a>00286 
-<a name="l00287"></a>00287 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
-<a name="l00288"></a><a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a">00288</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 ) {
-<a name="l00289"></a>00289         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> );
-<a name="l00290"></a>00290         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> );
-<a name="l00291"></a>00291         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> );
-<a name="l00292"></a>00292         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> );
-<a name="l00293"></a>00293         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> );
-<a name="l00294"></a>00294         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> );
-<a name="l00295"></a>00295 
-<a name="l00296"></a>00296         <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> = A0;
-<a name="l00297"></a>00297         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a> = B0;
-<a name="l00298"></a>00298         <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a> = C0;
-<a name="l00299"></a>00299         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a> = D0;
-<a name="l00300"></a>00300         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0;
-<a name="l00301"></a>00301         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0;
-<a name="l00302"></a>00302 }
-<a name="l00303"></a>00303 
-<a name="l00304"></a>00304 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
-<a name="l00305"></a><a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c">00305</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 ) {
-<a name="l00306"></a>00306         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> );
-<a name="l00307"></a>00307 
-<a name="l00308"></a>00308         sq_T iRy(<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>);
-<a name="l00309"></a>00309         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 );
-<a name="l00310"></a>00310         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 );
-<a name="l00311"></a>00311         <span class="comment">//Time update</span>
-<a name="l00312"></a>00312         <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>* <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> + <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>*u;
-<a name="l00313"></a>00313         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span>
-<a name="l00314"></a>00314         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> );
-<a name="l00315"></a>00315         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>  +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>;
-<a name="l00316"></a>00316 
-<a name="l00317"></a>00317         <span class="comment">//Data update</span>
-<a name="l00318"></a>00318         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span>
-<a name="l00319"></a>00319         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> );
-<a name="l00320"></a>00320         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>  +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>;
-<a name="l00321"></a>00321 
-<a name="l00322"></a>00322         mat Pfull = <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.to_mat();
-<a name="l00323"></a>00323 
-<a name="l00324"></a>00324         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>.inv ( iRy ); <span class="comment">// result is in _iRy;</span>
-<a name="l00325"></a>00325         <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() * ( iRy.to_mat() );
-<a name="l00326"></a>00326 
-<a name="l00327"></a>00327         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() );
-<a name="l00328"></a>00328         iRy.mult_sym_t ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom );
-<a name="l00329"></a>00329         (<a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span>
-<a name="l00330"></a>00330         (<a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> ) = <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>* <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>  +<a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>*u; <span class="comment">//y prediction</span>
-<a name="l00331"></a>00331         (<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y- <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a>  );
-<a name="l00332"></a>00332 
+<a name="l00221"></a>00221         <span class="keyword">protected</span>:
+<a name="l00223"></a><a class="code" href="classEKFCh.html#f4d595c2a674f8c6831eb1909bf21757">00223</a>         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classEKFCh.html#f4d595c2a674f8c6831eb1909bf21757" title="Internal Model f(x,u).">pfxu</a>;
+<a name="l00225"></a><a class="code" href="classEKFCh.html#02503fee942fb63bfbe27354cdf8e734">00225</a>         <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classEKFCh.html#02503fee942fb63bfbe27354cdf8e734" title="Observation Model h(x,u).">phxu</a>;
+<a name="l00226"></a>00226 <span class="keyword">public</span>:
+<a name="l00228"></a>00228         <a class="code" href="classEKFCh.html#e9e39a9204db3dda88d06e47c1e19064" title="Default constructor.">EKFCh</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 name="l00230"></a>00230         <span class="keywordtype">void</span> <a class="code" href="classEKFCh.html#0216bed270df59fe65d0d62d41f8257c" 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 .">diffbifn</a>* <a class="code" href="classEKFCh.html#f4d595c2a674f8c6831eb1909bf21757" title="Internal Model f(x,u).">pfxu</a>, <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classEKFCh.html#02503fee942fb63bfbe27354cdf8e734" title="Observation Model h(x,u).">phxu</a>, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> Q0, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> R0 );
+<a name="l00232"></a>00232         <span class="keywordtype">void</span> <a class="code" href="classEKFCh.html#96f6edda324a0b7ef8b4e86cc7af60c1" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &amp;dt );
+<a name="l00233"></a>00233 };
+<a name="l00234"></a>00234 
+<a name="l00239"></a><a class="code" href="classKFcondQR.html">00239</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> {
+<a name="l00240"></a>00240 <span class="comment">//protected:</span>
+<a name="l00241"></a>00241 <span class="keyword">public</span>:
+<a name="l00243"></a><a class="code" href="classKFcondQR.html#3f3968f92c7bbe4b0902d5e14ecc1cb4">00243</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, (commonly known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvRQ ) {};
+<a name="l00244"></a>00244 
+<a name="l00245"></a>00245         <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 );
+<a name="l00246"></a>00246 };
+<a name="l00247"></a>00247 
+<a name="l00252"></a><a class="code" href="classKFcondR.html">00252</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> {
+<a name="l00253"></a>00253 <span class="comment">//protected:</span>
+<a name="l00254"></a>00254 <span class="keyword">public</span>:
+<a name="l00256"></a><a class="code" href="classKFcondR.html#d2acbb8e66c7ee592b1a9da5b429a69e">00256</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, (commonly known as UD).">ldmat</a>&gt; ( rvx, rvy,rvu ),<a class="code" href="classBMcond.html" title="Conditional Bayesian Filter.">BMcond</a> ( rvR ) {};
+<a name="l00257"></a>00257 
+<a name="l00258"></a>00258         <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> );
+<a name="l00259"></a>00259 };
+<a name="l00260"></a>00260 
+<a name="l00262"></a>00262 
+<a name="l00263"></a>00263 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
+<a name="l00264"></a><a class="code" href="classKalman.html#ce38e31810aea4db45a83ad05eaba009">00264</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 ),
+<a name="l00265"></a>00265                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
+<a name="l00266"></a>00266                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
+<a name="l00267"></a>00267                 Q(dimx), R(dimy),
+<a name="l00268"></a>00268                 est ( rv ), fy ( rvy ), _yp(fy._mu()),_Ry(fy._R()), _mu(est._mu()), _P(est._R()) {
+<a name="l00269"></a>00269 
+<a name="l00270"></a>00270         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> );
+<a name="l00271"></a>00271 
+<a name="l00272"></a>00272 <span class="comment">// copy values in pointers</span>
+<a name="l00273"></a>00273         <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> = K0.<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>;
+<a name="l00274"></a>00274         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> = K0.<a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>;
+<a name="l00275"></a>00275         <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> = K0.<a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a>;
+<a name="l00276"></a>00276         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> = K0.<a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>;
+<a name="l00277"></a>00277 
+<a name="l00278"></a>00278 }
+<a name="l00279"></a>00279 
+<a name="l00280"></a>00280 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
+<a name="l00281"></a><a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3">00281</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 ),
+<a name="l00282"></a>00282                 <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() ),
+<a name="l00283"></a>00283                 <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> ),
+<a name="l00284"></a>00284                 <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>),
+<a name="l00285"></a>00285                 <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> ),  <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a>(<a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>()),<a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>(<a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>._R()),<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>(<a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>.<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>()), <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>(<a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>._R()) {
+<a name="l00286"></a>00286 };
+<a name="l00287"></a>00287 
+<a name="l00288"></a>00288 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
+<a name="l00289"></a><a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a">00289</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 ) {
+<a name="l00290"></a>00290         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> );
+<a name="l00291"></a>00291         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> );
+<a name="l00292"></a>00292         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> );
+<a name="l00293"></a>00293         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> );
+<a name="l00294"></a>00294         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> );
+<a name="l00295"></a>00295         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> );
+<a name="l00296"></a>00296 
+<a name="l00297"></a>00297         <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> = A0;
+<a name="l00298"></a>00298         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a> = B0;
+<a name="l00299"></a>00299         <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a> = C0;
+<a name="l00300"></a>00300         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a> = D0;
+<a name="l00301"></a>00301         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0;
+<a name="l00302"></a>00302         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0;
+<a name="l00303"></a>00303 }
+<a name="l00304"></a>00304 
+<a name="l00305"></a>00305 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
+<a name="l00306"></a><a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c">00306</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 ) {
+<a name="l00307"></a>00307         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> );
+<a name="l00308"></a>00308 
+<a name="l00309"></a>00309         sq_T iRy(<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>);
+<a name="l00310"></a>00310         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 );
+<a name="l00311"></a>00311         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 );
+<a name="l00312"></a>00312         <span class="comment">//Time update</span>
+<a name="l00313"></a>00313         <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> = <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>* <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> + <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>*u;
+<a name="l00314"></a>00314         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span>
+<a name="l00315"></a>00315         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> );
+<a name="l00316"></a>00316         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>  +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>;
+<a name="l00317"></a>00317 
+<a name="l00318"></a>00318         <span class="comment">//Data update</span>
+<a name="l00319"></a>00319         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span>
+<a name="l00320"></a>00320         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> );
+<a name="l00321"></a>00321         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>  +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>;
+<a name="l00322"></a>00322 
+<a name="l00323"></a>00323         mat Pfull = <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.to_mat();
+<a name="l00324"></a>00324 
+<a name="l00325"></a>00325         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>.inv ( iRy ); <span class="comment">// result is in _iRy;</span>
+<a name="l00326"></a>00326         <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() * ( iRy.to_mat() );
+<a name="l00327"></a>00327 
+<a name="l00328"></a>00328         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() );
+<a name="l00329"></a>00329         iRy.mult_sym_t ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom );
+<a name="l00330"></a>00330         (<a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span>
+<a name="l00331"></a>00331         (<a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> ) = <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>* <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>  +<a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>*u; <span class="comment">//y prediction</span>
+<a name="l00332"></a>00332         (<a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y- <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a>  );
 <a name="l00333"></a>00333 
-<a name="l00334"></a>00334         <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>
-<a name="l00335"></a>00335                 <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>.evallog ( y );
-<a name="l00336"></a>00336         }
-<a name="l00337"></a>00337 
-<a name="l00338"></a>00338 <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>
-<a name="l00339"></a>00339 
-<a name="l00340"></a>00340 };
-<a name="l00341"></a>00341  
-<a name="l00342"></a>00342 
+<a name="l00334"></a>00334 
+<a name="l00335"></a>00335         <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>
+<a name="l00336"></a>00336                 <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>.evallog ( y );
+<a name="l00337"></a>00337         }
+<a name="l00338"></a>00338 
+<a name="l00339"></a>00339 <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>
+<a name="l00340"></a>00340 
+<a name="l00341"></a>00341 };
+<a name="l00342"></a>00342  
 <a name="l00343"></a>00343 
-<a name="l00344"></a>00344 <span class="comment">//TODO why not const pointer??</span>
-<a name="l00345"></a>00345 
-<a name="l00346"></a>00346 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
-<a name="l00347"></a><a class="code" href="classEKF.html#ea4f3254cacf0a92d2a820b1201d049e">00347</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;sq_T&gt; ( rvx0,rvy0,rvu0 ) {}
-<a name="l00348"></a>00348 
-<a name="l00349"></a>00349 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
-<a name="l00350"></a><a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66">00350</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 .">diffbifn</a>* pfxu0,  <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu0,<span class="keyword">const</span> sq_T Q0,<span class="keyword">const</span> sq_T R0 ) {
-<a name="l00351"></a>00351         pfxu = pfxu0;
-<a name="l00352"></a>00352         phxu = phxu0;
-<a name="l00353"></a>00353 
-<a name="l00354"></a>00354         <span class="comment">//initialize matrices A C, later, these will be only updated!</span>
-<a name="l00355"></a>00355         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#fe803a81d2d847b0b1db3c6b29c18061" 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> );
-<a name="l00356"></a>00356 <span class="comment">//      pfxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),B,true );</span>
-<a name="l00357"></a>00357         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>.clear();
-<a name="l00358"></a>00358         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#fe803a81d2d847b0b1db3c6b29c18061" 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> );
-<a name="l00359"></a>00359 <span class="comment">//      phxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),D,true );</span>
-<a name="l00360"></a>00360         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>.clear();
-<a name="l00361"></a>00361 
-<a name="l00362"></a>00362         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0;
-<a name="l00363"></a>00363         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0;
-<a name="l00364"></a>00364 }
-<a name="l00365"></a>00365 
-<a name="l00366"></a>00366 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
-<a name="l00367"></a><a class="code" href="classEKF.html#c79c62c9b3e0b56b3aaa1b6f1d9a7af7">00367</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 ) {
-<a name="l00368"></a>00368         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> );
-<a name="l00369"></a>00369 
-<a name="l00370"></a>00370         sq_T iRy(<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 name="l00371"></a>00371         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 );
-<a name="l00372"></a>00372         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 );
-<a name="l00373"></a>00373         <span class="comment">//Time update</span>
-<a name="l00374"></a>00374         <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> = pfxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates  (VS: Do we really need common eval? ).">eval</a> ( <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>, u );
-<a name="l00375"></a>00375         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#fe803a81d2d847b0b1db3c6b29c18061" 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>
-<a name="l00376"></a>00376 
-<a name="l00377"></a>00377         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span>
-<a name="l00378"></a>00378         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#5530d2756b5d991de755e6121c9a452e" title="Inplace symmetric multiplication by a SQUARE matrix , i.e. .">mult_sym</a> ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> );
-<a name="l00379"></a>00379         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>;
-<a name="l00380"></a>00380 
-<a name="l00381"></a>00381         <span class="comment">//Data update</span>
-<a name="l00382"></a>00382         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#fe803a81d2d847b0b1db3c6b29c18061" 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>
-<a name="l00383"></a>00383         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span>
-<a name="l00384"></a>00384         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#5530d2756b5d991de755e6121c9a452e" title="Inplace symmetric multiplication by a SQUARE matrix , i.e. .">mult_sym</a> ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> );
-<a name="l00385"></a>00385         ( <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> ) +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>;
-<a name="l00386"></a>00386 
-<a name="l00387"></a>00387         mat Pfull = <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#f54fc955e8e3b43d15afa92124bc24b3" title="Conversion to full matrix.">to_mat</a>();
-<a name="l00388"></a>00388 
-<a name="l00389"></a>00389         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>.<a class="code" href="classfsqmat.html#9fa853e1ca28f2a1a1c43377e798ecb1" title="Matrix inversion preserving the chosen form.">inv</a> ( iRy ); <span class="comment">// result is in _iRy;</span>
-<a name="l00390"></a>00390         <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() * ( iRy.to_mat() );
-<a name="l00391"></a>00391 
-<a name="l00392"></a>00392         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() );
-<a name="l00393"></a>00393         iRy.mult_sym_t ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom );
-<a name="l00394"></a>00394         (<a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span>
-<a name="l00395"></a>00395         <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> = phxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates  (VS: Do we really need common eval? ).">eval</a> ( <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>,u ); <span class="comment">//y prediction</span>
-<a name="l00396"></a>00396         ( <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y-<a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> );
-<a name="l00397"></a>00397 
-<a name="l00398"></a>00398         <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="classeEF.html#357512dd565e199904d367294b7dd862" title="Evaluate normalized log-probability.">evallog</a> ( y );}
-<a name="l00399"></a>00399 };
-<a name="l00400"></a>00400 
+<a name="l00344"></a>00344 
+<a name="l00345"></a>00345 <span class="comment">//TODO why not const pointer??</span>
+<a name="l00346"></a>00346 
+<a name="l00347"></a>00347 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
+<a name="l00348"></a><a class="code" href="classEKF.html#ea4f3254cacf0a92d2a820b1201d049e">00348</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;sq_T&gt; ( rvx0,rvy0,rvu0 ) {}
+<a name="l00349"></a>00349 
+<a name="l00350"></a>00350 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
+<a name="l00351"></a><a class="code" href="classEKF.html#28d058ae4d24d992d2f055419a06ee66">00351</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 .">diffbifn</a>* pfxu0,  <a class="code" href="classdiffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu0,<span class="keyword">const</span> sq_T Q0,<span class="keyword">const</span> sq_T R0 ) {
+<a name="l00352"></a>00352         pfxu = pfxu0;
+<a name="l00353"></a>00353         phxu = phxu0;
+<a name="l00354"></a>00354 
+<a name="l00355"></a>00355         <span class="comment">//initialize matrices A C, later, these will be only updated!</span>
+<a name="l00356"></a>00356         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#fe803a81d2d847b0b1db3c6b29c18061" 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> );
+<a name="l00357"></a>00357 <span class="comment">//      pfxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),B,true );</span>
+<a name="l00358"></a>00358         <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>.clear();
+<a name="l00359"></a>00359         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#fe803a81d2d847b0b1db3c6b29c18061" 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> );
+<a name="l00360"></a>00360 <span class="comment">//      phxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),D,true );</span>
+<a name="l00361"></a>00361         <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>.clear();
+<a name="l00362"></a>00362 
+<a name="l00363"></a>00363         <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a> = R0;
+<a name="l00364"></a>00364         <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a> = Q0;
+<a name="l00365"></a>00365 }
+<a name="l00366"></a>00366 
+<a name="l00367"></a>00367 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
+<a name="l00368"></a><a class="code" href="classEKF.html#c79c62c9b3e0b56b3aaa1b6f1d9a7af7">00368</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 ) {
+<a name="l00369"></a>00369         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> );
+<a name="l00370"></a>00370 
+<a name="l00371"></a>00371         sq_T iRy(<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 name="l00372"></a>00372         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 );
+<a name="l00373"></a>00373         vec y = dt.get ( 0,<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>-1 );
+<a name="l00374"></a>00374         <span class="comment">//Time update</span>
+<a name="l00375"></a>00375         <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> = pfxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates  (VS: Do we really need common eval? ).">eval</a> ( <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>, u );
+<a name="l00376"></a>00376         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#fe803a81d2d847b0b1db3c6b29c18061" 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>
+<a name="l00377"></a>00377 
+<a name="l00378"></a>00378         <span class="comment">//P  = A*P*A.transpose() + Q; in sq_T</span>
+<a name="l00379"></a>00379         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#5530d2756b5d991de755e6121c9a452e" title="Inplace symmetric multiplication by a SQUARE matrix , i.e. .">mult_sym</a> ( <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a> );
+<a name="l00380"></a>00380         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> +=<a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>;
+<a name="l00381"></a>00381 
+<a name="l00382"></a>00382         <span class="comment">//Data update</span>
+<a name="l00383"></a>00383         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#fe803a81d2d847b0b1db3c6b29c18061" 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>
+<a name="l00384"></a>00384         <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span>
+<a name="l00385"></a>00385         <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#5530d2756b5d991de755e6121c9a452e" title="Inplace symmetric multiplication by a SQUARE matrix , i.e. .">mult_sym</a> ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>, <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> );
+<a name="l00386"></a>00386         ( <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a> ) +=<a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>;
+<a name="l00387"></a>00387 
+<a name="l00388"></a>00388         mat Pfull = <a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#f54fc955e8e3b43d15afa92124bc24b3" title="Conversion to full matrix.">to_mat</a>();
+<a name="l00389"></a>00389 
+<a name="l00390"></a>00390         <a class="code" href="classKalman.html#45c9f928d2d62e0c884900fb3380f904" title="cache of fy.R">_Ry</a>.<a class="code" href="classfsqmat.html#9fa853e1ca28f2a1a1c43377e798ecb1" title="Matrix inversion preserving the chosen form.">inv</a> ( iRy ); <span class="comment">// result is in _iRy;</span>
+<a name="l00391"></a>00391         <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() * ( iRy.to_mat() );
+<a name="l00392"></a>00392 
+<a name="l00393"></a>00393         sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() );
+<a name="l00394"></a>00394         iRy.mult_sym_t ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*Pfull,pom );
+<a name="l00395"></a>00395         (<a class="code" href="classKalman.html#9fb808cc94a4c2652e1fb93be9bb7dcf" title="cache of est.R">_P</a> ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span>
+<a name="l00396"></a>00396         <a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> = phxu-&gt;<a class="code" href="classdiffbifn.html#ad7673e16aa1a046b131b24c731c4632" title="Evaluates  (VS: Do we really need common eval? ).">eval</a> ( <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a>,u ); <span class="comment">//y prediction</span>
+<a name="l00397"></a>00397         ( <a class="code" href="classKalman.html#fe803a81d2d847b0b1db3c6b29c18061" title="cache of est.mu">_mu</a> ) += <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>* ( y-<a class="code" href="classKalman.html#764bbc95238eda11fc81c5ebd0b1dcfd" title="cache of fy.mu">_yp</a> );
+<a name="l00398"></a>00398 
+<a name="l00399"></a>00399         <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="classeEF.html#357512dd565e199904d367294b7dd862" title="Evaluate normalized log-probability.">evallog</a> ( y );}
+<a name="l00400"></a>00400 };
 <a name="l00401"></a>00401 
-<a name="l00402"></a>00402 <span class="preprocessor">#endif // KF_H</span>
-<a name="l00403"></a>00403 <span class="preprocessor"></span>
-<a name="l00404"></a>00404 
+<a name="l00402"></a>00402 
+<a name="l00403"></a>00403 <span class="preprocessor">#endif // KF_H</span>
+<a name="l00404"></a>00404 <span class="preprocessor"></span>
+<a name="l00405"></a>00405 
 </pre></div></div>
-<hr size="1"><address style="text-align: right;"><small>Generated on Thu Dec 4 14:42:13 2008 for mixpp by&nbsp;
+<hr size="1"><address style="text-align: right;"><small>Generated on Mon Jan 5 19:29:09 2009 for mixpp by&nbsp;
 <a href="http://www.doxygen.org/index.html">
 <img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.6 </small></address>
