root/doc/html/libKF_8h-source.html @ 33

Revision 33, 52.3 kB (checked in by smidl, 17 years ago)

Oprava PF a MPF + jejich implementace pro pmsm system

RevLine 
[8]1<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
2<html><head><meta http-equiv="Content-Type" content="text/html;charset=UTF-8">
[19]3<title>mixpp: work/mixpp/bdm/estim/libKF.h Source File</title>
[8]4<link href="doxygen.css" rel="stylesheet" type="text/css">
5<link href="tabs.css" rel="stylesheet" type="text/css">
6</head><body>
7<!-- Generated by Doxygen 1.5.3 -->
8<div class="tabs">
9  <ul>
10    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
11    <li><a href="annotated.html"><span>Classes</span></a></li>
12    <li class="current"><a href="files.html"><span>Files</span></a></li>
[28]13    <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
[8]14  </ul>
15</div>
[19]16<h1>work/mixpp/bdm/estim/libKF.h</h1><a href="libKF_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001
[8]17<a name="l00013"></a>00013 <span class="preprocessor">#ifndef KF_H</span>
18<a name="l00014"></a>00014 <span class="preprocessor"></span><span class="preprocessor">#define KF_H</span>
19<a name="l00015"></a>00015 <span class="preprocessor"></span>
20<a name="l00016"></a>00016 <span class="preprocessor">#include &lt;itpp/itbase.h&gt;</span>
[22]21<a name="l00017"></a>00017 <span class="preprocessor">#include "../stat/libFN.h"</span>
[32]22<a name="l00018"></a>00018 <span class="preprocessor">#include "../stat/libEF.h"</span>
[8]23<a name="l00019"></a>00019
24<a name="l00020"></a>00020
25<a name="l00021"></a>00021 <span class="keyword">using namespace </span>itpp;
26<a name="l00022"></a>00022
[33]27<a name="l00027"></a><a class="code" href="classKalmanFull.html">00027</a> <span class="keyword">class </span><a class="code" href="classKalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> {
28<a name="l00028"></a>00028         <span class="keywordtype">int</span> dimx, dimy, dimu;
29<a name="l00029"></a>00029         mat A, B, C, D, R, Q;
30<a name="l00030"></a>00030
31<a name="l00031"></a>00031         <span class="comment">//cache</span>
32<a name="l00032"></a>00032         mat _Pp, _Ry, _iRy, _K;
33<a name="l00033"></a>00033 <span class="keyword">public</span>:
34<a name="l00034"></a>00034         <span class="comment">//posterior</span>
35<a name="l00036"></a><a class="code" href="classKalmanFull.html#fb5aec635e2720cc5ac31bc01c18a68a">00036</a> <span class="comment"></span>        vec <a class="code" href="classKalmanFull.html#fb5aec635e2720cc5ac31bc01c18a68a" title="Mean value of the posterior density.">mu</a>;
36<a name="l00038"></a><a class="code" href="classKalmanFull.html#b75dc059e84fa8ffc076203b30f926cc">00038</a>         mat <a class="code" href="classKalmanFull.html#b75dc059e84fa8ffc076203b30f926cc" title="Variance of the posterior density.">P</a>;
37<a name="l00039"></a>00039
38<a name="l00040"></a>00040 <span class="keyword">public</span>:
39<a name="l00042"></a>00042         <a class="code" href="classKalmanFull.html#7197ab6e7380790006394eabd3b97043" title="Full constructor.">KalmanFull</a> ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
40<a name="l00044"></a>00044         <span class="keywordtype">void</span> <a class="code" href="classKalmanFull.html#13a041cd98ff157703766be275a657bb" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &amp;dt );
41<a name="l00046"></a>00046         <span class="keyword">friend</span> std::ostream &amp;<a class="code" href="classKalmanFull.html#86ba216243ed95bb46d80d88775d16af" title="print elements of KF">operator&lt;&lt; </a>( std::ostream &amp;os, <span class="keyword">const</span> <a class="code" href="classKalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> &amp;kf );
42<a name="l00047"></a>00047
43<a name="l00048"></a>00048 };
[8]44<a name="l00049"></a>00049
[33]45<a name="l00050"></a>00050
46<a name="l00058"></a>00058 <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
47<a name="l00059"></a>00059
48<a name="l00060"></a><a class="code" href="classKalman.html">00060</a> <span class="keyword">class </span><a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a> : <span class="keyword">public</span> <a class="code" href="classBM.html" title="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</a> {
49<a name="l00061"></a>00061 <span class="keyword">protected</span>:
50<a name="l00063"></a><a class="code" href="classKalman.html#7501230c2fafa3655887d2da23b3184c">00063</a>         <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>;
51<a name="l00065"></a><a class="code" href="classKalman.html#44a16ffd5ac1e6e39bae34fea9e1e498">00065</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>;
52<a name="l00067"></a><a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb">00067</a>         <span class="keywordtype">int</span> <a class="code" href="classKalman.html#39c8c403b46fa3b8c7da77cb2e3729eb" title="cache of rv.count()">dimx</a>;
53<a name="l00069"></a><a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a">00069</a>         <span class="keywordtype">int</span> <a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>;
54<a name="l00071"></a><a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce">00071</a>         <span class="keywordtype">int</span> <a class="code" href="classKalman.html#b0153795a1444b6968a86409c778d9ce" title="cache of rvu.count()">dimu</a>;
55<a name="l00073"></a><a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9">00073</a>         mat <a class="code" href="classKalman.html#5e02efe86ee91e9c74b93b425fe060b9" title="Matrix A.">A</a>;
56<a name="l00075"></a><a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a">00075</a>         mat <a class="code" href="classKalman.html#dc87704284a6c0bca13bf51f4345a50a" title="Matrix B.">B</a>;
57<a name="l00077"></a><a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13">00077</a>         mat <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>;
58<a name="l00079"></a><a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1">00079</a>         mat <a class="code" href="classKalman.html#d69f774ba3335c970c1c5b1d182f4dd1" title="Matrix D.">D</a>;
59<a name="l00081"></a><a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9">00081</a>         sq_T <a class="code" href="classKalman.html#9b69015c800eb93f3ee49da23a6f55d9" title="Matrix Q in square-root form.">Q</a>;
60<a name="l00083"></a><a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec">00083</a>         sq_T <a class="code" href="classKalman.html#11d171dc0e0ab111c56a70f98b97b3ec" title="Matrix R in square-root form.">R</a>;
61<a name="l00084"></a>00084
62<a name="l00086"></a><a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424">00086</a>         <a class="code" href="classenorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm&lt;sq_T&gt;</a> <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>;
63<a name="l00088"></a><a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f">00088</a>         <a class="code" href="classenorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm&lt;sq_T&gt;</a> <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>;
64<a name="l00089"></a>00089
65<a name="l00091"></a><a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132">00091</a>         mat <a class="code" href="classKalman.html#d422f51467c7a06174af2476d2826132" title="placeholder for Kalman gain">_K</a>;
66<a name="l00093"></a><a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8">00093</a>         vec* <a class="code" href="classKalman.html#5188eb0329f8561f0b357af329769bf8" title="cache of fy.mu">_yp</a>;
67<a name="l00095"></a><a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674">00095</a>         sq_T* <a class="code" href="classKalman.html#e17dd745daa8a958035a334a56fa4674" title="cache of fy.R">_Ry</a>;
68<a name="l00097"></a><a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874">00097</a>         sq_T* <a class="code" href="classKalman.html#8a35bd14afa5a2d9bbd23ad333bec874" title="cache of fy.iR">_iRy</a>;
69<a name="l00099"></a><a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734">00099</a>         vec* <a class="code" href="classKalman.html#d1f669b5b3421a070cc75d77b55ba734" title="cache of est.mu">_mu</a>;
70<a name="l00101"></a><a class="code" href="classKalman.html#b3388218567128a797e69b109138271d">00101</a>         sq_T* <a class="code" href="classKalman.html#b3388218567128a797e69b109138271d" title="cache of est.R">_P</a>;
71<a name="l00103"></a><a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c">00103</a>         sq_T* <a class="code" href="classKalman.html#13fec2c93d8a132201e28b70270acf5c" title="cache of est.iR">_iP</a>;
72<a name="l00104"></a>00104
73<a name="l00105"></a>00105 <span class="keyword">public</span>:
74<a name="l00107"></a>00107         <a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3" title="Default constructor.">Kalman</a> ( <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvx0, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvy0, <a class="code" href="classRV.html" title="Class representing variables, most often random variables.">RV</a> rvu0 );
75<a name="l00109"></a>00109         <a class="code" href="classKalman.html#3d56b0a97b8c1e25fdd3b10eef3c2ad3" title="Default constructor.">Kalman</a> ( <span class="keyword">const</span> <a class="code" href="classKalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman&lt;sq_T&gt;</a> &amp;K0 );
76<a name="l00111"></a>00111         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#239b28a0380946f5749b2f8d2807f93a" title="Set parameters with check of relevance.">set_parameters</a> ( <span class="keyword">const</span> mat &amp;A0,<span class="keyword">const</span> mat &amp;B0,<span class="keyword">const</span> mat &amp;C0,<span class="keyword">const</span> mat &amp;D0,<span class="keyword">const</span> sq_T &amp;R0,<span class="keyword">const</span> sq_T &amp;Q0 );
77<a name="l00113"></a><a class="code" href="classKalman.html#80bcf29466d9a9dd2b8f74699807d0c0">00113</a>         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#80bcf29466d9a9dd2b8f74699807d0c0" title="Set estimate values, used e.g. in initialization.">set_est</a> ( <span class="keyword">const</span> vec &amp;mu0, <span class="keyword">const</span> sq_T &amp;P0 ) {
78<a name="l00114"></a>00114                 sq_T pom(<a class="code" href="classKalman.html#ba17b956df1e38b31fbbc299c8213b6a" title="cache of rvy.count()">dimy</a>);
79<a name="l00115"></a>00115                 <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>.set_parameters ( mu0,P0 );
80<a name="l00116"></a>00116                 P0.mult_sym(<a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>,pom);
81<a name="l00117"></a>00117                 <a class="code" href="classKalman.html#e580ab06483952bd03f2e651763e184f" title="preditive density on $y_t$">fy</a>.set_parameters ( <a class="code" href="classKalman.html#86a805cd6515872d1132ad0d6eb5dc13" title="Matrix C.">C</a>*mu0, pom );
82<a name="l00118"></a>00118         };
83<a name="l00119"></a>00119
84<a name="l00121"></a>00121         <span class="keywordtype">void</span> <a class="code" href="classKalman.html#7750ffd73f261828a32c18aaeb65c75c" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &amp;dt );
85<a name="l00123"></a><a class="code" href="classKalman.html#a213c57aef55b2645e550bed81cfc0d4">00123</a>         <a class="code" href="classepdf.html" title="Probability density function with numerical statistics, e.g. posterior density.">epdf</a>&amp; <a class="code" href="classKalman.html#a213c57aef55b2645e550bed81cfc0d4" title="access function">_epdf</a>() {<span class="keywordflow">return</span> <a class="code" href="classKalman.html#5568c74bac67ae6d3b1061dba60c9424" title="posterior density on $x_t$">est</a>;}
86<a name="l00124"></a>00124 };
87<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 };
[32]98<a name="l00146"></a>00146
[33]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 };
[32]114<a name="l00172"></a>00172
[33]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 ) {
120<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> );
[32]126<a name="l00185"></a>00185
[33]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 }
[32]173<a name="l00232"></a>00232
[33]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>;
185<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>;
190<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();
192<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() );
196<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
[32]203<a name="l00262"></a>00262
[33]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         }
[32]207<a name="l00266"></a>00266
[33]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;
221<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>
243<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>;
247<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>();
255<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>
272<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;
[8]274<a href="http://www.doxygen.org/index.html">
275<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.3 </small></address>
276</body>
277</html>
Note: See TracBrowser for help on using the browser.