73 | | <a name="l00020"></a>00020 |
74 | | <a name="l00021"></a>00021 <span class="keyword">namespace </span>bdm |
75 | | <a name="l00022"></a>00022 { |
76 | | <a name="l00023"></a>00023 |
77 | | <a name="l00028"></a><a class="code" href="classbdm_1_1KalmanFull.html">00028</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> |
78 | | <a name="l00029"></a>00029 { |
79 | | <a name="l00030"></a>00030 <span class="keyword">protected</span>: |
80 | | <a name="l00031"></a>00031 <span class="keywordtype">int</span> dimx, dimy, dimu; |
81 | | <a name="l00032"></a>00032 mat A, B, C, D, R, Q; |
82 | | <a name="l00033"></a>00033 |
83 | | <a name="l00034"></a>00034 <span class="comment">//cache</span> |
84 | | <a name="l00035"></a>00035 mat _Pp, _Ry, _iRy, _K; |
85 | | <a name="l00036"></a>00036 <span class="keyword">public</span>: |
86 | | <a name="l00037"></a>00037 <span class="comment">//posterior</span> |
87 | | <a name="l00039"></a><a class="code" href="classbdm_1_1KalmanFull.html#2defb75e58892615c5f95fd844f3a666">00039</a> <span class="comment"></span> vec <a class="code" href="classbdm_1_1KalmanFull.html#2defb75e58892615c5f95fd844f3a666" title="Mean value of the posterior density.">mu</a>; |
88 | | <a name="l00041"></a><a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c">00041</a> mat <a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c" title="Variance of the posterior density.">P</a>; |
89 | | <a name="l00042"></a>00042 |
90 | | <a name="l00043"></a>00043 <span class="keywordtype">bool</span> evalll; |
91 | | <a name="l00044"></a>00044 <span class="keywordtype">double</span> ll; |
92 | | <a name="l00045"></a>00045 <span class="keyword">public</span>: |
93 | | <a name="l00047"></a>00047 <a class="code" href="classbdm_1_1KalmanFull.html#bdcc98c8b18c1cbdebdf218ae838fd11" title="For EKFfull;.">KalmanFull</a> ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 ); |
94 | | <a name="l00049"></a>00049 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KalmanFull.html#081924bc97f453f674bb982b7951d053" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
95 | | <a name="l00051"></a>00051 <span class="keyword">friend</span> std::ostream &<a class="code" href="classbdm_1_1KalmanFull.html#86ba216243ed95bb46d80d88775d16af" title="print elements of KF">operator<< </a>( std::ostream &os, <span class="keyword">const</span> <a class="code" href="classbdm_1_1KalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> &kf ); |
96 | | <a name="l00053"></a><a class="code" href="classbdm_1_1KalmanFull.html#bdcc98c8b18c1cbdebdf218ae838fd11">00053</a> <a class="code" href="classbdm_1_1KalmanFull.html#bdcc98c8b18c1cbdebdf218ae838fd11" title="For EKFfull;.">KalmanFull</a>() {}; |
97 | | <a name="l00054"></a>00054 }; |
98 | | <a name="l00055"></a>00055 |
| 73 | <a name="l00020"></a>00020 <span class="preprocessor">#include "../user_info.h"</span> |
| 74 | <a name="l00021"></a>00021 |
| 75 | <a name="l00022"></a>00022 <span class="keyword">namespace </span>bdm |
| 76 | <a name="l00023"></a>00023 { |
| 77 | <a name="l00024"></a>00024 |
| 78 | <a name="l00029"></a><a class="code" href="classbdm_1_1KalmanFull.html">00029</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> |
| 79 | <a name="l00030"></a>00030 { |
| 80 | <a name="l00031"></a>00031 <span class="keyword">protected</span>: |
| 81 | <a name="l00032"></a>00032 <span class="keywordtype">int</span> dimx, dimy, dimu; |
| 82 | <a name="l00033"></a>00033 mat A, B, C, D, R, Q; |
| 83 | <a name="l00034"></a>00034 |
| 84 | <a name="l00035"></a>00035 <span class="comment">//cache</span> |
| 85 | <a name="l00036"></a>00036 mat _Pp, _Ry, _iRy, _K; |
| 86 | <a name="l00037"></a>00037 <span class="keyword">public</span>: |
| 87 | <a name="l00038"></a>00038 <span class="comment">//posterior</span> |
| 88 | <a name="l00040"></a><a class="code" href="classbdm_1_1KalmanFull.html#2defb75e58892615c5f95fd844f3a666">00040</a> <span class="comment"></span> vec <a class="code" href="classbdm_1_1KalmanFull.html#2defb75e58892615c5f95fd844f3a666" title="Mean value of the posterior density.">mu</a>; |
| 89 | <a name="l00042"></a><a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c">00042</a> mat <a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c" title="Variance of the posterior density.">P</a>; |
| 90 | <a name="l00043"></a>00043 |
| 91 | <a name="l00044"></a>00044 <span class="keywordtype">bool</span> evalll; |
| 92 | <a name="l00045"></a>00045 <span class="keywordtype">double</span> ll; |
| 93 | <a name="l00046"></a>00046 <span class="keyword">public</span>: |
| 94 | <a name="l00048"></a>00048 <a class="code" href="classbdm_1_1KalmanFull.html#bdcc98c8b18c1cbdebdf218ae838fd11" title="For EKFfull;.">KalmanFull</a> ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 ); |
| 95 | <a name="l00050"></a>00050 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KalmanFull.html#081924bc97f453f674bb982b7951d053" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
| 96 | <a name="l00052"></a>00052 <span class="keyword">friend</span> std::ostream &<a class="code" href="classbdm_1_1KalmanFull.html#86ba216243ed95bb46d80d88775d16af" title="print elements of KF">operator<< </a>( std::ostream &os, <span class="keyword">const</span> <a class="code" href="classbdm_1_1KalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a> &kf ); |
| 97 | <a name="l00054"></a><a class="code" href="classbdm_1_1KalmanFull.html#bdcc98c8b18c1cbdebdf218ae838fd11">00054</a> <a class="code" href="classbdm_1_1KalmanFull.html#bdcc98c8b18c1cbdebdf218ae838fd11" title="For EKFfull;.">KalmanFull</a>() {}; |
| 98 | <a name="l00055"></a>00055 }; |
100 | | <a name="l00064"></a>00064 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
101 | | <a name="l00065"></a>00065 |
102 | | <a name="l00066"></a><a class="code" href="classbdm_1_1Kalman.html">00066</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> |
103 | | <a name="l00067"></a>00067 { |
104 | | <a name="l00068"></a>00068 <span class="keyword">protected</span>: |
105 | | <a name="l00070"></a><a class="code" href="classbdm_1_1Kalman.html#3fe475a1e920b20b63bb342c0e1571f7">00070</a> <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#3fe475a1e920b20b63bb342c0e1571f7" title="Indetifier of output rv.">rvy</a>; |
106 | | <a name="l00072"></a><a class="code" href="classbdm_1_1Kalman.html#149e27424fd1a7cc1c998ea088618a94">00072</a> <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#149e27424fd1a7cc1c998ea088618a94" title="Indetifier of exogeneous rv.">rvu</a>; |
107 | | <a name="l00074"></a><a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa">00074</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>; |
108 | | <a name="l00076"></a><a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f">00076</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>; |
109 | | <a name="l00078"></a><a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b">00078</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a>; |
110 | | <a name="l00080"></a><a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace">00080</a> mat <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>; |
111 | | <a name="l00082"></a><a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c">00082</a> mat <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>; |
112 | | <a name="l00084"></a><a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177">00084</a> mat <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>; |
113 | | <a name="l00086"></a><a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456">00086</a> mat <a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a>; |
114 | | <a name="l00088"></a><a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee">00088</a> sq_T <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>; |
115 | | <a name="l00090"></a><a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7">00090</a> sq_T <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>; |
116 | | <a name="l00091"></a>00091 |
117 | | <a name="l00093"></a><a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d">00093</a> <a class="code" href="classbdm_1_1enorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm<sq_T></a> <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>; |
118 | | <a name="l00095"></a><a class="code" href="classbdm_1_1Kalman.html#ba555c394c429f6831c9bbabfa2c944c">00095</a> <a class="code" href="classbdm_1_1enorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm<sq_T></a> <a class="code" href="classbdm_1_1Kalman.html#ba555c394c429f6831c9bbabfa2c944c" title="preditive density on $y_t$">fy</a>; |
119 | | <a name="l00096"></a>00096 |
120 | | <a name="l00098"></a><a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92">00098</a> mat <a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92" title="placeholder for Kalman gain">_K</a>; |
121 | | <a name="l00100"></a><a class="code" href="classbdm_1_1Kalman.html#c249d45258c8578b13858ad3e7b729b1">00100</a> vec& <a class="code" href="classbdm_1_1Kalman.html#c249d45258c8578b13858ad3e7b729b1" title="cache of fy.mu">_yp</a>; |
122 | | <a name="l00102"></a><a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a">00102</a> sq_T& <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a>; |
123 | | <a name="l00104"></a><a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0">00104</a> vec& <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>; |
124 | | <a name="l00106"></a><a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed">00106</a> sq_T& <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>; |
125 | | <a name="l00107"></a>00107 |
126 | | <a name="l00108"></a>00108 <span class="keyword">public</span>: |
127 | | <a name="l00110"></a>00110 <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman</a> ( ); |
128 | | <a name="l00112"></a>00112 <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman</a> ( <span class="keyword">const</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman<sq_T></a> &K0 ); |
129 | | <a name="l00114"></a>00114 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#3c7fb87fb6b87d08deb6a5a7862da957" 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 &Q0,<span class="keyword">const</span> sq_T &R0 ); |
130 | | <a name="l00116"></a><a class="code" href="classbdm_1_1Kalman.html#9264fc6b173ecb803d2684b883f32c68">00116</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#9264fc6b173ecb803d2684b883f32c68" 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 ) |
131 | | <a name="l00117"></a>00117 { |
132 | | <a name="l00118"></a>00118 sq_T pom ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ); |
133 | | <a name="l00119"></a>00119 <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>.set_parameters ( mu0,P0 ); |
134 | | <a name="l00120"></a>00120 P0.mult_sym ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>,pom ); |
135 | | <a name="l00121"></a>00121 <a class="code" href="classbdm_1_1Kalman.html#ba555c394c429f6831c9bbabfa2c944c" title="preditive density on $y_t$">fy</a>.set_parameters ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>*mu0, pom ); |
136 | | <a name="l00122"></a>00122 }; |
137 | | <a name="l00123"></a>00123 |
138 | | <a name="l00125"></a>00125 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#4a39330c14eff8d13179e868a1d1aa8c" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
139 | | <a name="l00127"></a><a class="code" href="classbdm_1_1Kalman.html#f75e487ff6c129d7012d702030f8c890">00127</a> <span class="keyword">const</span> <a class="code" href="classbdm_1_1epdf.html" title="Probability density function with numerical statistics, e.g. posterior density.">epdf</a>& <a class="code" href="classbdm_1_1Kalman.html#f75e487ff6c129d7012d702030f8c890" title="access function">posterior</a>()<span class="keyword"> const </span>{<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>;} |
140 | | <a name="l00128"></a>00128 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm<sq_T></a>* _e()<span class="keyword"> const </span>{<span class="keywordflow">return</span> &<a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>;} |
141 | | <a name="l00130"></a><a class="code" href="classbdm_1_1Kalman.html#c788ec6e6c6f5f5861ae8a56d8ede277">00130</a> mat& <a class="code" href="classbdm_1_1Kalman.html#c788ec6e6c6f5f5861ae8a56d8ede277" title="access function">__K</a>() {<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92" title="placeholder for Kalman gain">_K</a>;} |
142 | | <a name="l00132"></a><a class="code" href="classbdm_1_1Kalman.html#a250d1dbe7bba861dba2a324520cfa48">00132</a> vec <a class="code" href="classbdm_1_1Kalman.html#a250d1dbe7bba861dba2a324520cfa48" title="access function">_dP</a>() {<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>->getD();} |
143 | | <a name="l00133"></a>00133 }; |
144 | | <a name="l00134"></a>00134 |
145 | | <a name="l00141"></a><a class="code" href="classbdm_1_1KalmanCh.html">00141</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><chmat> |
146 | | <a name="l00142"></a>00142 { |
147 | | <a name="l00143"></a>00143 <span class="keyword">protected</span>: |
148 | | <a name="l00145"></a><a class="code" href="classbdm_1_1KalmanCh.html#48611c8582706cfa62e832be0972e75d">00145</a> mat <a class="code" href="classbdm_1_1KalmanCh.html#48611c8582706cfa62e832be0972e75d" title="pre array (triangular matrix)">preA</a>; |
149 | | <a name="l00147"></a><a class="code" href="classbdm_1_1KalmanCh.html#bcbd68f51d4b57246e7784ca5900171f">00147</a> mat <a class="code" href="classbdm_1_1KalmanCh.html#bcbd68f51d4b57246e7784ca5900171f" title="post array (triangular matrix)">postA</a>; |
150 | | <a name="l00148"></a>00148 |
151 | | <a name="l00149"></a>00149 <span class="keyword">public</span>: |
152 | | <a name="l00151"></a><a class="code" href="classbdm_1_1KalmanCh.html#24ce65bdaa538d4d5153d709a929b996">00151</a> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a>* <a class="code" href="classbdm_1_1KalmanCh.html#24ce65bdaa538d4d5153d709a929b996" title="copy constructor">_copy_</a>()<span class="keyword"> const</span> |
153 | | <a name="l00152"></a>00152 <span class="keyword"> </span>{ |
154 | | <a name="l00153"></a>00153 <a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a>* K=<span class="keyword">new</span> <a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a>; |
155 | | <a name="l00154"></a>00154 K-><a class="code" href="classbdm_1_1KalmanCh.html#20a4d4c664e8ac8a3f1bb7b0d11c6d87" title="Set parameters with check of relevance.">set_parameters</a> ( <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>,<a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>,<a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>,<a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a>,<a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>,<a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> ); |
156 | | <a name="l00155"></a>00155 K-><a class="code" href="classbdm_1_1KalmanCh.html#6e169272657ed101f3d128b49c59b890">set_statistics</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,<a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> ); |
157 | | <a name="l00156"></a>00156 <span class="keywordflow">return</span> K; |
158 | | <a name="l00157"></a>00157 } |
159 | | <a name="l00159"></a>00159 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KalmanCh.html#20a4d4c664e8ac8a3f1bb7b0d11c6d87" 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> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &Q0,<span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &R0 ); |
160 | | <a name="l00160"></a>00160 <span class="keywordtype">void</span> set_statistics ( <span class="keyword">const</span> vec &mu0, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &P0 ) |
161 | | <a name="l00161"></a>00161 { |
162 | | <a name="l00162"></a>00162 <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>.<a class="code" href="classbdm_1_1enorm.html#b8322f2c11560871dd922c660f4771bb">set_parameters</a> ( mu0,P0 ); |
163 | | <a name="l00163"></a>00163 }; |
164 | | <a name="l00164"></a>00164 |
| 100 | <a name="l00057"></a>00057 |
| 101 | <a name="l00065"></a>00065 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 102 | <a name="l00066"></a>00066 |
| 103 | <a name="l00067"></a><a class="code" href="classbdm_1_1Kalman.html">00067</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> |
| 104 | <a name="l00068"></a>00068 { |
| 105 | <a name="l00069"></a>00069 <span class="keyword">protected</span>: |
| 106 | <a name="l00071"></a><a class="code" href="classbdm_1_1Kalman.html#3fe475a1e920b20b63bb342c0e1571f7">00071</a> <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#3fe475a1e920b20b63bb342c0e1571f7" title="Indetifier of output rv.">rvy</a>; |
| 107 | <a name="l00073"></a><a class="code" href="classbdm_1_1Kalman.html#149e27424fd1a7cc1c998ea088618a94">00073</a> <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#149e27424fd1a7cc1c998ea088618a94" title="Indetifier of exogeneous rv.">rvu</a>; |
| 108 | <a name="l00075"></a><a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa">00075</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>; |
| 109 | <a name="l00077"></a><a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f">00077</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>; |
| 110 | <a name="l00079"></a><a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b">00079</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a>; |
| 111 | <a name="l00081"></a><a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace">00081</a> mat <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>; |
| 112 | <a name="l00083"></a><a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c">00083</a> mat <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>; |
| 113 | <a name="l00085"></a><a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177">00085</a> mat <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>; |
| 114 | <a name="l00087"></a><a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456">00087</a> mat <a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a>; |
| 115 | <a name="l00089"></a><a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee">00089</a> sq_T <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>; |
| 116 | <a name="l00091"></a><a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7">00091</a> sq_T <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>; |
| 117 | <a name="l00092"></a>00092 |
| 118 | <a name="l00094"></a><a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d">00094</a> <a class="code" href="classbdm_1_1enorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm<sq_T></a> <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>; |
| 119 | <a name="l00096"></a><a class="code" href="classbdm_1_1Kalman.html#ba555c394c429f6831c9bbabfa2c944c">00096</a> <a class="code" href="classbdm_1_1enorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm<sq_T></a> <a class="code" href="classbdm_1_1Kalman.html#ba555c394c429f6831c9bbabfa2c944c" title="preditive density on $y_t$">fy</a>; |
| 120 | <a name="l00097"></a>00097 |
| 121 | <a name="l00099"></a><a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92">00099</a> mat <a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92" title="placeholder for Kalman gain">_K</a>; |
| 122 | <a name="l00101"></a><a class="code" href="classbdm_1_1Kalman.html#c249d45258c8578b13858ad3e7b729b1">00101</a> vec& <a class="code" href="classbdm_1_1Kalman.html#c249d45258c8578b13858ad3e7b729b1" title="cache of fy.mu">_yp</a>; |
| 123 | <a name="l00103"></a><a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a">00103</a> sq_T& <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a>; |
| 124 | <a name="l00105"></a><a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0">00105</a> vec& <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>; |
| 125 | <a name="l00107"></a><a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed">00107</a> sq_T& <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>; |
| 126 | <a name="l00108"></a>00108 |
| 127 | <a name="l00109"></a>00109 <span class="keyword">public</span>: |
| 128 | <a name="l00111"></a>00111 <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman</a> ( ); |
| 129 | <a name="l00113"></a>00113 <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman</a> ( <span class="keyword">const</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman<sq_T></a> &K0 ); |
| 130 | <a name="l00115"></a>00115 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#3c7fb87fb6b87d08deb6a5a7862da957" 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 &Q0,<span class="keyword">const</span> sq_T &R0 ); |
| 131 | <a name="l00117"></a><a class="code" href="classbdm_1_1Kalman.html#9264fc6b173ecb803d2684b883f32c68">00117</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#9264fc6b173ecb803d2684b883f32c68" 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 ) |
| 132 | <a name="l00118"></a>00118 { |
| 133 | <a name="l00119"></a>00119 sq_T pom ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ); |
| 134 | <a name="l00120"></a>00120 <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>.set_parameters ( mu0,P0 ); |
| 135 | <a name="l00121"></a>00121 P0.mult_sym ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>,pom ); |
| 136 | <a name="l00122"></a>00122 <a class="code" href="classbdm_1_1Kalman.html#ba555c394c429f6831c9bbabfa2c944c" title="preditive density on $y_t$">fy</a>.set_parameters ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>*mu0, pom ); |
| 137 | <a name="l00123"></a>00123 }; |
| 138 | <a name="l00124"></a>00124 |
| 139 | <a name="l00126"></a>00126 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#4a39330c14eff8d13179e868a1d1aa8c" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
| 140 | <a name="l00128"></a><a class="code" href="classbdm_1_1Kalman.html#f75e487ff6c129d7012d702030f8c890">00128</a> <span class="keyword">const</span> <a class="code" href="classbdm_1_1epdf.html" title="Probability density function with numerical statistics, e.g. posterior density.">epdf</a>& <a class="code" href="classbdm_1_1Kalman.html#f75e487ff6c129d7012d702030f8c890" title="access function">posterior</a>()<span class="keyword"> const </span>{<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>;} |
| 141 | <a name="l00129"></a>00129 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html" title="Gaussian density with positive definite (decomposed) covariance matrix.">enorm<sq_T></a>* _e()<span class="keyword"> const </span>{<span class="keywordflow">return</span> &<a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>;} |
| 142 | <a name="l00131"></a><a class="code" href="classbdm_1_1Kalman.html#c788ec6e6c6f5f5861ae8a56d8ede277">00131</a> mat& <a class="code" href="classbdm_1_1Kalman.html#c788ec6e6c6f5f5861ae8a56d8ede277" title="access function">__K</a>() {<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92" title="placeholder for Kalman gain">_K</a>;} |
| 143 | <a name="l00133"></a><a class="code" href="classbdm_1_1Kalman.html#a250d1dbe7bba861dba2a324520cfa48">00133</a> vec <a class="code" href="classbdm_1_1Kalman.html#a250d1dbe7bba861dba2a324520cfa48" title="access function">_dP</a>() {<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>->getD();} |
| 144 | <a name="l00134"></a>00134 }; |
| 145 | <a name="l00135"></a>00135 |
| 146 | <a name="l00142"></a><a class="code" href="classbdm_1_1KalmanCh.html">00142</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><chmat> |
| 147 | <a name="l00143"></a>00143 { |
| 148 | <a name="l00144"></a>00144 <span class="keyword">protected</span>: |
| 149 | <a name="l00146"></a><a class="code" href="classbdm_1_1KalmanCh.html#48611c8582706cfa62e832be0972e75d">00146</a> mat <a class="code" href="classbdm_1_1KalmanCh.html#48611c8582706cfa62e832be0972e75d" title="pre array (triangular matrix)">preA</a>; |
| 150 | <a name="l00148"></a><a class="code" href="classbdm_1_1KalmanCh.html#bcbd68f51d4b57246e7784ca5900171f">00148</a> mat <a class="code" href="classbdm_1_1KalmanCh.html#bcbd68f51d4b57246e7784ca5900171f" title="post array (triangular matrix)">postA</a>; |
| 151 | <a name="l00149"></a>00149 |
| 152 | <a name="l00150"></a>00150 <span class="keyword">public</span>: |
| 153 | <a name="l00152"></a><a class="code" href="classbdm_1_1KalmanCh.html#24ce65bdaa538d4d5153d709a929b996">00152</a> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a>* <a class="code" href="classbdm_1_1KalmanCh.html#24ce65bdaa538d4d5153d709a929b996" title="copy constructor">_copy_</a>()<span class="keyword"> const</span> |
| 154 | <a name="l00153"></a>00153 <span class="keyword"> </span>{ |
| 155 | <a name="l00154"></a>00154 <a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a>* K=<span class="keyword">new</span> <a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a>; |
| 156 | <a name="l00155"></a>00155 K-><a class="code" href="classbdm_1_1KalmanCh.html#20a4d4c664e8ac8a3f1bb7b0d11c6d87" title="Set parameters with check of relevance.">set_parameters</a> ( <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>,<a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>,<a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>,<a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a>,<a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>,<a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> ); |
| 157 | <a name="l00156"></a>00156 K-><a class="code" href="classbdm_1_1KalmanCh.html#6e169272657ed101f3d128b49c59b890">set_statistics</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,<a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> ); |
| 158 | <a name="l00157"></a>00157 <span class="keywordflow">return</span> K; |
| 159 | <a name="l00158"></a>00158 } |
| 160 | <a name="l00160"></a>00160 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KalmanCh.html#20a4d4c664e8ac8a3f1bb7b0d11c6d87" 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> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &Q0,<span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &R0 ); |
| 161 | <a name="l00161"></a>00161 <span class="keywordtype">void</span> set_statistics ( <span class="keyword">const</span> vec &mu0, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &P0 ) |
| 162 | <a name="l00162"></a>00162 { |
| 163 | <a name="l00163"></a>00163 <a class="code" href="classbdm_1_1Kalman.html#383f329ff18bbe219254c8b3b916f40d" title="posterior density on $x_t$">est</a>.<a class="code" href="classbdm_1_1enorm.html#b8322f2c11560871dd922c660f4771bb">set_parameters</a> ( mu0,P0 ); |
| 164 | <a name="l00164"></a>00164 }; |
166 | | <a name="l00179"></a>00179 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KalmanCh.html#b41fe5540548100b08e1684c3be767b6" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
167 | | <a name="l00180"></a>00180 }; |
168 | | <a name="l00181"></a>00181 |
169 | | <a name="l00187"></a><a class="code" href="classbdm_1_1EKFfull.html">00187</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1EKFfull.html" title="Extended Kalman Filter in full matrices.">EKFfull</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1KalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a>, <span class="keyword">public</span> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> |
170 | | <a name="l00188"></a>00188 { |
171 | | <a name="l00189"></a>00189 <span class="keyword">protected</span>: |
172 | | <a name="l00191"></a><a class="code" href="classbdm_1_1EKFfull.html#016d3ec108a430b1e70cf7d78bb963f4">00191</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#016d3ec108a430b1e70cf7d78bb963f4" title="Internal Model f(x,u).">pfxu</a>; |
173 | | <a name="l00193"></a><a class="code" href="classbdm_1_1EKFfull.html#f7cdf9cf74284630b4578a2cb8ba92c7">00193</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#f7cdf9cf74284630b4578a2cb8ba92c7" title="Observation Model h(x,u).">phxu</a>; |
174 | | <a name="l00194"></a>00194 |
175 | | <a name="l00195"></a>00195 <a class="code" href="classbdm_1_1enorm.html">enorm<fsqmat></a> E; |
176 | | <a name="l00196"></a>00196 <span class="keyword">public</span>: |
177 | | <a name="l00198"></a>00198 <a class="code" href="classbdm_1_1EKFfull.html#6939c345389abb8b2481457b4cfe1165" title="Default constructor.">EKFfull</a> ( ); |
178 | | <a name="l00200"></a>00200 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFfull.html#78748da361ba61fef162b0d8956d1743" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#016d3ec108a430b1e70cf7d78bb963f4" title="Internal Model f(x,u).">pfxu</a>, <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#f7cdf9cf74284630b4578a2cb8ba92c7" title="Observation Model h(x,u).">phxu</a>, <span class="keyword">const</span> mat Q0, <span class="keyword">const</span> mat R0 ); |
179 | | <a name="l00202"></a>00202 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFfull.html#f149ae8e9ce14d9931a7bb2850736699" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
180 | | <a name="l00204"></a><a class="code" href="classbdm_1_1EKFfull.html#1949a9b1496a855cc7c24e619bc52365">00204</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFfull.html#1949a9b1496a855cc7c24e619bc52365" title="set estimates">set_statistics</a> ( vec mu0, mat P0 ) {<a class="code" href="classbdm_1_1KalmanFull.html#2defb75e58892615c5f95fd844f3a666" title="Mean value of the posterior density.">mu</a>=mu0;<a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c" title="Variance of the posterior density.">P</a>=P0;}; |
181 | | <a name="l00206"></a><a class="code" href="classbdm_1_1EKFfull.html#7e9a69f36a0a0615c9abb806772ef36d">00206</a> <span class="keyword">const</span> <a class="code" href="classbdm_1_1epdf.html" title="Probability density function with numerical statistics, e.g. posterior density.">epdf</a>& <a class="code" href="classbdm_1_1EKFfull.html#7e9a69f36a0a0615c9abb806772ef36d" title="dummy!">posterior</a>()<span class="keyword"> const</span>{<span class="keywordflow">return</span> E;}; |
182 | | <a name="l00207"></a>00207 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html">enorm<fsqmat></a>* _e()<span class="keyword"> const</span>{<span class="keywordflow">return</span> &E;}; |
183 | | <a name="l00208"></a>00208 <span class="keyword">const</span> mat _R() {<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c" title="Variance of the posterior density.">P</a>;} |
184 | | <a name="l00209"></a>00209 }; |
185 | | <a name="l00210"></a>00210 |
186 | | <a name="l00216"></a>00216 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
187 | | <a name="l00217"></a><a class="code" href="classbdm_1_1EKF.html">00217</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1EKF.html" title="Extended Kalman Filter.">EKF</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><fsqmat> |
188 | | <a name="l00218"></a>00218 { |
189 | | <a name="l00220"></a>00220 <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* pfxu; |
190 | | <a name="l00222"></a>00222 <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu; |
191 | | <a name="l00223"></a>00223 <span class="keyword">public</span>: |
192 | | <a name="l00225"></a>00225 <a class="code" href="classbdm_1_1EKF.html#d087a8bb408d26ac4f5c542746b81059" title="Default constructor.">EKF</a> ( <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> rvx, <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#3fe475a1e920b20b63bb342c0e1571f7" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#149e27424fd1a7cc1c998ea088618a94" title="Indetifier of exogeneous rv.">rvu</a> ); |
193 | | <a name="l00227"></a><a class="code" href="classbdm_1_1EKF.html#fe9b2e227255ad32dc73df316b7318f4">00227</a> <a class="code" href="classbdm_1_1EKF.html" title="Extended Kalman Filter.">EKF<sq_T></a>* <a class="code" href="classbdm_1_1EKF.html#fe9b2e227255ad32dc73df316b7318f4" title="copy constructor">_copy_</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <span class="keyword">new</span> <a class="code" href="classbdm_1_1EKF.html" title="Extended Kalman Filter.">EKF<sq_T></a> ( this ); } |
194 | | <a name="l00229"></a>00229 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKF.html#00fec1a0a6a467eb83fb36c65eba7bcb" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* pfxu, <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu, <span class="keyword">const</span> sq_T Q0, <span class="keyword">const</span> sq_T R0 ); |
195 | | <a name="l00231"></a>00231 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKF.html#3fb182ecc29b10ca1163cecbf3bcccfa" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
196 | | <a name="l00232"></a>00232 }; |
197 | | <a name="l00233"></a>00233 |
198 | | <a name="l00240"></a><a class="code" href="classbdm_1_1EKFCh.html">00240</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1EKFCh.html" title="Extended Kalman Filter in Square root.">EKFCh</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a> |
199 | | <a name="l00241"></a>00241 { |
200 | | <a name="l00242"></a>00242 <span class="keyword">protected</span>: |
201 | | <a name="l00244"></a><a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3">00244</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3" title="Internal Model f(x,u).">pfxu</a>; |
202 | | <a name="l00246"></a><a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317">00246</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317" title="Observation Model h(x,u).">phxu</a>; |
203 | | <a name="l00247"></a>00247 <span class="keyword">public</span>: |
204 | | <a name="l00249"></a><a class="code" href="classbdm_1_1EKFCh.html#1d1d91400e3f177de9fe7962ea17adc4">00249</a> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a>* <a class="code" href="classbdm_1_1EKFCh.html#1d1d91400e3f177de9fe7962ea17adc4" title="copy constructor duplicated - calls different set_parameters">_copy_</a>()<span class="keyword"> const</span> |
205 | | <a name="l00250"></a>00250 <span class="keyword"> </span>{ |
206 | | <a name="l00251"></a>00251 <a class="code" href="classbdm_1_1EKFCh.html" title="Extended Kalman Filter in Square root.">EKFCh</a>* E=<span class="keyword">new</span> <a class="code" href="classbdm_1_1EKFCh.html" title="Extended Kalman Filter in Square root.">EKFCh</a>; |
207 | | <a name="l00252"></a>00252 E-><a class="code" href="classbdm_1_1EKFCh.html#50f9fbffad721f35e5ccb75d0f6b842a" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3" title="Internal Model f(x,u).">pfxu</a>,<a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317" title="Observation Model h(x,u).">phxu</a>,<a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>,<a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> ); |
208 | | <a name="l00253"></a>00253 E-><a class="code" href="classbdm_1_1KalmanCh.html#6e169272657ed101f3d128b49c59b890">set_statistics</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,<a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> ); |
209 | | <a name="l00254"></a>00254 <span class="keywordflow">return</span> E; |
210 | | <a name="l00255"></a>00255 } |
211 | | <a name="l00257"></a>00257 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFCh.html#50f9fbffad721f35e5ccb75d0f6b842a" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3" title="Internal Model f(x,u).">pfxu</a>, <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317" title="Observation Model h(x,u).">phxu</a>, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> Q0, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> R0 ); |
212 | | <a name="l00259"></a>00259 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFCh.html#4c8609c37290b158f88a31dae4047225" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
213 | | <a name="l00260"></a>00260 }; |
| 166 | <a name="l00166"></a>00166 |
| 167 | <a name="l00180"></a>00180 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KalmanCh.html#b41fe5540548100b08e1684c3be767b6" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
| 168 | <a name="l00181"></a>00181 }; |
| 169 | <a name="l00182"></a>00182 |
| 170 | <a name="l00188"></a><a class="code" href="classbdm_1_1EKFfull.html">00188</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1EKFfull.html" title="Extended Kalman Filter in full matrices.">EKFfull</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1KalmanFull.html" title="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</a>, <span class="keyword">public</span> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> |
| 171 | <a name="l00189"></a>00189 { |
| 172 | <a name="l00190"></a>00190 <span class="keyword">protected</span>: |
| 173 | <a name="l00192"></a><a class="code" href="classbdm_1_1EKFfull.html#016d3ec108a430b1e70cf7d78bb963f4">00192</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#016d3ec108a430b1e70cf7d78bb963f4" title="Internal Model f(x,u).">pfxu</a>; |
| 174 | <a name="l00194"></a><a class="code" href="classbdm_1_1EKFfull.html#f7cdf9cf74284630b4578a2cb8ba92c7">00194</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#f7cdf9cf74284630b4578a2cb8ba92c7" title="Observation Model h(x,u).">phxu</a>; |
| 175 | <a name="l00195"></a>00195 |
| 176 | <a name="l00196"></a>00196 <a class="code" href="classbdm_1_1enorm.html">enorm<fsqmat></a> E; |
| 177 | <a name="l00197"></a>00197 <span class="keyword">public</span>: |
| 178 | <a name="l00199"></a>00199 <a class="code" href="classbdm_1_1EKFfull.html#6939c345389abb8b2481457b4cfe1165" title="Default constructor.">EKFfull</a> ( ); |
| 179 | <a name="l00201"></a>00201 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFfull.html#78748da361ba61fef162b0d8956d1743" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#016d3ec108a430b1e70cf7d78bb963f4" title="Internal Model f(x,u).">pfxu</a>, <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFfull.html#f7cdf9cf74284630b4578a2cb8ba92c7" title="Observation Model h(x,u).">phxu</a>, <span class="keyword">const</span> mat Q0, <span class="keyword">const</span> mat R0 ); |
| 180 | <a name="l00203"></a>00203 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFfull.html#f149ae8e9ce14d9931a7bb2850736699" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
| 181 | <a name="l00205"></a><a class="code" href="classbdm_1_1EKFfull.html#1949a9b1496a855cc7c24e619bc52365">00205</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFfull.html#1949a9b1496a855cc7c24e619bc52365" title="set estimates">set_statistics</a> ( vec mu0, mat P0 ) {<a class="code" href="classbdm_1_1KalmanFull.html#2defb75e58892615c5f95fd844f3a666" title="Mean value of the posterior density.">mu</a>=mu0;<a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c" title="Variance of the posterior density.">P</a>=P0;}; |
| 182 | <a name="l00207"></a><a class="code" href="classbdm_1_1EKFfull.html#7e9a69f36a0a0615c9abb806772ef36d">00207</a> <span class="keyword">const</span> <a class="code" href="classbdm_1_1epdf.html" title="Probability density function with numerical statistics, e.g. posterior density.">epdf</a>& <a class="code" href="classbdm_1_1EKFfull.html#7e9a69f36a0a0615c9abb806772ef36d" title="dummy!">posterior</a>()<span class="keyword"> const</span>{<span class="keywordflow">return</span> E;}; |
| 183 | <a name="l00208"></a>00208 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html">enorm<fsqmat></a>* _e()<span class="keyword"> const</span>{<span class="keywordflow">return</span> &E;}; |
| 184 | <a name="l00209"></a>00209 <span class="keyword">const</span> mat _R() {<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1KalmanFull.html#acacd228e100c3e937de575ad2d7cd9c" title="Variance of the posterior density.">P</a>;} |
| 185 | <a name="l00210"></a>00210 }; |
| 186 | <a name="l00211"></a>00211 |
| 187 | <a name="l00217"></a>00217 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 188 | <a name="l00218"></a><a class="code" href="classbdm_1_1EKF.html">00218</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1EKF.html" title="Extended Kalman Filter.">EKF</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><fsqmat> |
| 189 | <a name="l00219"></a>00219 { |
| 190 | <a name="l00221"></a>00221 <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* pfxu; |
| 191 | <a name="l00223"></a>00223 <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu; |
| 192 | <a name="l00224"></a>00224 <span class="keyword">public</span>: |
| 193 | <a name="l00226"></a>00226 <a class="code" href="classbdm_1_1EKF.html#d087a8bb408d26ac4f5c542746b81059" title="Default constructor.">EKF</a> ( <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> rvx, <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#3fe475a1e920b20b63bb342c0e1571f7" title="Indetifier of output rv.">rvy</a>, <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> <a class="code" href="classbdm_1_1Kalman.html#149e27424fd1a7cc1c998ea088618a94" title="Indetifier of exogeneous rv.">rvu</a> ); |
| 194 | <a name="l00228"></a><a class="code" href="classbdm_1_1EKF.html#fe9b2e227255ad32dc73df316b7318f4">00228</a> <a class="code" href="classbdm_1_1EKF.html" title="Extended Kalman Filter.">EKF<sq_T></a>* <a class="code" href="classbdm_1_1EKF.html#fe9b2e227255ad32dc73df316b7318f4" title="copy constructor">_copy_</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <span class="keyword">new</span> <a class="code" href="classbdm_1_1EKF.html" title="Extended Kalman Filter.">EKF<sq_T></a> ( this ); } |
| 195 | <a name="l00230"></a>00230 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKF.html#00fec1a0a6a467eb83fb36c65eba7bcb" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* pfxu, <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu, <span class="keyword">const</span> sq_T Q0, <span class="keyword">const</span> sq_T R0 ); |
| 196 | <a name="l00232"></a>00232 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKF.html#3fb182ecc29b10ca1163cecbf3bcccfa" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
| 197 | <a name="l00233"></a>00233 }; |
| 198 | <a name="l00234"></a>00234 |
| 199 | <a name="l00241"></a><a class="code" href="classbdm_1_1EKFCh.html">00241</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1EKFCh.html" title="Extended Kalman Filter in Square root.">EKFCh</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1KalmanCh.html" title="Kalman filter in square root form.">KalmanCh</a> |
| 200 | <a name="l00242"></a>00242 { |
| 201 | <a name="l00243"></a>00243 <span class="keyword">protected</span>: |
| 202 | <a name="l00245"></a><a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3">00245</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3" title="Internal Model f(x,u).">pfxu</a>; |
| 203 | <a name="l00247"></a><a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317">00247</a> <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317" title="Observation Model h(x,u).">phxu</a>; |
| 204 | <a name="l00248"></a>00248 <span class="keyword">public</span>: |
| 205 | <a name="l00250"></a><a class="code" href="classbdm_1_1EKFCh.html#1d1d91400e3f177de9fe7962ea17adc4">00250</a> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a>* <a class="code" href="classbdm_1_1EKFCh.html#1d1d91400e3f177de9fe7962ea17adc4" title="copy constructor duplicated - calls different set_parameters">_copy_</a>()<span class="keyword"> const</span> |
| 206 | <a name="l00251"></a>00251 <span class="keyword"> </span>{ |
| 207 | <a name="l00252"></a>00252 <a class="code" href="classbdm_1_1EKFCh.html" title="Extended Kalman Filter in Square root.">EKFCh</a>* E=<span class="keyword">new</span> <a class="code" href="classbdm_1_1EKFCh.html" title="Extended Kalman Filter in Square root.">EKFCh</a>; |
| 208 | <a name="l00253"></a>00253 E-><a class="code" href="classbdm_1_1EKFCh.html#50f9fbffad721f35e5ccb75d0f6b842a" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3" title="Internal Model f(x,u).">pfxu</a>,<a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317" title="Observation Model h(x,u).">phxu</a>,<a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>,<a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> ); |
| 209 | <a name="l00254"></a>00254 E-><a class="code" href="classbdm_1_1KalmanCh.html#6e169272657ed101f3d128b49c59b890">set_statistics</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,<a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> ); |
| 210 | <a name="l00255"></a>00255 <span class="keywordflow">return</span> E; |
| 211 | <a name="l00256"></a>00256 } |
| 212 | <a name="l00258"></a>00258 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFCh.html#50f9fbffad721f35e5ccb75d0f6b842a" title="Set nonlinear functions for mean values and covariance matrices.">set_parameters</a> ( <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#e1e895f994398a55bc425551fc275ba3" title="Internal Model f(x,u).">pfxu</a>, <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* <a class="code" href="classbdm_1_1EKFCh.html#6b34c69641826322467b704e8252f317" title="Observation Model h(x,u).">phxu</a>, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> Q0, <span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> R0 ); |
| 213 | <a name="l00260"></a>00260 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFCh.html#4c8609c37290b158f88a31dae4047225" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &dt ); |
215 | | <a name="l00266"></a><a class="code" href="classbdm_1_1KFcondQR.html">00266</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KFcondQR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondQR</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><ldmat> |
216 | | <a name="l00267"></a>00267 { |
217 | | <a name="l00268"></a>00268 <span class="comment">//protected:</span> |
218 | | <a name="l00269"></a>00269 <span class="keyword">public</span>: |
219 | | <a name="l00270"></a><a class="code" href="classbdm_1_1KFcondQR.html#31bc31087ee7ed6c0bfb92d626321b91">00270</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KFcondQR.html#31bc31087ee7ed6c0bfb92d626321b91" title="Substitute val for rvc.">condition</a> ( <span class="keyword">const</span> vec &QR ) |
220 | | <a name="l00271"></a>00271 { |
221 | | <a name="l00272"></a>00272 it_assert_debug ( QR.length() == ( <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>+<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ),<span class="stringliteral">"KFcondRQ: conditioning by incompatible vector"</span> ); |
222 | | <a name="l00273"></a>00273 |
223 | | <a name="l00274"></a>00274 <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>.<a class="code" href="classldmat.html#0884a613b94fde61bfc84288e73ce57f" title="Access functions.">setD</a> ( QR ( 0, <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>-1 ) ); |
224 | | <a name="l00275"></a>00275 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>.<a class="code" href="classldmat.html#0884a613b94fde61bfc84288e73ce57f" title="Access functions.">setD</a> ( QR ( <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, -1 ) ); |
225 | | <a name="l00276"></a>00276 }; |
226 | | <a name="l00277"></a>00277 }; |
227 | | <a name="l00278"></a>00278 |
228 | | <a name="l00283"></a><a class="code" href="classbdm_1_1KFcondR.html">00283</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KFcondR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondR</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><ldmat> |
229 | | <a name="l00284"></a>00284 { |
230 | | <a name="l00285"></a>00285 <span class="comment">//protected:</span> |
231 | | <a name="l00286"></a>00286 <span class="keyword">public</span>: |
232 | | <a name="l00288"></a><a class="code" href="classbdm_1_1KFcondR.html#f11639d79f10b1e7dad16a0d8233450d">00288</a> <a class="code" href="classbdm_1_1KFcondR.html#f11639d79f10b1e7dad16a0d8233450d" title="Default constructor.">KFcondR</a> ( ) : <a class="code" href="classbdm_1_1Kalman.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, (commonly known as UD).">ldmat</a>> ( ) {}; |
233 | | <a name="l00289"></a>00289 |
234 | | <a name="l00290"></a><a class="code" href="classbdm_1_1KFcondR.html#7d42a421acbdcf9b610a5682ee5fb9a8">00290</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KFcondR.html#7d42a421acbdcf9b610a5682ee5fb9a8" title="Substitute val for rvc.">condition</a> ( <span class="keyword">const</span> vec &R0 ) |
235 | | <a name="l00291"></a>00291 { |
236 | | <a name="l00292"></a>00292 it_assert_debug ( R0.length() == ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ),<span class="stringliteral">"KFcondR: conditioning by incompatible vector"</span> ); |
237 | | <a name="l00293"></a>00293 |
238 | | <a name="l00294"></a>00294 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>.<a class="code" href="classldmat.html#0884a613b94fde61bfc84288e73ce57f" title="Access functions.">setD</a> ( R0 ); |
239 | | <a name="l00295"></a>00295 }; |
240 | | <a name="l00296"></a>00296 |
241 | | <a name="l00297"></a>00297 }; |
| 215 | <a name="l00262"></a>00262 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKFCh.html#0c2d3a9d57f23b998c53d7c12fa2a724" title="This method arrange instance properties according the data stored in the Setting...">from_setting</a>( <span class="keyword">const</span> Setting &root ); |
| 216 | <a name="l00263"></a>00263 |
| 217 | <a name="l00264"></a>00264 <span class="comment">// TODO dodelat void to_setting( Setting &root ) const;</span> |
| 218 | <a name="l00265"></a>00265 |
| 219 | <a name="l00266"></a>00266 }; |
| 220 | <a name="l00267"></a>00267 |
| 221 | <a name="l00268"></a>00268 UIREGISTER(EKFCh); |
| 222 | <a name="l00269"></a>00269 |
| 223 | <a name="l00270"></a>00270 |
| 224 | <a name="l00275"></a><a class="code" href="classbdm_1_1KFcondQR.html">00275</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KFcondQR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondQR</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><ldmat> |
| 225 | <a name="l00276"></a>00276 { |
| 226 | <a name="l00277"></a>00277 <span class="comment">//protected:</span> |
| 227 | <a name="l00278"></a>00278 <span class="keyword">public</span>: |
| 228 | <a name="l00279"></a><a class="code" href="classbdm_1_1KFcondQR.html#31bc31087ee7ed6c0bfb92d626321b91">00279</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KFcondQR.html#31bc31087ee7ed6c0bfb92d626321b91" title="Substitute val for rvc.">condition</a> ( <span class="keyword">const</span> vec &QR ) |
| 229 | <a name="l00280"></a>00280 { |
| 230 | <a name="l00281"></a>00281 it_assert_debug ( QR.length() == ( <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>+<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ),<span class="stringliteral">"KFcondRQ: conditioning by incompatible vector"</span> ); |
| 231 | <a name="l00282"></a>00282 |
| 232 | <a name="l00283"></a>00283 <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>.<a class="code" href="classldmat.html#0884a613b94fde61bfc84288e73ce57f" title="Access functions.">setD</a> ( QR ( 0, <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>-1 ) ); |
| 233 | <a name="l00284"></a>00284 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>.<a class="code" href="classldmat.html#0884a613b94fde61bfc84288e73ce57f" title="Access functions.">setD</a> ( QR ( <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, -1 ) ); |
| 234 | <a name="l00285"></a>00285 }; |
| 235 | <a name="l00286"></a>00286 }; |
| 236 | <a name="l00287"></a>00287 |
| 237 | <a name="l00292"></a><a class="code" href="classbdm_1_1KFcondR.html">00292</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1KFcondR.html" title="Kalman Filter with conditional diagonal matrices R and Q.">KFcondR</a> : <span class="keyword">public</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><ldmat> |
| 238 | <a name="l00293"></a>00293 { |
| 239 | <a name="l00294"></a>00294 <span class="comment">//protected:</span> |
| 240 | <a name="l00295"></a>00295 <span class="keyword">public</span>: |
| 241 | <a name="l00297"></a><a class="code" href="classbdm_1_1KFcondR.html#f11639d79f10b1e7dad16a0d8233450d">00297</a> <a class="code" href="classbdm_1_1KFcondR.html#f11639d79f10b1e7dad16a0d8233450d" title="Default constructor.">KFcondR</a> ( ) : <a class="code" href="classbdm_1_1Kalman.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, (commonly known as UD).">ldmat</a>> ( ) {}; |
243 | | <a name="l00300"></a>00300 |
244 | | <a name="l00301"></a>00301 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
245 | | <a name="l00302"></a><a class="code" href="classbdm_1_1Kalman.html#8b22c45cffa949d70b8e5ac92ed5ce25">00302</a> <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman<sq_T>::Kalman</a> ( <span class="keyword">const</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman<sq_T></a> &K0 ) : <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> ( ),rvy ( K0.rvy ),rvu ( K0.rvu ), |
246 | | <a name="l00303"></a>00303 dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ), |
247 | | <a name="l00304"></a>00304 A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ), |
248 | | <a name="l00305"></a>00305 Q ( K0.Q ), R ( K0.R ), |
249 | | <a name="l00306"></a>00306 est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) |
250 | | <a name="l00307"></a>00307 { |
251 | | <a name="l00308"></a>00308 |
252 | | <a name="l00309"></a>00309 <span class="comment">// copy values in pointers</span> |
253 | | <a name="l00310"></a>00310 <span class="comment">// _mu = K0._mu;</span> |
254 | | <a name="l00311"></a>00311 <span class="comment">// _P = K0._P;</span> |
255 | | <a name="l00312"></a>00312 <span class="comment">// _yp = K0._yp;</span> |
256 | | <a name="l00313"></a>00313 <span class="comment">// _Ry = K0._Ry;</span> |
257 | | <a name="l00314"></a>00314 |
258 | | <a name="l00315"></a>00315 } |
259 | | <a name="l00316"></a>00316 |
260 | | <a name="l00317"></a>00317 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
261 | | <a name="l00318"></a><a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4">00318</a> <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman<sq_T>::Kalman</a> ( ) : <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> (), est ( ), fy (), _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) |
262 | | <a name="l00319"></a>00319 { |
263 | | <a name="l00320"></a>00320 }; |
264 | | <a name="l00321"></a>00321 |
265 | | <a name="l00322"></a>00322 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
266 | | <a name="l00323"></a><a class="code" href="classbdm_1_1Kalman.html#3c7fb87fb6b87d08deb6a5a7862da957">00323</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#3c7fb87fb6b87d08deb6a5a7862da957" 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 &Q0, <span class="keyword">const</span> sq_T &R0 ) |
267 | | <a name="l00324"></a>00324 { |
268 | | <a name="l00325"></a>00325 <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a> = A0.rows(); |
269 | | <a name="l00326"></a>00326 <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> = C0.rows(); |
270 | | <a name="l00327"></a>00327 <a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> = B0.cols(); |
271 | | <a name="l00328"></a>00328 |
272 | | <a name="l00329"></a>00329 it_assert_debug ( A0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: A is not square"</span> ); |
273 | | <a name="l00330"></a>00330 it_assert_debug ( B0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: B is not compatible"</span> ); |
274 | | <a name="l00331"></a>00331 it_assert_debug ( C0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: C is not square"</span> ); |
275 | | <a name="l00332"></a>00332 it_assert_debug ( ( D0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ) || ( D0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> ), <span class="stringliteral">"Kalman: D is not compatible"</span> ); |
276 | | <a name="l00333"></a>00333 it_assert_debug ( ( R0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ) || ( R0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); |
277 | | <a name="l00334"></a>00334 it_assert_debug ( ( Q0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a> ) || ( Q0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a> ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); |
278 | | <a name="l00335"></a>00335 |
279 | | <a name="l00336"></a>00336 <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a> = A0; |
280 | | <a name="l00337"></a>00337 <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a> = B0; |
281 | | <a name="l00338"></a>00338 <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a> = C0; |
282 | | <a name="l00339"></a>00339 <a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a> = D0; |
283 | | <a name="l00340"></a>00340 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> = R0; |
284 | | <a name="l00341"></a>00341 <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a> = Q0; |
285 | | <a name="l00342"></a>00342 } |
286 | | <a name="l00343"></a>00343 |
287 | | <a name="l00344"></a>00344 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
288 | | <a name="l00345"></a><a class="code" href="classbdm_1_1Kalman.html#4a39330c14eff8d13179e868a1d1aa8c">00345</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#4a39330c14eff8d13179e868a1d1aa8c" title="Here dt = [yt;ut] of appropriate dimensions.">Kalman<sq_T>::bayes</a> ( <span class="keyword">const</span> vec &dt ) |
289 | | <a name="l00346"></a>00346 { |
290 | | <a name="l00347"></a>00347 it_assert_debug ( dt.length() == ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>+<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); |
291 | | <a name="l00348"></a>00348 |
292 | | <a name="l00349"></a>00349 sq_T iRy ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ); |
293 | | <a name="l00350"></a>00350 vec u = dt.get ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>,<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>+<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a>-1 ); |
294 | | <a name="l00351"></a>00351 vec y = dt.get ( 0,<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>-1 ); |
295 | | <a name="l00352"></a>00352 <span class="comment">//Time update</span> |
296 | | <a name="l00353"></a>00353 <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a> = <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>* <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a> + <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>*u; |
297 | | <a name="l00354"></a>00354 <span class="comment">//P = A*P*A.transpose() + Q; in sq_T</span> |
298 | | <a name="l00355"></a>00355 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a> ); |
299 | | <a name="l00356"></a>00356 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> +=<a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>; |
| 243 | <a name="l00299"></a><a class="code" href="classbdm_1_1KFcondR.html#7d42a421acbdcf9b610a5682ee5fb9a8">00299</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1KFcondR.html#7d42a421acbdcf9b610a5682ee5fb9a8" title="Substitute val for rvc.">condition</a> ( <span class="keyword">const</span> vec &R0 ) |
| 244 | <a name="l00300"></a>00300 { |
| 245 | <a name="l00301"></a>00301 it_assert_debug ( R0.length() == ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ),<span class="stringliteral">"KFcondR: conditioning by incompatible vector"</span> ); |
| 246 | <a name="l00302"></a>00302 |
| 247 | <a name="l00303"></a>00303 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>.<a class="code" href="classldmat.html#0884a613b94fde61bfc84288e73ce57f" title="Access functions.">setD</a> ( R0 ); |
| 248 | <a name="l00304"></a>00304 }; |
| 249 | <a name="l00305"></a>00305 |
| 250 | <a name="l00306"></a>00306 }; |
| 251 | <a name="l00307"></a>00307 |
| 252 | <a name="l00309"></a>00309 |
| 253 | <a name="l00310"></a>00310 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 254 | <a name="l00311"></a><a class="code" href="classbdm_1_1Kalman.html#8b22c45cffa949d70b8e5ac92ed5ce25">00311</a> <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman<sq_T>::Kalman</a> ( <span class="keyword">const</span> <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman<sq_T></a> &K0 ) : <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> ( ),rvy ( K0.rvy ),rvu ( K0.rvu ), |
| 255 | <a name="l00312"></a>00312 dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ), |
| 256 | <a name="l00313"></a>00313 A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ), |
| 257 | <a name="l00314"></a>00314 Q ( K0.Q ), R ( K0.R ), |
| 258 | <a name="l00315"></a>00315 est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) |
| 259 | <a name="l00316"></a>00316 { |
| 260 | <a name="l00317"></a>00317 |
| 261 | <a name="l00318"></a>00318 <span class="comment">// copy values in pointers</span> |
| 262 | <a name="l00319"></a>00319 <span class="comment">// _mu = K0._mu;</span> |
| 263 | <a name="l00320"></a>00320 <span class="comment">// _P = K0._P;</span> |
| 264 | <a name="l00321"></a>00321 <span class="comment">// _yp = K0._yp;</span> |
| 265 | <a name="l00322"></a>00322 <span class="comment">// _Ry = K0._Ry;</span> |
| 266 | <a name="l00323"></a>00323 |
| 267 | <a name="l00324"></a>00324 } |
| 268 | <a name="l00325"></a>00325 |
| 269 | <a name="l00326"></a>00326 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 270 | <a name="l00327"></a><a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4">00327</a> <a class="code" href="classbdm_1_1Kalman.html#025a0196cbcc2e6adb13311f9d3d52b4" title="Default constructor.">Kalman<sq_T>::Kalman</a> ( ) : <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> (), est ( ), fy (), _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) |
| 271 | <a name="l00328"></a>00328 { |
| 272 | <a name="l00329"></a>00329 }; |
| 273 | <a name="l00330"></a>00330 |
| 274 | <a name="l00331"></a>00331 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 275 | <a name="l00332"></a><a class="code" href="classbdm_1_1Kalman.html#3c7fb87fb6b87d08deb6a5a7862da957">00332</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#3c7fb87fb6b87d08deb6a5a7862da957" 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 &Q0, <span class="keyword">const</span> sq_T &R0 ) |
| 276 | <a name="l00333"></a>00333 { |
| 277 | <a name="l00334"></a>00334 <a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a> = A0.rows(); |
| 278 | <a name="l00335"></a>00335 <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> = C0.rows(); |
| 279 | <a name="l00336"></a>00336 <a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> = B0.cols(); |
| 280 | <a name="l00337"></a>00337 |
| 281 | <a name="l00338"></a>00338 it_assert_debug ( A0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: A is not square"</span> ); |
| 282 | <a name="l00339"></a>00339 it_assert_debug ( B0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: B is not compatible"</span> ); |
| 283 | <a name="l00340"></a>00340 it_assert_debug ( C0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"Kalman: C is not square"</span> ); |
| 284 | <a name="l00341"></a>00341 it_assert_debug ( ( D0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ) || ( D0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> ), <span class="stringliteral">"Kalman: D is not compatible"</span> ); |
| 285 | <a name="l00342"></a>00342 it_assert_debug ( ( R0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ) || ( R0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ), <span class="stringliteral">"Kalman: R is not compatible"</span> ); |
| 286 | <a name="l00343"></a>00343 it_assert_debug ( ( Q0.cols() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a> ) || ( Q0.rows() ==<a class="code" href="classbdm_1_1Kalman.html#ba7699cdb3b1382a54d3e28b9b7517fa" title="cache of rv.count()">dimx</a> ), <span class="stringliteral">"Kalman: Q is not compatible"</span> ); |
| 287 | <a name="l00344"></a>00344 |
| 288 | <a name="l00345"></a>00345 <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a> = A0; |
| 289 | <a name="l00346"></a>00346 <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a> = B0; |
| 290 | <a name="l00347"></a>00347 <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a> = C0; |
| 291 | <a name="l00348"></a>00348 <a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a> = D0; |
| 292 | <a name="l00349"></a>00349 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> = R0; |
| 293 | <a name="l00350"></a>00350 <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a> = Q0; |
| 294 | <a name="l00351"></a>00351 } |
| 295 | <a name="l00352"></a>00352 |
| 296 | <a name="l00353"></a>00353 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 297 | <a name="l00354"></a><a class="code" href="classbdm_1_1Kalman.html#4a39330c14eff8d13179e868a1d1aa8c">00354</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1Kalman.html#4a39330c14eff8d13179e868a1d1aa8c" title="Here dt = [yt;ut] of appropriate dimensions.">Kalman<sq_T>::bayes</a> ( <span class="keyword">const</span> vec &dt ) |
| 298 | <a name="l00355"></a>00355 { |
| 299 | <a name="l00356"></a>00356 it_assert_debug ( dt.length() == ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>+<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); |
301 | | <a name="l00358"></a>00358 <span class="comment">//Data update</span> |
302 | | <a name="l00359"></a>00359 <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> |
303 | | <a name="l00360"></a>00360 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>, <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a> ); |
304 | | <a name="l00361"></a>00361 <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a> +=<a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>; |
305 | | <a name="l00362"></a>00362 |
306 | | <a name="l00363"></a>00363 mat Pfull = <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.to_mat(); |
307 | | <a name="l00364"></a>00364 |
308 | | <a name="l00365"></a>00365 <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a>.inv ( iRy ); <span class="comment">// result is in _iRy;</span> |
309 | | <a name="l00366"></a>00366 <a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92" title="placeholder for Kalman gain">_K</a> = Pfull*<a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>.transpose() * ( iRy.to_mat() ); |
310 | | <a name="l00367"></a>00367 |
311 | | <a name="l00368"></a>00368 sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() ); |
312 | | <a name="l00369"></a>00369 iRy.mult_sym_t ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>*Pfull,pom ); |
313 | | <a name="l00370"></a>00370 ( <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> ) -= pom; <span class="comment">// P = P -PC'iRy*CP;</span> |
314 | | <a name="l00371"></a>00371 ( <a class="code" href="classbdm_1_1Kalman.html#c249d45258c8578b13858ad3e7b729b1" title="cache of fy.mu">_yp</a> ) = <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>* <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a> +<a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a>*u; <span class="comment">//y prediction</span> |
315 | | <a name="l00372"></a>00372 ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a> ) += <a class="code" href="classbdm_1_1Kalman.html#bd69dfb802465f22dd84d73a180d5c92" title="placeholder for Kalman gain">_K</a>* ( y- <a class="code" href="classbdm_1_1Kalman.html#c249d45258c8578b13858ad3e7b729b1" title="cache of fy.mu">_yp</a> ); |
| 301 | <a name="l00358"></a>00358 sq_T iRy ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ); |
| 302 | <a name="l00359"></a>00359 vec u = dt.get ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>,<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>+<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a>-1 ); |
| 303 | <a name="l00360"></a>00360 vec y = dt.get ( 0,<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>-1 ); |
| 304 | <a name="l00361"></a>00361 <span class="comment">//Time update</span> |
| 305 | <a name="l00362"></a>00362 <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a> = <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>* <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a> + <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>*u; |
| 306 | <a name="l00363"></a>00363 <span class="comment">//P = A*P*A.transpose() + Q; in sq_T</span> |
| 307 | <a name="l00364"></a>00364 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a> ); |
| 308 | <a name="l00365"></a>00365 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> +=<a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>; |
| 309 | <a name="l00366"></a>00366 |
| 310 | <a name="l00367"></a>00367 <span class="comment">//Data update</span> |
| 311 | <a name="l00368"></a>00368 <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> |
| 312 | <a name="l00369"></a>00369 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.mult_sym ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>, <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a> ); |
| 313 | <a name="l00370"></a>00370 <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a> +=<a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>; |
| 314 | <a name="l00371"></a>00371 |
| 315 | <a name="l00372"></a>00372 mat Pfull = <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.to_mat(); |
327 | | <a name="l00391"></a><a class="code" href="classbdm_1_1MultiModel.html">00391</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1MultiModel.html" title="(Switching) Multiple Model The model runs several models in parallel and evaluates...">MultiModel</a>: <span class="keyword">public</span> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> |
328 | | <a name="l00392"></a>00392 { |
329 | | <a name="l00393"></a>00393 <span class="keyword">protected</span>: |
330 | | <a name="l00395"></a><a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93">00395</a> Array<EKFCh*> <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>; |
331 | | <a name="l00397"></a><a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a">00397</a> vec <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>; |
332 | | <a name="l00399"></a><a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634">00399</a> vec <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a>; |
333 | | <a name="l00401"></a><a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c">00401</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c" title="type of switching policy [1=maximum,2=...]">policy</a>; |
334 | | <a name="l00403"></a><a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b">00403</a> <a class="code" href="classbdm_1_1enorm.html">enorm<chmat></a> <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>; |
335 | | <a name="l00404"></a>00404 <span class="keyword">public</span>: |
336 | | <a name="l00405"></a>00405 <span class="keywordtype">void</span> set_parameters ( Array<EKFCh*> A, <span class="keywordtype">int</span> pol0=1 ) |
337 | | <a name="l00406"></a>00406 { |
338 | | <a name="l00407"></a>00407 <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>=A;<span class="comment">//TODO: test if evalll is set</span> |
339 | | <a name="l00408"></a>00408 <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>.set_length ( A.length() ); |
340 | | <a name="l00409"></a>00409 <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a>.set_length ( A.length() ); |
341 | | <a name="l00410"></a>00410 <a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c" title="type of switching policy [1=maximum,2=...]">policy</a>=pol0; |
342 | | <a name="l00411"></a>00411 |
343 | | <a name="l00412"></a>00412 <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1epdf.html#f423e28448dbb69ef4905295ec8de8ff" title="Name its rv.">set_rv</a>(<a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a>(<span class="stringliteral">"MM"</span>,A(0)->posterior().dimension(),0)); |
344 | | <a name="l00413"></a>00413 <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#b8322f2c11560871dd922c660f4771bb">set_parameters</a>(A(0)->_e()->mean(), A(0)->_e()->_R()); |
345 | | <a name="l00414"></a>00414 } |
346 | | <a name="l00415"></a><a class="code" href="classbdm_1_1MultiModel.html#a915deeddb0e94c337d02ebc0abe535e">00415</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1MultiModel.html#a915deeddb0e94c337d02ebc0abe535e" title="Incremental Bayes rule.">bayes</a> ( <span class="keyword">const</span> vec &dt ) |
347 | | <a name="l00416"></a>00416 { |
348 | | <a name="l00417"></a>00417 <span class="keywordtype">int</span> n = <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>.length(); |
349 | | <a name="l00418"></a>00418 <span class="keywordtype">int</span> i; |
350 | | <a name="l00419"></a>00419 <span class="keywordflow">for</span> ( i=0;i<n;i++ ) |
351 | | <a name="l00420"></a>00420 { |
352 | | <a name="l00421"></a>00421 <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a> ( i )->bayes ( dt ); |
353 | | <a name="l00422"></a>00422 <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a> ( i ) = <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a> ( i )->_ll(); |
354 | | <a name="l00423"></a>00423 } |
355 | | <a name="l00424"></a>00424 <span class="keywordtype">double</span> mlls=max ( <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a> ); |
356 | | <a name="l00425"></a>00425 <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>=exp ( <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a>-mlls ); |
357 | | <a name="l00426"></a>00426 <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>/=sum ( <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a> ); <span class="comment">//normalization</span> |
358 | | <a name="l00427"></a>00427 <span class="comment">//set statistics</span> |
359 | | <a name="l00428"></a>00428 <span class="keywordflow">switch</span> ( <a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c" title="type of switching policy [1=maximum,2=...]">policy</a> ) |
| 327 | <a name="l00384"></a>00384 <span class="keywordflow">if</span> ( <a class="code" href="classbdm_1_1BM.html#faff0ad12556fe7dc0e2807d4fd938ee" 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> |
| 328 | <a name="l00385"></a>00385 { |
| 329 | <a name="l00386"></a>00386 <a class="code" href="classbdm_1_1BM.html#4064b6559d962633e4372b12f4cd204a" title="Logarithm of marginalized data likelihood.">ll</a>=<a class="code" href="classbdm_1_1Kalman.html#ba555c394c429f6831c9bbabfa2c944c" title="preditive density on $y_t$">fy</a>.evallog ( y ); |
| 330 | <a name="l00387"></a>00387 } |
| 331 | <a name="l00388"></a>00388 |
| 332 | <a name="l00389"></a>00389 <span class="comment">//cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;</span> |
| 333 | <a name="l00390"></a>00390 |
| 334 | <a name="l00391"></a>00391 }; |
| 335 | <a name="l00392"></a>00392 |
| 336 | <a name="l00400"></a><a class="code" href="classbdm_1_1MultiModel.html">00400</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1MultiModel.html" title="(Switching) Multiple Model The model runs several models in parallel and evaluates...">MultiModel</a>: <span class="keyword">public</span> <a class="code" href="classbdm_1_1BM.html" title="Bayesian Model of a system, i.e. all uncertainty is modeled by probabilities.">BM</a> |
| 337 | <a name="l00401"></a>00401 { |
| 338 | <a name="l00402"></a>00402 <span class="keyword">protected</span>: |
| 339 | <a name="l00404"></a><a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93">00404</a> Array<EKFCh*> <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>; |
| 340 | <a name="l00406"></a><a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a">00406</a> vec <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>; |
| 341 | <a name="l00408"></a><a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634">00408</a> vec <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a>; |
| 342 | <a name="l00410"></a><a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c">00410</a> <span class="keywordtype">int</span> <a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c" title="type of switching policy [1=maximum,2=...]">policy</a>; |
| 343 | <a name="l00412"></a><a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b">00412</a> <a class="code" href="classbdm_1_1enorm.html">enorm<chmat></a> <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>; |
| 344 | <a name="l00413"></a>00413 <span class="keyword">public</span>: |
| 345 | <a name="l00414"></a>00414 <span class="keywordtype">void</span> set_parameters ( Array<EKFCh*> A, <span class="keywordtype">int</span> pol0=1 ) |
| 346 | <a name="l00415"></a>00415 { |
| 347 | <a name="l00416"></a>00416 <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>=A;<span class="comment">//TODO: test if evalll is set</span> |
| 348 | <a name="l00417"></a>00417 <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>.set_length ( A.length() ); |
| 349 | <a name="l00418"></a>00418 <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a>.set_length ( A.length() ); |
| 350 | <a name="l00419"></a>00419 <a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c" title="type of switching policy [1=maximum,2=...]">policy</a>=pol0; |
| 351 | <a name="l00420"></a>00420 |
| 352 | <a name="l00421"></a>00421 <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1epdf.html#f423e28448dbb69ef4905295ec8de8ff" title="Name its rv.">set_rv</a>(<a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a>(<span class="stringliteral">"MM"</span>,A(0)->posterior().dimension(),0)); |
| 353 | <a name="l00422"></a>00422 <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#b8322f2c11560871dd922c660f4771bb">set_parameters</a>(A(0)->_e()->mean(), A(0)->_e()->_R()); |
| 354 | <a name="l00423"></a>00423 } |
| 355 | <a name="l00424"></a><a class="code" href="classbdm_1_1MultiModel.html#a915deeddb0e94c337d02ebc0abe535e">00424</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1MultiModel.html#a915deeddb0e94c337d02ebc0abe535e" title="Incremental Bayes rule.">bayes</a> ( <span class="keyword">const</span> vec &dt ) |
| 356 | <a name="l00425"></a>00425 { |
| 357 | <a name="l00426"></a>00426 <span class="keywordtype">int</span> n = <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>.length(); |
| 358 | <a name="l00427"></a>00427 <span class="keywordtype">int</span> i; |
| 359 | <a name="l00428"></a>00428 <span class="keywordflow">for</span> ( i=0;i<n;i++ ) |
361 | | <a name="l00430"></a>00430 <span class="keywordflow">case</span> 1: |
362 | | <a name="l00431"></a>00431 { |
363 | | <a name="l00432"></a>00432 <span class="keywordtype">int</span> mi=max_index ( <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a> ); |
364 | | <a name="l00433"></a>00433 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html">enorm<chmat></a>* st=(<a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>(mi)->_e()); |
365 | | <a name="l00434"></a>00434 <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#b8322f2c11560871dd922c660f4771bb">set_parameters</a>(st-><a class="code" href="classbdm_1_1enorm.html#b2fa2915c35366392fe9bb022ca1a600" title="return expected value">mean</a>(), st-><a class="code" href="classbdm_1_1enorm.html#81d81e35e57c9f194bde248e3affcf1f">_R</a>()); |
366 | | <a name="l00435"></a>00435 } |
367 | | <a name="l00436"></a>00436 <span class="keywordflow">break</span>; |
368 | | <a name="l00437"></a>00437 <span class="keywordflow">default</span>: it_error ( <span class="stringliteral">"unknown policy"</span> ); |
369 | | <a name="l00438"></a>00438 } |
370 | | <a name="l00439"></a>00439 <span class="comment">// copy result to all models</span> |
371 | | <a name="l00440"></a>00440 <span class="keywordflow">for</span> ( i=0;i<n;i++ ) |
372 | | <a name="l00441"></a>00441 { |
373 | | <a name="l00442"></a>00442 <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a> ( i )->set_statistics ( <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#b2fa2915c35366392fe9bb022ca1a600" title="return expected value">mean</a>(),<a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#81d81e35e57c9f194bde248e3affcf1f">_R</a>()); |
374 | | <a name="l00443"></a>00443 } |
375 | | <a name="l00444"></a>00444 } |
376 | | <a name="l00445"></a>00445 <span class="comment">//all posterior densities are equal => return the first one</span> |
377 | | <a name="l00446"></a>00446 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html">enorm<chmat></a>* _e()<span class="keyword"> const </span>{<span class="keywordflow">return</span> &<a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>;} |
378 | | <a name="l00447"></a>00447 <span class="comment">//all posterior densities are equal => return the first one</span> |
379 | | <a name="l00448"></a>00448 <span class="keyword">const</span> enorm<chmat>& posterior()<span class="keyword"> const </span>{<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>;} |
380 | | <a name="l00449"></a>00449 }; |
381 | | <a name="l00450"></a>00450 |
382 | | <a name="l00451"></a>00451 |
383 | | <a name="l00452"></a>00452 <span class="comment">//TODO why not const pointer??</span> |
384 | | <a name="l00453"></a>00453 |
385 | | <a name="l00454"></a>00454 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
386 | | <a name="l00455"></a><a class="code" href="classbdm_1_1EKF.html#d087a8bb408d26ac4f5c542746b81059">00455</a> <a class="code" href="classbdm_1_1EKF.html#d087a8bb408d26ac4f5c542746b81059" title="Default constructor.">EKF<sq_T>::EKF</a> ( <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> rvx0, <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> rvy0, <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> rvu0 ) : <a class="code" href="classbdm_1_1Kalman.html" title="Kalman filter with covariance matrices in square root form.">Kalman</a><sq_T> ( rvx0,rvy0,rvu0 ) {} |
387 | | <a name="l00456"></a>00456 |
388 | | <a name="l00457"></a>00457 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
389 | | <a name="l00458"></a><a class="code" href="classbdm_1_1EKF.html#00fec1a0a6a467eb83fb36c65eba7bcb">00458</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKF.html#00fec1a0a6a467eb83fb36c65eba7bcb" title="Set nonlinear functions for mean values and covariance matrices.">EKF<sq_T>::set_parameters</a> ( <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* pfxu0, <a class="code" href="classbdm_1_1diffbifn.html" title="Class representing a differentiable function of two variables .">diffbifn</a>* phxu0,<span class="keyword">const</span> sq_T Q0,<span class="keyword">const</span> sq_T R0 ) |
390 | | <a name="l00459"></a>00459 { |
391 | | <a name="l00460"></a>00460 pfxu = pfxu0; |
392 | | <a name="l00461"></a>00461 phxu = phxu0; |
| 361 | <a name="l00430"></a>00430 <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a> ( i )->bayes ( dt ); |
| 362 | <a name="l00431"></a>00431 <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a> ( i ) = <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a> ( i )->_ll(); |
| 363 | <a name="l00432"></a>00432 } |
| 364 | <a name="l00433"></a>00433 <span class="keywordtype">double</span> mlls=max ( <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a> ); |
| 365 | <a name="l00434"></a>00434 <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>=exp ( <a class="code" href="classbdm_1_1MultiModel.html#7b4012fc2208ce4ddd5c0d1fe69d7634" title="cache of model lls">_lls</a>-mlls ); |
| 366 | <a name="l00435"></a>00435 <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a>/=sum ( <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a> ); <span class="comment">//normalization</span> |
| 367 | <a name="l00436"></a>00436 <span class="comment">//set statistics</span> |
| 368 | <a name="l00437"></a>00437 <span class="keywordflow">switch</span> ( <a class="code" href="classbdm_1_1MultiModel.html#9b56bcde4664bd53f8995d7ee7ed415c" title="type of switching policy [1=maximum,2=...]">policy</a> ) |
| 369 | <a name="l00438"></a>00438 { |
| 370 | <a name="l00439"></a>00439 <span class="keywordflow">case</span> 1: |
| 371 | <a name="l00440"></a>00440 { |
| 372 | <a name="l00441"></a>00441 <span class="keywordtype">int</span> mi=max_index ( <a class="code" href="classbdm_1_1MultiModel.html#ef85ea61575bffa8beac8040869ee47a" title="vector of model weights">w</a> ); |
| 373 | <a name="l00442"></a>00442 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html">enorm<chmat></a>* st=(<a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>(mi)->_e()); |
| 374 | <a name="l00443"></a>00443 <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#b8322f2c11560871dd922c660f4771bb">set_parameters</a>(st-><a class="code" href="classbdm_1_1enorm.html#b2fa2915c35366392fe9bb022ca1a600" title="return expected value">mean</a>(), st-><a class="code" href="classbdm_1_1enorm.html#81d81e35e57c9f194bde248e3affcf1f">_R</a>()); |
| 375 | <a name="l00444"></a>00444 } |
| 376 | <a name="l00445"></a>00445 <span class="keywordflow">break</span>; |
| 377 | <a name="l00446"></a>00446 <span class="keywordflow">default</span>: it_error ( <span class="stringliteral">"unknown policy"</span> ); |
| 378 | <a name="l00447"></a>00447 } |
| 379 | <a name="l00448"></a>00448 <span class="comment">// copy result to all models</span> |
| 380 | <a name="l00449"></a>00449 <span class="keywordflow">for</span> ( i=0;i<n;i++ ) |
| 381 | <a name="l00450"></a>00450 { |
| 382 | <a name="l00451"></a>00451 <a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a> ( i )->set_statistics ( <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#b2fa2915c35366392fe9bb022ca1a600" title="return expected value">mean</a>(),<a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>.<a class="code" href="classbdm_1_1enorm.html#81d81e35e57c9f194bde248e3affcf1f">_R</a>()); |
| 383 | <a name="l00452"></a>00452 } |
| 384 | <a name="l00453"></a>00453 } |
| 385 | <a name="l00454"></a>00454 <span class="comment">//all posterior densities are equal => return the first one</span> |
| 386 | <a name="l00455"></a>00455 <span class="keyword">const</span> <a class="code" href="classbdm_1_1enorm.html">enorm<chmat></a>* _e()<span class="keyword"> const </span>{<span class="keywordflow">return</span> &<a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>;} |
| 387 | <a name="l00456"></a>00456 <span class="comment">//all posterior densities are equal => return the first one</span> |
| 388 | <a name="l00457"></a>00457 <span class="keyword">const</span> enorm<chmat>& posterior()<span class="keyword"> const </span>{<span class="keywordflow">return</span> <a class="code" href="classbdm_1_1MultiModel.html#d665551d045b1a1055eeb9185558ff0b" title="internal statistics">est</a>;} |
| 389 | <a name="l00458"></a>00458 |
| 390 | <a name="l00459"></a>00459 <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1MultiModel.html#2357434d2ec3c9b4e44e962bdfceda4e" title="This method arrange instance properties according the data stored in the Setting...">from_setting</a>( <span class="keyword">const</span> Setting &root ); |
| 391 | <a name="l00460"></a>00460 |
| 392 | <a name="l00461"></a>00461 <span class="comment">// TODO dodelat void to_setting( Setting &root ) const;</span> |
411 | | <a name="l00480"></a>00480 sq_T iRy ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>,<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a> ); |
412 | | <a name="l00481"></a>00481 vec u = dt.get ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>,<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>+<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a>-1 ); |
413 | | <a name="l00482"></a>00482 vec y = dt.get ( 0,<a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>-1 ); |
414 | | <a name="l00483"></a>00483 <span class="comment">//Time update</span> |
415 | | <a name="l00484"></a>00484 <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a> = pfxu-><a class="code" href="classbdm_1_1diffbifn.html#188f31066bd72e1bf0ddacd1eb0e6af3" title="Evaluates (VS: Do we really need common eval? ).">eval</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>, u ); |
416 | | <a name="l00485"></a>00485 pfxu-><a class="code" href="classbdm_1_1diffbifn.html#651184f808a35f236dbfea21aca1b6ac" title="Evaluates and writes result into A .">dfdx_cond</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,u,<a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>,<span class="keyword">false</span> ); <span class="comment">//update A by a derivative of fx</span> |
417 | | <a name="l00486"></a>00486 |
418 | | <a name="l00487"></a>00487 <span class="comment">//P = A*P*A.transpose() + Q; in sq_T</span> |
419 | | <a name="l00488"></a>00488 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#5530d2756b5d991de755e6121c9a452e" title="Inplace symmetric multiplication by a SQUARE matrix , i.e. .">mult_sym</a> ( <a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a> ); |
420 | | <a name="l00489"></a>00489 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a> +=<a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a>; |
421 | | <a name="l00490"></a>00490 |
422 | | <a name="l00491"></a>00491 <span class="comment">//Data update</span> |
423 | | <a name="l00492"></a>00492 phxu-><a class="code" href="classbdm_1_1diffbifn.html#651184f808a35f236dbfea21aca1b6ac" title="Evaluates and writes result into A .">dfdx_cond</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,u,<a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>,<span class="keyword">false</span> ); <span class="comment">//update C by a derivative hx</span> |
424 | | <a name="l00493"></a>00493 <span class="comment">//_Ry = C*P*C.transpose() + R; in sq_T</span> |
425 | | <a name="l00494"></a>00494 <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#5530d2756b5d991de755e6121c9a452e" title="Inplace symmetric multiplication by a SQUARE matrix , i.e. .">mult_sym</a> ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>, <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a> ); |
426 | | <a name="l00495"></a>00495 ( <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a> ) +=<a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a>; |
| 411 | <a name="l00480"></a>00480 <span class="comment">//initialize matrices A C, later, these will be only updated!</span> |
| 412 | <a name="l00481"></a>00481 pfxu-><a class="code" href="classbdm_1_1diffbifn.html#651184f808a35f236dbfea21aca1b6ac" title="Evaluates and writes result into A .">dfdx_cond</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,zeros ( <a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> ),<a class="code" href="classbdm_1_1Kalman.html#0a2072e2090c10fac74ad30a023a4ace" title="Matrix A.">A</a>,<span class="keyword">true</span> ); |
| 413 | <a name="l00482"></a>00482 <span class="comment">// pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );</span> |
| 414 | <a name="l00483"></a>00483 <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>.clear(); |
| 415 | <a name="l00484"></a>00484 phxu-><a class="code" href="classbdm_1_1diffbifn.html#651184f808a35f236dbfea21aca1b6ac" title="Evaluates and writes result into A .">dfdx_cond</a> ( <a class="code" href="classbdm_1_1Kalman.html#fa172078091e45561343fa513dd573b0" title="cache of est.mu">_mu</a>,zeros ( <a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> ),<a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>,<span class="keyword">true</span> ); |
| 416 | <a name="l00485"></a>00485 <span class="comment">// phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );</span> |
| 417 | <a name="l00486"></a>00486 <a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a>.clear(); |
| 418 | <a name="l00487"></a>00487 |
| 419 | <a name="l00488"></a>00488 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> = R0; |
| 420 | <a name="l00489"></a>00489 <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a> = Q0; |
| 421 | <a name="l00490"></a>00490 } |
| 422 | <a name="l00491"></a>00491 |
| 423 | <a name="l00492"></a>00492 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 424 | <a name="l00493"></a><a class="code" href="classbdm_1_1EKF.html#3fb182ecc29b10ca1163cecbf3bcccfa">00493</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKF.html#3fb182ecc29b10ca1163cecbf3bcccfa" title="Here dt = [yt;ut] of appropriate dimensions.">EKF<sq_T>::bayes</a> ( <span class="keyword">const</span> vec &dt ) |
| 425 | <a name="l00494"></a>00494 { |
| 426 | <a name="l00495"></a>00495 it_assert_debug ( dt.length() == ( <a class="code" href="classbdm_1_1Kalman.html#d2c36ba01760bf207b985bf321b7817f" title="cache of rvy.count()">dimy</a>+<a class="code" href="classbdm_1_1Kalman.html#c5136ef617f6ac0e426bea222755d92b" title="cache of rvu.count()">dimu</a> ),<span class="stringliteral">"KalmanFull::bayes wrong size of dt"</span> ); |