Context Navigation

← Previous Change
Next Change →

libKF_8h-source.html

Timestamp:

03/05/08 16:01:56 (17 years ago)

Author:

smidl

Message:

Oprava PF a MPF + jejich implementace pro pmsm system

Files:

: 1 modified

doc/html/libKF_8h-source.html (modified) (1 diff)

Legend:

: Unmodified
: Added
: Removed

doc/html/libKF_8h-source.html

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

Context Navigation

Changeset 33 for doc/html/libKF_8h-source.html

Legend:

doc/html/libKF_8h-source.html

Download in other formats: