root/doc/html/ekf__obj_8h-source.html @ 330

Revision 323, 10.3 kB (checked in by smidl, 15 years ago)

doc

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: ekf_obj.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>ekf_obj.h</h1><a href="ekf__obj_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 EKFfix_H</span>
67<a name="l00014"></a>00014 <span class="preprocessor"></span><span class="preprocessor">#define EKFfix_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 &lt;<a class="code" href="libKF_8h.html" title="Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions.">estim/libKF.h</a>&gt;</span>
71<a name="l00018"></a>00018 <span class="preprocessor">#include "fixed.h"</span>
72<a name="l00019"></a>00019 <span class="preprocessor">#include "matrix.h"</span>
73<a name="l00020"></a>00020 <span class="preprocessor">#include "reference.h"</span>
74<a name="l00021"></a>00021 <span class="preprocessor">#include "parametry_motoru.h"</span>
75<a name="l00022"></a>00022
76<a name="l00023"></a>00023 <span class="keyword">using namespace </span>bdm;
77<a name="l00024"></a>00024
78<a name="l00025"></a>00025 <span class="keywordtype">double</span> minQ(<span class="keywordtype">double</span> Q);
79<a name="l00026"></a>00026
80<a name="l00032"></a><a class="code" href="classEKFfixed.html">00032</a> <span class="keyword">class </span><a class="code" href="classEKFfixed.html" title="Extended Kalman Filter with full matrices in fixed point arithmetic.">EKFfixed</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> {
81<a name="l00033"></a>00033 <span class="keyword">public</span>:
82<a name="l00034"></a>00034 <span class="keywordtype">void</span> init_ekf(<span class="keywordtype">double</span> Tv);
83<a name="l00035"></a>00035 <span class="keywordtype">void</span> ekf(<span class="keywordtype">double</span> ux, <span class="keywordtype">double</span> uy, <span class="keywordtype">double</span> isxd, <span class="keywordtype">double</span> isyd);
84<a name="l00036"></a>00036
85<a name="l00037"></a>00037 <span class="comment">/* Declaration of local functions */</span>
86<a name="l00038"></a>00038 <span class="keywordtype">void</span> prediction(<span class="keywordtype">int</span> *ux);
87<a name="l00039"></a>00039 <span class="keywordtype">void</span> correction(<span class="keywordtype">void</span>);
88<a name="l00040"></a>00040 <span class="keywordtype">void</span> update_psi(<span class="keywordtype">void</span>);
89<a name="l00041"></a>00041
90<a name="l00042"></a>00042 <span class="comment">/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/</span>
91<a name="l00043"></a>00043  <span class="keywordtype">int</span> Q[16]; <span class="comment">/* matrix [4,4] */</span>
92<a name="l00044"></a>00044  <span class="keywordtype">int</span> R[4]; <span class="comment">/* matrix [2,2] */</span>
93<a name="l00045"></a>00045
94<a name="l00046"></a>00046  <span class="keywordtype">int</span> x_est[4];
95<a name="l00047"></a>00047  <span class="keywordtype">int</span> x_pred[4];
96<a name="l00048"></a>00048  <span class="keywordtype">int</span> P_pred[16]; <span class="comment">/* matrix [4,4] */</span>
97<a name="l00049"></a>00049  <span class="keywordtype">int</span> P_est[16]; <span class="comment">/* matrix [4,4] */</span>
98<a name="l00050"></a>00050  <span class="keywordtype">int</span> Y_mes[2];
99<a name="l00051"></a>00051  <span class="keywordtype">int</span> ukalm[2];
100<a name="l00052"></a>00052  <span class="keywordtype">int</span> Kalm[8]; <span class="comment">/* matrix [5,2] */</span>
101<a name="l00053"></a>00053
102<a name="l00054"></a>00054  <span class="keywordtype">int</span> PSI[16]; <span class="comment">/* matrix [4,4] */</span>
103<a name="l00055"></a>00055
104<a name="l00056"></a>00056  <span class="keywordtype">int</span> temp15a[16];
105<a name="l00057"></a>00057
106<a name="l00058"></a>00058  <span class="keywordtype">int</span> cA, cB, cC, cG, cH;  <span class="comment">// cD, cE, cF, cI ... nepouzivane</span>
107<a name="l00059"></a>00059
108<a name="l00060"></a>00060  <span class="keywordtype">long</span> temp30a[4]; <span class="comment">/* matrix [2,2] - temporary matrix for inversion */</span>
109<a name="l00061"></a>00061  <a class="code" href="classbdm_1_1enorm.html">enorm&lt;fsqmat&gt;</a> E;
110<a name="l00062"></a>00062  mat Ry;
111<a name="l00063"></a>00063   
112<a name="l00064"></a>00064 <span class="keyword">public</span>:
113<a name="l00066"></a><a class="code" href="classEKFfixed.html#05d1f11bd56305420b56fe83ada6e586">00066</a>         <a class="code" href="classEKFfixed.html#05d1f11bd56305420b56fe83ada6e586" title="Default constructor.">EKFfixed</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>(),E(),Ry(2,2){
114<a name="l00067"></a>00067         <span class="keywordtype">int</span> i;
115<a name="l00068"></a>00068  <span class="keywordflow">for</span>(i=0;i&lt;16;i++){Q[i]=0;}
116<a name="l00069"></a>00069  <span class="keywordflow">for</span>(i=0;i&lt;4;i++){R[i]=0;}
117<a name="l00070"></a>00070
118<a name="l00071"></a>00071  <span class="keywordflow">for</span>(i=0;i&lt;4;i++){x_est[i]=0;}
119<a name="l00072"></a>00072  <span class="keywordflow">for</span>(i=0;i&lt;4;i++){x_pred[i]=0;}
120<a name="l00073"></a>00073  <span class="keywordflow">for</span>(i=0;i&lt;16;i++){P_pred[i]=0;}
121<a name="l00074"></a>00074  <span class="keywordflow">for</span>(i=0;i&lt;16;i++){P_est[i]=0;}
122<a name="l00075"></a>00075  P_est[0]=0x7FFF;
123<a name="l00076"></a>00076  P_est[5]=0x7FFF;
124<a name="l00077"></a>00077  P_est[10]=0x7FFF;
125<a name="l00078"></a>00078  P_est[15]=0x7FFF;
126<a name="l00079"></a>00079  <span class="keywordflow">for</span>(i=0;i&lt;2;i++){Y_mes[i]=0;}
127<a name="l00080"></a>00080  <span class="keywordflow">for</span>(i=0;i&lt;2;i++){ukalm[i]=0;}
128<a name="l00081"></a>00081  <span class="keywordflow">for</span>(i=0;i&lt;8;i++){Kalm[i]=0;}
129<a name="l00082"></a>00082
130<a name="l00083"></a>00083  <span class="keywordflow">for</span>(i=0;i&lt;16;i++){PSI[i]=0;}
131<a name="l00084"></a>00084 };
132<a name="l00086"></a>00086         <span class="keywordtype">void</span> <a class="code" href="classEKFfixed.html#ddf5334bc1207658fd53698fffbac028" title="Here dt = [yt;ut] of appropriate dimensions.">bayes</a> ( <span class="keyword">const</span> vec &amp;dt );
133<a name="l00088"></a><a class="code" href="classEKFfixed.html#f3979e5514fe4278a519ba4481e287ac">00088</a>         <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="classEKFfixed.html#f3979e5514fe4278a519ba4481e287ac" title="dummy!">posterior</a>(){<span class="keywordflow">return</span> E;};
134<a name="l00089"></a><a class="code" href="classEKFfixed.html#c7fee79e75ad7f0c0e96c5a322cbf44e">00089</a>         <span class="keywordtype">void</span> <a class="code" href="classEKFfixed.html#c7fee79e75ad7f0c0e96c5a322cbf44e" title="Substitute val for rvc.">condition</a> ( <span class="keyword">const</span> vec &amp;Q0 ) {
135<a name="l00090"></a>00090                 
136<a name="l00091"></a>00091                 Q[0]=prevod(minQ(Q0(0)),15);       <span class="comment">// 0.05</span>
137<a name="l00092"></a>00092                 Q[5]=prevod(minQ(Q0(1)),15);
138<a name="l00093"></a>00093                 Q[10]=prevod(minQ(Q0(2)),15);      <span class="comment">// 1e-3</span>
139<a name="l00094"></a>00094                 Q[15]=prevod(minQ(Q0(3)),15);      <span class="comment">// 1e-3</span>
140<a name="l00095"></a>00095
141<a name="l00096"></a>00096         }
142<a name="l00097"></a>00097 };
143<a name="l00098"></a>00098
144<a name="l00099"></a>00099
145<a name="l00100"></a>00100 <span class="preprocessor">#endif // KF_H</span>
146<a name="l00101"></a>00101 <span class="preprocessor"></span>
147<a name="l00102"></a>00102
148</pre></div></div>
149<hr size="1"><address style="text-align: right;"><small>Generated on Thu Apr 23 21:06:42 2009 for mixpp by&nbsp;
150<a href="http://www.doxygen.org/index.html">
151<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.5.8 </small></address>
152</body>
153</html>
Note: See TracBrowser for help on using the browser.