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

Revision 354, 76.7 kB (checked in by smidl, 15 years ago)

doc - oprava

Line 
1<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
2<html><head><meta http-equiv="Content-Type" content="text/html;charset=UTF-8">
3<title>mixpp: libKF.h Source File</title>
4<link href="tabs.css" rel="stylesheet" type="text/css">
5<link href="doxygen.css" rel="stylesheet" type="text/css">
6</head><body>
7<!-- Generated by Doxygen 1.5.8 -->
8<script type="text/javascript">
9<!--
10function changeDisplayState (e){
11  var num=this.id.replace(/[^[0-9]/g,'');
12  var button=this.firstChild;
13  var sectionDiv=document.getElementById('dynsection'+num);
14  if (sectionDiv.style.display=='none'||sectionDiv.style.display==''){
15    sectionDiv.style.display='block';
16    button.src='open.gif';
17  }else{
18    sectionDiv.style.display='none';
19    button.src='closed.gif';
20  }
21}
22function initDynSections(){
23  var divs=document.getElementsByTagName('div');
24  var sectionCounter=1;
25  for(var i=0;i<divs.length-1;i++){
26    if(divs[i].className=='dynheader'&&divs[i+1].className=='dynsection'){
27      var header=divs[i];
28      var section=divs[i+1];
29      var button=header.firstChild;
30      if (button!='IMG'){
31        divs[i].insertBefore(document.createTextNode(' '),divs[i].firstChild);
32        button=document.createElement('img');
33        divs[i].insertBefore(button,divs[i].firstChild);
34      }
35      header.style.cursor='pointer';
36      header.onclick=changeDisplayState;
37      header.id='dynheader'+sectionCounter;
38      button.src='closed.gif';
39      section.id='dynsection'+sectionCounter;
40      section.style.display='none';
41      section.style.marginLeft='14px';
42      sectionCounter++;
43    }
44  }
45}
46window.onload = initDynSections;
47-->
48</script>
49<div class="navigation" id="top">
50  <div class="tabs">
51    <ul>
52      <li><a href="main.html"><span>Main&nbsp;Page</span></a></li>
53      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
54      <li><a href="modules.html"><span>Modules</span></a></li>
55      <li><a href="annotated.html"><span>Classes</span></a></li>
56      <li class="current"><a href="files.html"><span>Files</span></a></li>
57    </ul>
58  </div>
59  <div class="tabs">
60    <ul>
61      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
62      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
63    </ul>
64  </div>
65<h1>libKF.h</h1><a href="libKF_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001
66<a name="l00013"></a>00013 <span class="preprocessor">#ifndef KF_H</span>
67<a name="l00014"></a>00014 <span class="preprocessor"></span><span class="preprocessor">#define KF_H</span>
68<a name="l00015"></a>00015 <span class="preprocessor"></span>
69<a name="l00016"></a>00016
70<a name="l00017"></a>00017 <span class="preprocessor">#include "../stat/libFN.h"</span>
71<a name="l00018"></a>00018 <span class="preprocessor">#include "../stat/libEF.h"</span>
72<a name="l00019"></a>00019 <span class="preprocessor">#include "../math/chmat.h"</span>
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 &amp;dt );
95<a name="l00051"></a>00051                         <span class="keyword">friend</span> std::ostream &amp;<a class="code" href="classbdm_1_1KalmanFull.html#86ba216243ed95bb46d80d88775d16af" title="print elements of KF">operator&lt;&lt; </a>( std::ostream &amp;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> &amp;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
99<a name="l00056"></a>00056
100<a name="l00064"></a>00064         <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
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&lt;sq_T&gt;</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&lt;sq_T&gt;</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&amp; <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&amp; <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&amp; <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&amp; <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&lt;sq_T&gt;</a> &amp;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 &amp;A0,<span class="keyword">const</span> mat &amp;B0,<span class="keyword">const</span> mat &amp;C0,<span class="keyword">const</span> mat &amp;D0,<span class="keyword">const</span> sq_T &amp;Q0,<span class="keyword">const</span> sq_T &amp;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 &amp;mu0, <span class="keyword">const</span> sq_T &amp;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 &amp;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>&amp; <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&lt;sq_T&gt;</a>* _e()<span class="keyword"> const </span>{<span class="keywordflow">return</span> &amp;<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&amp; <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>-&gt;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>&lt;chmat&gt;
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-&gt;<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-&gt;<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 &amp;A0,<span class="keyword">const</span> mat &amp;B0,<span class="keyword">const</span> mat &amp;C0,<span class="keyword">const</span> mat &amp;D0,<span class="keyword">const</span> <a class="code" href="classchmat.html" title="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</a> &amp;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> &amp;R0 );
160<a name="l00160"></a>00160                         <span class="keywordtype">void</span> set_statistics ( <span class="keyword">const</span> vec &amp;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> &amp;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
165<a name="l00165"></a>00165
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 &amp;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&lt;fsqmat&gt;</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 &amp;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>&amp; <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&lt;fsqmat&gt;</a>* _e()<span class="keyword"> const</span>{<span class="keywordflow">return</span> &amp;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>&lt;<span class="keyword">class</span> sq_T&gt;
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>&lt;fsqmat&gt;
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&lt;sq_T&gt;</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&lt;sq_T&gt;</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 &amp;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-&gt;<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-&gt;<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 &amp;dt );
213<a name="l00260"></a>00260         };
214<a name="l00261"></a>00261
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>&lt;ldmat&gt;
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 &amp;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>&lt;ldmat&gt;
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>&lt;<a class="code" href="classldmat.html" title="Matrix stored in LD form, (commonly known as UD).">ldmat</a>&gt; ( ) {};
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 &amp;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         };
242<a name="l00298"></a>00298
243<a name="l00300"></a>00300
244<a name="l00301"></a>00301         <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
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&lt;sq_T&gt;::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&lt;sq_T&gt;</a> &amp;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>&lt;<span class="keyword">class</span> sq_T&gt;
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&lt;sq_T&gt;::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>&lt;<span class="keyword">class</span> sq_T&gt;
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&lt;sq_T&gt;::set_parameters</a> ( <span class="keyword">const</span> mat &amp;A0,<span class="keyword">const</span>  mat &amp;B0, <span class="keyword">const</span> mat &amp;C0, <span class="keyword">const</span> mat &amp;D0, <span class="keyword">const</span> sq_T &amp;Q0, <span class="keyword">const</span> sq_T &amp;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>&lt;<span class="keyword">class</span> sq_T&gt;
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&lt;sq_T&gt;::bayes</a> ( <span class="keyword">const</span> vec &amp;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>;
300<a name="l00357"></a>00357
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> );
316<a name="l00373"></a>00373
317<a name="l00374"></a>00374
318<a name="l00375"></a>00375                 <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>
319<a name="l00376"></a>00376                 {
320<a name="l00377"></a>00377                         <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 );
321<a name="l00378"></a>00378                 }
322<a name="l00379"></a>00379
323<a name="l00380"></a>00380 <span class="comment">//cout &lt;&lt; "y: " &lt;&lt; y-(*_yp) &lt;&lt;" R: " &lt;&lt; _Ry-&gt;to_mat() &lt;&lt; " iR: " &lt;&lt; _iRy-&gt;to_mat() &lt;&lt; " ll: " &lt;&lt; ll &lt;&lt;endl;</span>
324<a name="l00381"></a>00381
325<a name="l00382"></a>00382         };
326<a name="l00383"></a>00383
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&lt;EKFCh*&gt; <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&lt;chmat&gt;</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&lt;EKFCh*&gt; 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)-&gt;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)-&gt;_e()-&gt;mean(), A(0)-&gt;_e()-&gt;_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 &amp;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&lt;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 )-&gt;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 )-&gt;_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> )
360<a name="l00429"></a>00429                                 {
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&lt;chmat&gt;</a>* st=(<a class="code" href="classbdm_1_1MultiModel.html#33de5d07ee774070632de8963b5d4c93" title="List of models between which we switch.">Models</a>(mi)-&gt;_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-&gt;<a class="code" href="classbdm_1_1enorm.html#b2fa2915c35366392fe9bb022ca1a600" title="return expected value">mean</a>(), st-&gt;<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&lt;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 )-&gt;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 =&gt; 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&lt;chmat&gt;</a>* _e()<span class="keyword"> const </span>{<span class="keywordflow">return</span> &amp;<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 =&gt; return the first one</span>
379<a name="l00448"></a>00448                         <span class="keyword">const</span> enorm&lt;chmat&gt;&amp; 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>&lt;<span class="keyword">class</span> sq_T&gt;
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&lt;sq_T&gt;::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>&lt;sq_T&gt; ( rvx0,rvy0,rvu0 ) {}
387<a name="l00456"></a>00456
388<a name="l00457"></a>00457         <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
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&lt;sq_T&gt;::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;
393<a name="l00462"></a>00462
394<a name="l00463"></a>00463                 <span class="comment">//initialize matrices A C, later, these will be only updated!</span>
395<a name="l00464"></a>00464                 pfxu-&gt;<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> );
396<a name="l00465"></a>00465 <span class="comment">//      pfxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),B,true );</span>
397<a name="l00466"></a>00466                 <a class="code" href="classbdm_1_1Kalman.html#5977b2c81857948a35105f0e7840203c" title="Matrix B.">B</a>.clear();
398<a name="l00467"></a>00467                 phxu-&gt;<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> );
399<a name="l00468"></a>00468 <span class="comment">//      phxu-&gt;dfdu_cond ( *_mu,zeros ( dimu ),D,true );</span>
400<a name="l00469"></a>00469                 <a class="code" href="classbdm_1_1Kalman.html#7b56ac423d0654b5755e4f852a870456" title="Matrix D.">D</a>.clear();
401<a name="l00470"></a>00470
402<a name="l00471"></a>00471                 <a class="code" href="classbdm_1_1Kalman.html#475b088287cdfbba4dc60a3d027728b7" title="Matrix R in square-root form.">R</a> = R0;
403<a name="l00472"></a>00472                 <a class="code" href="classbdm_1_1Kalman.html#70f8bf19e81b532c60fd3a7a152425ee" title="Matrix Q in square-root form.">Q</a> = Q0;
404<a name="l00473"></a>00473         }
405<a name="l00474"></a>00474
406<a name="l00475"></a>00475         <span class="keyword">template</span>&lt;<span class="keyword">class</span> sq_T&gt;
407<a name="l00476"></a><a class="code" href="classbdm_1_1EKF.html#3fb182ecc29b10ca1163cecbf3bcccfa">00476</a>         <span class="keywordtype">void</span> <a class="code" href="classbdm_1_1EKF.html#3fb182ecc29b10ca1163cecbf3bcccfa" title="Here dt = [yt;ut] of appropriate dimensions.">EKF&lt;sq_T&gt;::bayes</a> ( <span class="keyword">const</span> vec &amp;dt )
408<a name="l00477"></a>00477         {
409<a name="l00478"></a>00478                 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> );
410<a name="l00479"></a>00479
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-&gt;<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-&gt;<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-&gt;<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>;
427<a name="l00496"></a>00496
428<a name="l00497"></a>00497                 mat Pfull = <a class="code" href="classbdm_1_1Kalman.html#00c27b0bf324f0018497921ca23c71ed" title="cache of est.R">_P</a>.<a class="code" href="classfsqmat.html#f54fc955e8e3b43d15afa92124bc24b3" title="Conversion to full matrix.">to_mat</a>();
429<a name="l00498"></a>00498
430<a name="l00499"></a>00499                 <a class="code" href="classbdm_1_1Kalman.html#2dd268f2d7fbe6382cb8825a1114192a" title="cache of fy.R">_Ry</a>.<a class="code" href="classfsqmat.html#9fa853e1ca28f2a1a1c43377e798ecb1" title="Matrix inversion preserving the chosen form.">inv</a> ( iRy ); <span class="comment">// result is in _iRy;</span>
431<a name="l00500"></a>00500                 <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() );
432<a name="l00501"></a>00501
433<a name="l00502"></a>00502                 sq_T pom ( ( <span class="keywordtype">int</span> ) Pfull.rows() );
434<a name="l00503"></a>00503                 iRy.mult_sym_t ( <a class="code" href="classbdm_1_1Kalman.html#818eba63a23972786a4579ad30294177" title="Matrix C.">C</a>*Pfull,pom );
435<a name="l00504"></a>00504                 ( <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>
436<a name="l00505"></a>00505                 <a class="code" href="classbdm_1_1Kalman.html#c249d45258c8578b13858ad3e7b729b1" title="cache of fy.mu">_yp</a> = phxu-&gt;<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 ); <span class="comment">//y prediction</span>
437<a name="l00506"></a>00506                 ( <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> );
438<a name="l00507"></a>00507
439<a name="l00508"></a>00508                 <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> ) {<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>.<a class="code" href="classbdm_1_1eEF.html#a36d06ecdd6f4c79dc122510eaccc692" title="Evaluate normalized log-probability.">evallog</a> ( y );}
440<a name="l00509"></a>00509         };
441<a name="l00510"></a>00510
442<a name="l00511"></a>00511
443<a name="l00512"></a>00512 }
444<a name="l00513"></a>00513 <span class="preprocessor">#endif // KF_H</span>
445<a name="l00514"></a>00514 <span class="preprocessor"></span>
446<a name="l00515"></a>00515
447</pre></div></div>
448<hr size="1"><address style="text-align: right;"><small>Generated on Tue Jun 2 10:11:00 2009 for mixpp by&nbsp;
449<a href="http://www.doxygen.org/index.html">
450<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.8 </small></address>
451</body>
452</html>
Note: See TracBrowser for help on using the browser.