| 305 | | <a name="l00361"></a>00361 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 306 | | <a name="l00362"></a>00362 shared_ptr<StateSpace<fsqmat> > to_state_space(<span class="keyword">const</span> mlnorm<sq_T > &ml, ivec &theta_in_A, ivec &theta_in_C){ |
| 307 | | <a name="l00363"></a>00363 <span class="comment">//get ids of yrv</span> |
| 308 | | <a name="l00364"></a>00364 |
| 309 | | <a name="l00365"></a>00365 ivec yids= unique(ml._rv()._ids()); <span class="comment">//yrv is now stored in _yrv</span> |
| 310 | | <a name="l00366"></a>00366 ivec dids= unique(ml._rvc()._ids()); <span class="comment">//yrv is now stored in _yrv</span> |
| 311 | | <a name="l00367"></a>00367 ivec uids=unique_complement(dids, yids); |
| 312 | | <a name="l00368"></a>00368 |
| 313 | | <a name="l00369"></a>00369 RV yrv_uni = RV(yids,zeros_i(yids.length())); |
| 314 | | <a name="l00370"></a>00370 RV urv_uni = RV(uids,zeros_i(yids.length())); |
| 315 | | <a name="l00371"></a>00371 <span class="comment">//We can do only 1d now... :(</span> |
| 316 | | <a name="l00372"></a>00372 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a>(yrv_uni._dsize()==urv_uni._dsize()==1, <span class="stringliteral">"Only for SISO so far..."</span> ); |
| 317 | | <a name="l00373"></a>00373 |
| 318 | | <a name="l00374"></a>00374 RV xrv; <span class="comment">//empty</span> |
| 319 | | <a name="l00375"></a>00375 RV Crv; <span class="comment">//empty</span> |
| 320 | | <a name="l00376"></a>00376 <span class="keywordtype">int</span> td=ml._rvc().mintd(); |
| 321 | | <a name="l00377"></a>00377 <span class="keywordflow">for</span> (<span class="keywordtype">int</span> t=-1;t>=td;t--){ |
| 322 | | <a name="l00378"></a>00378 xrv.add(yrv_uni); |
| 323 | | <a name="l00379"></a>00379 Crv.add(urv_uni); |
| 324 | | <a name="l00380"></a>00380 } |
| 325 | | <a name="l00381"></a>00381 |
| 326 | | <a name="l00382"></a>00382 <span class="keywordtype">int</span> dimx = xrv._dsize(); |
| 327 | | <a name="l00383"></a>00383 |
| 328 | | <a name="l00384"></a>00384 theta_in_A = ml._rv().dataind(xrv); |
| 329 | | <a name="l00385"></a>00385 theta_in_C = ml._rvc().dataind(xrv); |
| 330 | | <a name="l00386"></a>00386 <span class="comment">// some chcek of corretness</span> |
| 331 | | <a name="l00387"></a>00387 |
| 332 | | <a name="l00388"></a>00388 vec A1row = zeros(xrv._dsize()); |
| 333 | | <a name="l00389"></a>00389 vec C1row = zeros(xrv._dsize()); |
| 334 | | <a name="l00390"></a>00390 vec theta = ml._A().get_row(0); <span class="comment">// this </span> |
| 335 | | <a name="l00391"></a>00391 set_subvector( A1row, theta_in_A, theta); |
| 336 | | <a name="l00392"></a>00392 set_subvector( C1row, theta_in_C, theta); |
| 337 | | <a name="l00393"></a>00393 |
| 338 | | <a name="l00394"></a>00394 StateSpace<fsqmat> stsp=<span class="keyword">new</span> StateSpace<fsqmat>(); |
| 339 | | <a name="l00395"></a>00395 mat A=zeros(dimx,dimx); |
| 340 | | <a name="l00396"></a>00396 A.set_row(0,A1row); |
| 341 | | <a name="l00397"></a>00397 <span class="keywordflow">for</span> (<span class="keywordtype">int</span> j=1; j<dimx; j++){A(j,j-1)=1.0;} <span class="comment">// off diagonal</span> |
| 342 | | <a name="l00398"></a>00398 mat B=zeros(dimx,1); |
| 343 | | <a name="l00399"></a>00399 B(0) = 1.0; |
| 344 | | <a name="l00400"></a>00400 mat C=zeros(1,dimx); |
| 345 | | <a name="l00401"></a>00401 C.set_row(0,C1row); |
| 346 | | <a name="l00402"></a>00402 stsp.set_parameters(A,B,C,zeros(1,1), zeros(dimx,dimx), ml._R()); |
| 347 | | <a name="l00403"></a>00403 <span class="keywordflow">return</span> stsp; |
| 348 | | <a name="l00404"></a>00404 } |
| 349 | | <a name="l00405"></a>00405 |
| 350 | | <a name="l00407"></a>00407 |
| 351 | | <a name="l00408"></a>00408 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 352 | | <a name="l00409"></a>00409 <span class="keywordtype">void</span> StateSpace<sq_T>::set_parameters (<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) |
| 353 | | <a name="l00410"></a>00410 { |
| 354 | | <a name="l00411"></a>00411 dimx = A0.rows(); |
| 355 | | <a name="l00412"></a>00412 dimu = B0.cols(); |
| 356 | | <a name="l00413"></a>00413 dimy = C0.rows(); |
| 357 | | <a name="l00414"></a>00414 |
| 358 | | <a name="l00415"></a>00415 A = A0; |
| 359 | | <a name="l00416"></a>00416 B = B0; |
| 360 | | <a name="l00417"></a>00417 C = C0; |
| 361 | | <a name="l00418"></a>00418 D = D0; |
| 362 | | <a name="l00419"></a>00419 R = R0; |
| 363 | | <a name="l00420"></a>00420 Q = Q0; |
| 364 | | <a name="l00421"></a>00421 <a class="code" href="classbdm_1_1root.html#a1c314bd6d6dacb8ba78ea5eb88fd9516" title="This method TODO.">validate</a>(); |
| 365 | | <a name="l00422"></a>00422 } |
| 366 | | <a name="l00423"></a>00423 |
| 367 | | <a name="l00424"></a>00424 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| 368 | | <a name="l00425"></a>00425 <span class="keywordtype">void</span> StateSpace<sq_T>::validate(){ |
| 369 | | <a name="l00426"></a>00426 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> (A.cols() == dimx, <span class="stringliteral">"KalmanFull: A is not square"</span>); |
| 370 | | <a name="l00427"></a>00427 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> (B.rows() == dimx, <span class="stringliteral">"KalmanFull: B is not compatible"</span>); |
| 371 | | <a name="l00428"></a>00428 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> (C.cols() == dimx, <span class="stringliteral">"KalmanFull: C is not square"</span>); |
| 372 | | <a name="l00429"></a>00429 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> ( (D.rows() == dimy) || (D.cols() == dimu), <span class="stringliteral">"KalmanFull: D is not compatible"</span>); |
| 373 | | <a name="l00430"></a>00430 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> ( (Q.cols() == dimx) || (Q.rows() == dimx), <span class="stringliteral">"KalmanFull: Q is not compatible"</span>); |
| 374 | | <a name="l00431"></a>00431 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> ( (R.cols() == dimy) || (R.rows() == dimy), <span class="stringliteral">"KalmanFull: R is not compatible"</span>); |
| 375 | | <a name="l00432"></a>00432 } |
| 376 | | <a name="l00433"></a>00433 |
| 377 | | <a name="l00434"></a>00434 } |
| 378 | | <a name="l00435"></a>00435 <span class="preprocessor">#endif // KF_H</span> |
| 379 | | <a name="l00436"></a>00436 <span class="preprocessor"></span> |
| | 305 | <a name="l00362"></a>00362 <span class="comment">//template<class sq_T></span> |
| | 306 | <a name="l00363"></a><a class="code" href="classbdm_1_1StateCanonical.html">00363</a> <span class="keyword">class </span><a class="code" href="classbdm_1_1StateCanonical.html" title="conversion of outer ARX model (mlnorm) to state space model">StateCanonical</a>: <span class="keyword">public</span> <a class="code" href="classbdm_1_1StateSpace.html" title="Basic elements of linear state-space model.">StateSpace</a><fsqmat>{ |
| | 307 | <a name="l00364"></a>00364 <span class="keyword">protected</span>: |
| | 308 | <a name="l00366"></a><a class="code" href="classbdm_1_1StateCanonical.html#a4e6a4771f9444f9b55ddc0b6e65cb5d1">00366</a> <a class="code" href="classbdm_1_1datalink__part.html">datalink_part</a> <a class="code" href="classbdm_1_1StateCanonical.html#a4e6a4771f9444f9b55ddc0b6e65cb5d1" title="remember connection from theta -&gt;A">th2A</a>; |
| | 309 | <a name="l00368"></a><a class="code" href="classbdm_1_1StateCanonical.html#ab7c7aed9b66be7a24cc8ca43cea3123d">00368</a> <a class="code" href="classbdm_1_1datalink__part.html">datalink_part</a> <a class="code" href="classbdm_1_1StateCanonical.html#ab7c7aed9b66be7a24cc8ca43cea3123d" title="remember connection from theta -&gt;C">th2C</a>; |
| | 310 | <a name="l00370"></a><a class="code" href="classbdm_1_1StateCanonical.html#adf03b4d4c48d94f2f0510e086c651c89">00370</a> <a class="code" href="classbdm_1_1datalink__part.html">datalink_part</a> <a class="code" href="classbdm_1_1StateCanonical.html#adf03b4d4c48d94f2f0510e086c651c89" title="remember connection from theta -&gt;D">th2D</a>; |
| | 311 | <a name="l00372"></a><a class="code" href="classbdm_1_1StateCanonical.html#a770b84ff706d4888236096a96ef51730">00372</a> vec <a class="code" href="classbdm_1_1StateCanonical.html#a770b84ff706d4888236096a96ef51730" title="cached first row of A">A1row</a>; |
| | 312 | <a name="l00374"></a><a class="code" href="classbdm_1_1StateCanonical.html#a665df0310519066e7adbbd490baf157d">00374</a> vec <a class="code" href="classbdm_1_1StateCanonical.html#a665df0310519066e7adbbd490baf157d" title="cached first row of C">C1row</a>; |
| | 313 | <a name="l00376"></a><a class="code" href="classbdm_1_1StateCanonical.html#a8e890a7aee1e6c1d06074f6604609eb6">00376</a> vec <a class="code" href="classbdm_1_1StateCanonical.html#a8e890a7aee1e6c1d06074f6604609eb6" title="cached first row of D">D1row</a>; |
| | 314 | <a name="l00377"></a>00377 |
| | 315 | <a name="l00378"></a>00378 <span class="keyword">public</span>: |
| | 316 | <a name="l00380"></a><a class="code" href="classbdm_1_1StateCanonical.html#ad3ad57524c2af597b36ab1592fddc26c">00380</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1StateCanonical.html#ad3ad57524c2af597b36ab1592fddc26c" title="set up this object to match given mlnorm">connect_mlnorm</a>(<span class="keyword">const</span> <a class="code" href="classbdm_1_1mlnorm.html" title="Normal distributed linear function with linear function of mean value;.">mlnorm<fsqmat ></a> &ml){ |
| | 317 | <a name="l00381"></a>00381 <span class="comment">//get ids of yrv </span> |
| | 318 | <a name="l00382"></a>00382 <span class="keyword">const</span> <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> &yrv = ml._rv(); |
| | 319 | <a name="l00383"></a>00383 <span class="comment">//need to determine u_t - it is all in _rvc that is not in ml._rv()</span> |
| | 320 | <a name="l00384"></a>00384 <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> rgr0 = ml._rvc().remove_time(); |
| | 321 | <a name="l00385"></a>00385 <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> urv = rgr0.<a class="code" href="classbdm_1_1RV.html#aaec44dabdf0a6d90fbae95e1356eda39" title="Subtract another variable from the current one.">subt</a>(yrv); |
| | 322 | <a name="l00386"></a>00386 |
| | 323 | <a name="l00387"></a>00387 <span class="comment">//We can do only 1d now... :(</span> |
| | 324 | <a name="l00388"></a>00388 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a>(yrv.<a class="code" href="classbdm_1_1RV.html#ade30156104f61d86c94f758861418089" title="total size of a random variable">_dsize</a>()==1, <span class="stringliteral">"Only for SISO so far..."</span> ); |
| | 325 | <a name="l00389"></a>00389 |
| | 326 | <a name="l00390"></a>00390 <span class="comment">// create names for </span> |
| | 327 | <a name="l00391"></a>00391 <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> xrv; <span class="comment">//empty</span> |
| | 328 | <a name="l00392"></a>00392 <a class="code" href="classbdm_1_1RV.html" title="Class representing variables, most often random variables.">RV</a> Crv; <span class="comment">//empty</span> |
| | 329 | <a name="l00393"></a>00393 <span class="keywordtype">int</span> td=ml._rvc().<a class="code" href="classbdm_1_1RV.html#a9a5c5aeeaf39299d07763377b05a8241" title="Minimum time-offset.">mint</a>(); |
| | 330 | <a name="l00394"></a>00394 <span class="comment">// assuming strictly proper function!!!</span> |
| | 331 | <a name="l00395"></a>00395 <span class="keywordflow">for</span> (<span class="keywordtype">int</span> t=-1;t>=td;t--){ |
| | 332 | <a name="l00396"></a>00396 xrv.add(yrv.<a class="code" href="classbdm_1_1RV.html#a627222f49abc5d07e5e8aecac7708ee2" title="create new RV from the current one with time shifted by given value">copy_t</a>(t)); |
| | 333 | <a name="l00397"></a>00397 Crv.<a class="code" href="classbdm_1_1RV.html#a87841b5ee43997b79789c0c22047e224" title="Add (concat) another variable to the current one,.">add</a>(urv.<a class="code" href="classbdm_1_1RV.html#a627222f49abc5d07e5e8aecac7708ee2" title="create new RV from the current one with time shifted by given value">copy_t</a>(t)); |
| | 334 | <a name="l00398"></a>00398 } |
| | 335 | <a name="l00399"></a>00399 |
| | 336 | <a name="l00400"></a>00400 this-><a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a> = xrv._dsize(); |
| | 337 | <a name="l00401"></a>00401 this-><a class="code" href="classbdm_1_1StateSpace.html#a3b784d85ca911866225a79193ae1441f" title="cache of rvy.count()">dimy</a> = yrv.<a class="code" href="classbdm_1_1RV.html#ade30156104f61d86c94f758861418089" title="total size of a random variable">_dsize</a>(); |
| | 338 | <a name="l00402"></a>00402 this-><a class="code" href="classbdm_1_1StateSpace.html#a95bb59ffbfd34c55d03d4d7a2a9b4faf" title="cache of rvu.count()">dimu</a> = urv.<a class="code" href="classbdm_1_1RV.html#ade30156104f61d86c94f758861418089" title="total size of a random variable">_dsize</a>(); |
| | 339 | <a name="l00403"></a>00403 |
| | 340 | <a name="l00404"></a>00404 <span class="comment">// get mapp</span> |
| | 341 | <a name="l00405"></a>00405 <a class="code" href="classbdm_1_1StateCanonical.html#a4e6a4771f9444f9b55ddc0b6e65cb5d1" title="remember connection from theta -&gt;A">th2A</a>.<a class="code" href="classbdm_1_1datalink__part.html#a6cef1083755c27e4476501abe6f49dce" title="set connection, rv must be fully present in rv_up">set_connection</a>(xrv, ml._rvc()); |
| | 342 | <a name="l00406"></a>00406 <a class="code" href="classbdm_1_1StateCanonical.html#ab7c7aed9b66be7a24cc8ca43cea3123d" title="remember connection from theta -&gt;C">th2C</a>.<a class="code" href="classbdm_1_1datalink__part.html#a6cef1083755c27e4476501abe6f49dce" title="set connection, rv must be fully present in rv_up">set_connection</a>(Crv, ml._rvc()); |
| | 343 | <a name="l00407"></a>00407 <a class="code" href="classbdm_1_1StateCanonical.html#adf03b4d4c48d94f2f0510e086c651c89" title="remember connection from theta -&gt;D">th2D</a>.<a class="code" href="classbdm_1_1datalink__part.html#a6cef1083755c27e4476501abe6f49dce" title="set connection, rv must be fully present in rv_up">set_connection</a>(urv, ml._rvc()); |
| | 344 | <a name="l00408"></a>00408 |
| | 345 | <a name="l00409"></a>00409 <span class="comment">//set matrix sizes</span> |
| | 346 | <a name="l00410"></a>00410 this-><a class="code" href="classbdm_1_1StateSpace.html#ade7d828a8d06cbbd5ac45afeaf9f0c7b" title="Matrix A.">A</a>=zeros(<a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>,<a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>); |
| | 347 | <a name="l00411"></a>00411 <span class="keywordflow">for</span> (<span class="keywordtype">int</span> j=1; j<<a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>; j++){<a class="code" href="classbdm_1_1StateSpace.html#ade7d828a8d06cbbd5ac45afeaf9f0c7b" title="Matrix A.">A</a>(j,j-1)=1.0;} <span class="comment">// off diagonal</span> |
| | 348 | <a name="l00412"></a>00412 this-><a class="code" href="classbdm_1_1StateSpace.html#a0cdaa906550290b759a1584d02b3ca51" title="Matrix B.">B</a>=zeros(dimx,1); |
| | 349 | <a name="l00413"></a>00413 this-><a class="code" href="classbdm_1_1StateSpace.html#a0cdaa906550290b759a1584d02b3ca51" title="Matrix B.">B</a>(0) = 1.0; |
| | 350 | <a name="l00414"></a>00414 this-><a class="code" href="classbdm_1_1StateSpace.html#ac7ec82d2200c956cff6dff559bd32ff9" title="Matrix C.">C</a>=zeros(1,dimx); |
| | 351 | <a name="l00415"></a>00415 this-><a class="code" href="classbdm_1_1StateSpace.html#ad5769a41b34eb518716120cf04dbc23f" title="Matrix D.">D</a>=zeros(1,urv.<a class="code" href="classbdm_1_1RV.html#ade30156104f61d86c94f758861418089" title="total size of a random variable">_dsize</a>()); |
| | 352 | <a name="l00416"></a>00416 this-><a class="code" href="classbdm_1_1StateSpace.html#a66248c83836424f1cd7c1b5b9a92cf1f" title="Matrix Q in square-root form.">Q</a> = zeros(dimx,dimx); |
| | 353 | <a name="l00417"></a>00417 <span class="comment">// R is set by update</span> |
| | 354 | <a name="l00418"></a>00418 |
| | 355 | <a name="l00419"></a>00419 <span class="comment">//set cache</span> |
| | 356 | <a name="l00420"></a>00420 this-><a class="code" href="classbdm_1_1StateCanonical.html#a770b84ff706d4888236096a96ef51730" title="cached first row of A">A1row</a> = zeros(xrv._dsize()); |
| | 357 | <a name="l00421"></a>00421 this-><a class="code" href="classbdm_1_1StateCanonical.html#a665df0310519066e7adbbd490baf157d" title="cached first row of C">C1row</a> = zeros(xrv._dsize()); |
| | 358 | <a name="l00422"></a>00422 this-><a class="code" href="classbdm_1_1StateCanonical.html#a8e890a7aee1e6c1d06074f6604609eb6" title="cached first row of D">D1row</a> = zeros(urv.<a class="code" href="classbdm_1_1RV.html#ade30156104f61d86c94f758861418089" title="total size of a random variable">_dsize</a>()); |
| | 359 | <a name="l00423"></a>00423 |
| | 360 | <a name="l00424"></a>00424 <a class="code" href="classbdm_1_1StateCanonical.html#a25f55da4899112f210ade6d322c78b7e" title="fast function to update parameters from ml - not checked for compatibility!!">update_from</a>(ml); |
| | 361 | <a name="l00425"></a>00425 validate(); |
| | 362 | <a name="l00426"></a>00426 }; |
| | 363 | <a name="l00428"></a><a class="code" href="classbdm_1_1StateCanonical.html#a25f55da4899112f210ade6d322c78b7e">00428</a> <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1StateCanonical.html#a25f55da4899112f210ade6d322c78b7e" title="fast function to update parameters from ml - not checked for compatibility!!">update_from</a>(<span class="keyword">const</span> <a class="code" href="classbdm_1_1mlnorm.html" title="Normal distributed linear function with linear function of mean value;.">mlnorm<fsqmat></a> &ml){ |
| | 364 | <a name="l00429"></a>00429 |
| | 365 | <a name="l00430"></a>00430 vec theta = ml.<a class="code" href="classbdm_1_1mlnorm.html#a55e1bd7fa70b852bd7fe50ce72fb8f23" title="access function">_A</a>().get_row(0); <span class="comment">// this </span> |
| | 366 | <a name="l00431"></a>00431 |
| | 367 | <a name="l00432"></a>00432 <a class="code" href="classbdm_1_1StateCanonical.html#a4e6a4771f9444f9b55ddc0b6e65cb5d1" title="remember connection from theta -&gt;A">th2A</a>.<a class="code" href="classbdm_1_1datalink__part.html#aaba9b673ef82584e1bc06cc7a32b0a59" title="Get val for vector val_down from val of &quot;Up&quot;.">filldown</a>(theta,<a class="code" href="classbdm_1_1StateCanonical.html#a770b84ff706d4888236096a96ef51730" title="cached first row of A">A1row</a>); |
| | 368 | <a name="l00433"></a>00433 <a class="code" href="classbdm_1_1StateCanonical.html#ab7c7aed9b66be7a24cc8ca43cea3123d" title="remember connection from theta -&gt;C">th2C</a>.<a class="code" href="classbdm_1_1datalink__part.html#aaba9b673ef82584e1bc06cc7a32b0a59" title="Get val for vector val_down from val of &quot;Up&quot;.">filldown</a>(theta,<a class="code" href="classbdm_1_1StateCanonical.html#a665df0310519066e7adbbd490baf157d" title="cached first row of C">C1row</a>); |
| | 369 | <a name="l00434"></a>00434 <a class="code" href="classbdm_1_1StateCanonical.html#adf03b4d4c48d94f2f0510e086c651c89" title="remember connection from theta -&gt;D">th2D</a>.<a class="code" href="classbdm_1_1datalink__part.html#aaba9b673ef82584e1bc06cc7a32b0a59" title="Get val for vector val_down from val of &quot;Up&quot;.">filldown</a>(theta,<a class="code" href="classbdm_1_1StateCanonical.html#a8e890a7aee1e6c1d06074f6604609eb6" title="cached first row of D">D1row</a>); |
| | 370 | <a name="l00435"></a>00435 |
| | 371 | <a name="l00436"></a>00436 <a class="code" href="classbdm_1_1StateSpace.html#a11dd3493d62ec9329860386adb3e6084" title="Matrix R in square-root form.">R</a> = ml.<a class="code" href="classbdm_1_1mlnorm.html#a9af0714b7b29ab1162ebc3291b7c4a43" title="access function">_R</a>(); |
| | 373 | <a name="l00438"></a>00438 <a class="code" href="classbdm_1_1StateSpace.html#ade7d828a8d06cbbd5ac45afeaf9f0c7b" title="Matrix A.">A</a>.set_row(0,<a class="code" href="classbdm_1_1StateCanonical.html#a770b84ff706d4888236096a96ef51730" title="cached first row of A">A1row</a>); |
| | 374 | <a name="l00439"></a>00439 <a class="code" href="classbdm_1_1StateSpace.html#ac7ec82d2200c956cff6dff559bd32ff9" title="Matrix C.">C</a>.set_row(0,<a class="code" href="classbdm_1_1StateCanonical.html#a665df0310519066e7adbbd490baf157d" title="cached first row of C">C1row</a>+<a class="code" href="classbdm_1_1StateCanonical.html#a8e890a7aee1e6c1d06074f6604609eb6" title="cached first row of D">D1row</a>*<a class="code" href="classbdm_1_1StateCanonical.html#a770b84ff706d4888236096a96ef51730" title="cached first row of A">A1row</a>); |
| | 375 | <a name="l00440"></a>00440 <a class="code" href="classbdm_1_1StateSpace.html#ad5769a41b34eb518716120cf04dbc23f" title="Matrix D.">D</a>.set_row(0,<a class="code" href="classbdm_1_1StateCanonical.html#a8e890a7aee1e6c1d06074f6604609eb6" title="cached first row of D">D1row</a>); |
| | 376 | <a name="l00441"></a>00441 |
| | 377 | <a name="l00442"></a>00442 } |
| | 378 | <a name="l00443"></a>00443 }; |
| | 379 | <a name="l00444"></a>00444 |
| | 380 | <a name="l00446"></a>00446 |
| | 381 | <a name="l00447"></a>00447 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| | 382 | <a name="l00448"></a>00448 <span class="keywordtype">void</span> StateSpace<sq_T>::set_parameters (<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) |
| | 383 | <a name="l00449"></a>00449 { |
| | 384 | <a name="l00450"></a>00450 <a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a> = A0.rows(); |
| | 385 | <a name="l00451"></a>00451 <a class="code" href="classbdm_1_1StateSpace.html#a95bb59ffbfd34c55d03d4d7a2a9b4faf" title="cache of rvu.count()">dimu</a> = B0.cols(); |
| | 386 | <a name="l00452"></a>00452 <a class="code" href="classbdm_1_1StateSpace.html#a3b784d85ca911866225a79193ae1441f" title="cache of rvy.count()">dimy</a> = C0.rows(); |
| | 387 | <a name="l00453"></a>00453 |
| | 388 | <a name="l00454"></a>00454 <a class="code" href="classbdm_1_1StateSpace.html#ade7d828a8d06cbbd5ac45afeaf9f0c7b" title="Matrix A.">A</a> = A0; |
| | 389 | <a name="l00455"></a>00455 <a class="code" href="classbdm_1_1StateSpace.html#a0cdaa906550290b759a1584d02b3ca51" title="Matrix B.">B</a> = B0; |
| | 390 | <a name="l00456"></a>00456 <a class="code" href="classbdm_1_1StateSpace.html#ac7ec82d2200c956cff6dff559bd32ff9" title="Matrix C.">C</a> = C0; |
| | 391 | <a name="l00457"></a>00457 <a class="code" href="classbdm_1_1StateSpace.html#ad5769a41b34eb518716120cf04dbc23f" title="Matrix D.">D</a> = D0; |
| | 392 | <a name="l00458"></a>00458 <a class="code" href="classbdm_1_1StateSpace.html#a11dd3493d62ec9329860386adb3e6084" title="Matrix R in square-root form.">R</a> = R0; |
| | 393 | <a name="l00459"></a>00459 <a class="code" href="classbdm_1_1StateSpace.html#a66248c83836424f1cd7c1b5b9a92cf1f" title="Matrix Q in square-root form.">Q</a> = Q0; |
| | 394 | <a name="l00460"></a>00460 validate(); |
| | 395 | <a name="l00461"></a>00461 } |
| | 396 | <a name="l00462"></a>00462 |
| | 397 | <a name="l00463"></a>00463 <span class="keyword">template</span><<span class="keyword">class</span> sq_T> |
| | 398 | <a name="l00464"></a>00464 <span class="keywordtype">void</span> StateSpace<sq_T>::validate(){ |
| | 399 | <a name="l00465"></a>00465 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> (<a class="code" href="classbdm_1_1StateSpace.html#ade7d828a8d06cbbd5ac45afeaf9f0c7b" title="Matrix A.">A</a>.cols() == <a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"KalmanFull: A is not square"</span>); |
| | 400 | <a name="l00466"></a>00466 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> (<a class="code" href="classbdm_1_1StateSpace.html#a0cdaa906550290b759a1584d02b3ca51" title="Matrix B.">B</a>.rows() == <a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"KalmanFull: B is not compatible"</span>); |
| | 401 | <a name="l00467"></a>00467 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> (<a class="code" href="classbdm_1_1StateSpace.html#ac7ec82d2200c956cff6dff559bd32ff9" title="Matrix C.">C</a>.cols() == <a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>, <span class="stringliteral">"KalmanFull: C is not square"</span>); |
| | 402 | <a name="l00468"></a>00468 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> ( (<a class="code" href="classbdm_1_1StateSpace.html#ad5769a41b34eb518716120cf04dbc23f" title="Matrix D.">D</a>.rows() == <a class="code" href="classbdm_1_1StateSpace.html#a3b784d85ca911866225a79193ae1441f" title="cache of rvy.count()">dimy</a>) || (<a class="code" href="classbdm_1_1StateSpace.html#ad5769a41b34eb518716120cf04dbc23f" title="Matrix D.">D</a>.cols() == <a class="code" href="classbdm_1_1StateSpace.html#a95bb59ffbfd34c55d03d4d7a2a9b4faf" title="cache of rvu.count()">dimu</a>), <span class="stringliteral">"KalmanFull: D is not compatible"</span>); |
| | 403 | <a name="l00469"></a>00469 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> ( (<a class="code" href="classbdm_1_1StateSpace.html#a66248c83836424f1cd7c1b5b9a92cf1f" title="Matrix Q in square-root form.">Q</a>.cols() == <a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>) || (<a class="code" href="classbdm_1_1StateSpace.html#a66248c83836424f1cd7c1b5b9a92cf1f" title="Matrix Q in square-root form.">Q</a>.rows() == <a class="code" href="classbdm_1_1StateSpace.html#ac3120f1c9cd7c3a0ba9944e4a2381f21" title="cache of rv.count()">dimx</a>), <span class="stringliteral">"KalmanFull: Q is not compatible"</span>); |
| | 404 | <a name="l00470"></a>00470 <a class="code" href="bdmerror_8h.html#a89a0f906b242b79c5d3d342291b2cab4" title="Throw std::runtime_exception if t is not true and NDEBUG is not defined.">bdm_assert_debug</a> ( (<a class="code" href="classbdm_1_1StateSpace.html#a11dd3493d62ec9329860386adb3e6084" title="Matrix R in square-root form.">R</a>.cols() == <a class="code" href="classbdm_1_1StateSpace.html#a3b784d85ca911866225a79193ae1441f" title="cache of rvy.count()">dimy</a>) || (<a class="code" href="classbdm_1_1StateSpace.html#a11dd3493d62ec9329860386adb3e6084" title="Matrix R in square-root form.">R</a>.rows() == <a class="code" href="classbdm_1_1StateSpace.html#a3b784d85ca911866225a79193ae1441f" title="cache of rvy.count()">dimy</a>), <span class="stringliteral">"KalmanFull: R is not compatible"</span>); |
| | 405 | <a name="l00471"></a>00471 } |
| | 406 | <a name="l00472"></a>00472 |
| | 407 | <a name="l00473"></a>00473 } |
| | 408 | <a name="l00474"></a>00474 <span class="preprocessor">#endif // KF_H</span> |
| | 409 | <a name="l00475"></a>00475 <span class="preprocessor"></span> |
| | 410 | <a name="l00476"></a>00476 |
