root/doc/xml/ekf__obj_8cpp.xml @ 224

Revision 187, 29.4 kB (checked in by smidl, 17 years ago)

doc

Line 
1<?xml version='1.0' encoding='UTF-8' standalone='no'?>
2<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.5.6">
3  <compounddef id="ekf__obj_8cpp" kind="file">
4    <compoundname>ekf_obj.cpp</compoundname>
5    <includes local="no">itpp/itbase.h</includes>
6    <includes refid="libKF_8h" local="no">estim/libKF.h</includes>
7    <includes refid="ekf__obj_8h" local="yes">ekf_obj.h</includes>
8    <includes refid="simulator_8h" local="yes">../simulator.h</includes>
9    <incdepgraph>
10      <node id="725">
11        <label>libBM.h</label>
12        <link refid="libBM_8h"/>
13        <childnode refid="722" relation="include">
14        </childnode>
15      </node>
16      <node id="735">
17        <label>../simulator.h</label>
18        <link refid="simulator_8h-source"/>
19      </node>
20      <node id="728">
21        <label>../itpp_ext.h</label>
22        <link refid="itpp__ext_8h-source"/>
23        <childnode refid="722" relation="include">
24        </childnode>
25      </node>
26      <node id="721">
27        <label>work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp</label>
28        <link refid="ekf_obj.cpp"/>
29        <childnode refid="722" relation="include">
30        </childnode>
31        <childnode refid="723" relation="include">
32        </childnode>
33        <childnode refid="730" relation="include">
34        </childnode>
35        <childnode refid="735" relation="include">
36        </childnode>
37      </node>
38      <node id="726">
39        <label>../stat/libEF.h</label>
40        <link refid="libEF_8h"/>
41        <childnode refid="722" relation="include">
42        </childnode>
43        <childnode refid="727" relation="include">
44        </childnode>
45        <childnode refid="725" relation="include">
46        </childnode>
47        <childnode refid="728" relation="include">
48        </childnode>
49      </node>
50      <node id="724">
51        <label>../stat/libFN.h</label>
52        <link refid="libFN_8h-source"/>
53        <childnode refid="722" relation="include">
54        </childnode>
55        <childnode refid="725" relation="include">
56        </childnode>
57      </node>
58      <node id="733">
59        <label>reference.h</label>
60        <link refid="reference_8h-source"/>
61      </node>
62      <node id="722">
63        <label>itpp/itbase.h</label>
64      </node>
65      <node id="731">
66        <label>fixed.h</label>
67        <link refid="fixed_8h-source"/>
68      </node>
69      <node id="729">
70        <label>../math/chmat.h</label>
71        <link refid="chmat_8h"/>
72        <childnode refid="722" relation="include">
73        </childnode>
74        <childnode refid="727" relation="include">
75        </childnode>
76        <childnode refid="728" relation="include">
77        </childnode>
78      </node>
79      <node id="723">
80        <label>estim/libKF.h</label>
81        <link refid="libKF_8h"/>
82        <childnode refid="722" relation="include">
83        </childnode>
84        <childnode refid="724" relation="include">
85        </childnode>
86        <childnode refid="726" relation="include">
87        </childnode>
88        <childnode refid="729" relation="include">
89        </childnode>
90      </node>
91      <node id="727">
92        <label>../math/libDC.h</label>
93        <link refid="libDC_8h"/>
94        <childnode refid="722" relation="include">
95        </childnode>
96      </node>
97      <node id="732">
98        <label>matrix.h</label>
99        <link refid="matrix_8h-source"/>
100      </node>
101      <node id="734">
102        <label>parametry_motoru.h</label>
103        <link refid="parametry__motoru_8h-source"/>
104      </node>
105      <node id="730">
106        <label>ekf_obj.h</label>
107        <link refid="ekf__obj_8h"/>
108        <childnode refid="722" relation="include">
109        </childnode>
110        <childnode refid="723" relation="include">
111        </childnode>
112        <childnode refid="731" relation="include">
113        </childnode>
114        <childnode refid="732" relation="include">
115        </childnode>
116        <childnode refid="733" relation="include">
117        </childnode>
118        <childnode refid="734" relation="include">
119        </childnode>
120      </node>
121    </incdepgraph>
122      <sectiondef kind="func">
123      <memberdef kind="function" id="ekf__obj_8cpp_120a104802ddcb544b8de3fe3e5e03253" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
124        <type>double</type>
125        <definition>double minQ</definition>
126        <argsstring>(double Q)</argsstring>
127        <name>minQ</name>
128        <param>
129          <type>double</type>
130          <declname>Q</declname>
131        </param>
132        <briefdescription>
133        </briefdescription>
134        <detaileddescription>
135        </detaileddescription>
136        <inbodydescription>
137        </inbodydescription>
138        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp" line="7" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp" bodystart="7" bodyend="7"/>
139      </memberdef>
140      </sectiondef>
141    <briefdescription>
142    </briefdescription>
143    <detaileddescription>
144    </detaileddescription>
145    <programlisting>
146<codeline lineno="1"><highlight class="preprocessor">#include<sp/>&lt;itpp/itbase.h&gt;</highlight><highlight class="normal"></highlight></codeline>
147<codeline lineno="2"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;<ref refid="libKF_8h" kindref="compound" tooltip="Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions.">estim/libKF.h</ref>&gt;</highlight><highlight class="normal"></highlight></codeline>
148<codeline lineno="3"><highlight class="normal"></highlight></codeline>
149<codeline lineno="4"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;<ref refid="ekf__obj_8h" kindref="compound" tooltip="Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions.">ekf_obj.h</ref>&quot;</highlight><highlight class="normal"></highlight></codeline>
150<codeline lineno="5"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;../simulator.h&quot;</highlight><highlight class="normal"></highlight></codeline>
151<codeline lineno="6"><highlight class="normal"></highlight></codeline>
152<codeline lineno="7"><highlight class="normal"></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>minQ(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Q){</highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(Q&gt;1.0){<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>1.0;}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>Q;};};</highlight></codeline>
153<codeline lineno="8"><highlight class="normal"></highlight></codeline>
154<codeline lineno="10" refid="classEKFfixed_1ddf5334bc1207658fd53698fffbac028" refkind="member"><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classEKFfixed_1ddf5334bc1207658fd53698fffbac028" kindref="member" tooltip="Here dt = [yt;ut] of appropriate dimensions.">EKFfixed::bayes</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>vec<sp/>&amp;dt){</highlight></codeline>
155<codeline lineno="11"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>ekf(dt(2),dt(3),dt(0),dt(1));</highlight></codeline>
156<codeline lineno="12"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>xhat(4);<sp/><sp/><sp/><sp/></highlight></codeline>
157<codeline lineno="13"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//UGLY<sp/>HACK!!!<sp/>reliance<sp/>on<sp/>a<sp/>predictor!!</highlight><highlight class="normal"></highlight></codeline>
158<codeline lineno="14"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xhat(0)=zprevod(x_est[0],Qm)*Iref;</highlight></codeline>
159<codeline lineno="15"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xhat(1)=zprevod(x_est[1],Qm)*Iref;</highlight></codeline>
160<codeline lineno="16"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xhat(2)=zprevod(x_est[2],Qm)*Wref;</highlight></codeline>
161<codeline lineno="17"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xhat(3)=zprevod(x_est[3],15);</highlight></codeline>
162<codeline lineno="18"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline>
163<codeline lineno="19"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>E.<ref refid="classenorm_1d892a38f03be12e572ea57d9689cef6b" kindref="member" tooltip="access function">set_mu</ref>(xhat);</highlight></codeline>
164<codeline lineno="20"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline>
165<codeline lineno="21"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(<sp/><ref refid="classBM_1bf6fb59b30141074f8ee1e2f43d03129" kindref="member" tooltip="If true, the filter will compute likelihood of the data record and store it in ll...">BM::evalll</ref><sp/>)<sp/>{</highlight></codeline>
166<codeline lineno="22"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//from<sp/>enorm</highlight><highlight class="normal"></highlight></codeline>
167<codeline lineno="23"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>xdif(x,4);</highlight><highlight class="comment">//first<sp/>4<sp/>of<sp/>x</highlight><highlight class="normal"></highlight></codeline>
168<codeline lineno="24"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//UGLY<sp/>HACK!!!<sp/>reliance<sp/>on<sp/>a<sp/>predictor!!</highlight><highlight class="normal"></highlight></codeline>
169<codeline lineno="25"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref;</highlight></codeline>
170<codeline lineno="26"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref;</highlight></codeline>
171<codeline lineno="27"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref;</highlight></codeline>
172<codeline lineno="28"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xdif(3)=x[3]-zprevod(x_pred[3],15);*/</highlight><highlight class="normal"></highlight></codeline>
173<codeline lineno="29"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline>
174<codeline lineno="30"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>xdif<sp/>-=xhat;<sp/></highlight><highlight class="comment">//(xdif=x-xhat)</highlight><highlight class="normal"></highlight></codeline>
175<codeline lineno="31"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline>
176<codeline lineno="32"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Pfull(4,4);</highlight></codeline>
177<codeline lineno="33"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal">*<sp/>Pp=Pfull._data();</highlight></codeline>
178<codeline lineno="34"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal">(</highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>i=0;i&lt;16;i++){*(Pp++)<sp/>=<sp/>zprevod(P_est[i],15);}</highlight></codeline>
179<codeline lineno="35"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline>
180<codeline lineno="36"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classBM_15623fef6572a08c2b53b8c87b82dc979" kindref="member" tooltip="Logarithm of marginalized data likelihood.">BM::ll</ref><sp/>=<sp/>-0.5*<sp/>(<sp/>4<sp/>*<sp/>1.83787706640935<sp/>+log<sp/>(<sp/>det<sp/>(<sp/>Pfull<sp/>)<sp/>)<sp/>+xdif*<sp/>(<sp/>inv(Pfull)*xdif<sp/>)<sp/>);</highlight></codeline>
181<codeline lineno="37"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline>
182<codeline lineno="38"><highlight class="normal">};</highlight></codeline>
183<codeline lineno="39"><highlight class="normal"></highlight></codeline>
184<codeline lineno="40"><highlight class="normal"></highlight></codeline>
185<codeline lineno="41"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>EKFfixed::update_psi(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">)</highlight></codeline>
186<codeline lineno="42"><highlight class="normal">{</highlight></codeline>
187<codeline lineno="43"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t_sin,t_cos,tmp;</highlight></codeline>
188<codeline lineno="44"><highlight class="normal"></highlight></codeline>
189<codeline lineno="45"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline>
190<codeline lineno="46"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
191<codeline lineno="47"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
192<codeline lineno="48"><highlight class="normal"></highlight></codeline>
193<codeline lineno="49"><highlight class="normal"><sp/><sp/>PSI[2]=((long)cB*t_sin)&gt;&gt;15;</highlight></codeline>
194<codeline lineno="50"><highlight class="normal"><sp/><sp/>tmp=((long)cH*x_est[2])&gt;&gt;15;</highlight></codeline>
195<codeline lineno="51"><highlight class="normal"><sp/><sp/>PSI[3]=((long)tmp*t_cos)&gt;&gt;15;</highlight></codeline>
196<codeline lineno="52"><highlight class="normal"><sp/><sp/>PSI[6]=-((long)cB*t_cos)&gt;&gt;15;</highlight></codeline>
197<codeline lineno="53"><highlight class="normal"><sp/><sp/>PSI[7]=((long)tmp*t_sin)&gt;&gt;15;</highlight></codeline>
198<codeline lineno="54"><highlight class="normal">}</highlight></codeline>
199<codeline lineno="55"><highlight class="normal"></highlight></codeline>
200<codeline lineno="56"><highlight class="normal"></highlight></codeline>
201<codeline lineno="57"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>EKFfixed::prediction(</highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>*ux)</highlight></codeline>
202<codeline lineno="58"><highlight class="normal">{</highlight></codeline>
203<codeline lineno="59"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t_sin,t_cos,<sp/>tmp;</highlight></codeline>
204<codeline lineno="60"><highlight class="normal"></highlight></codeline>
205<codeline lineno="61"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline>
206<codeline lineno="62"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
207<codeline lineno="63"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
208<codeline lineno="64"><highlight class="normal"></highlight></codeline>
209<codeline lineno="65"><highlight class="normal"><sp/><sp/>tmp=((long)cB*x_est[2])&gt;&gt;15;</highlight></codeline>
210<codeline lineno="66"><highlight class="normal"><sp/><sp/>x_pred[0]=((long)cA*x_est[0]+(</highlight><highlight class="keywordtype">long</highlight><highlight class="normal">)tmp*t_sin+(long)cC*ux[0])&gt;&gt;15;</highlight></codeline>
211<codeline lineno="67"><highlight class="normal"><sp/><sp/>x_pred[1]=((long)cA*x_est[1]-(</highlight><highlight class="keywordtype">long</highlight><highlight class="normal">)tmp*t_cos+(long)cC*ux[1])&gt;&gt;15;</highlight></codeline>
212<codeline lineno="68"><highlight class="normal"><sp/><sp/>x_pred[2]=x_est[2];</highlight></codeline>
213<codeline lineno="69"><highlight class="normal"><sp/><sp/>x_pred[3]=(((long)x_est[3]&lt;&lt;15)+(long)cG*x_est[2])&gt;&gt;15;</highlight></codeline>
214<codeline lineno="70"><highlight class="normal"></highlight></codeline>
215<codeline lineno="71"><highlight class="normal"><sp/><sp/>update_psi();</highlight></codeline>
216<codeline lineno="72"><highlight class="normal"></highlight></codeline>
217<codeline lineno="73"><highlight class="normal"><sp/><sp/>mmult(PSI,P_est,temp15a,3,3,3);</highlight></codeline>
218<codeline lineno="74"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mtrans(PSI,temp15b,5,5);</highlight><highlight class="normal"></highlight></codeline>
219<codeline lineno="75"><highlight class="normal"><sp/><sp/>mmultt(temp15a,PSI,P_pred,3,3,3);</highlight></codeline>
220<codeline lineno="76"><highlight class="normal"><sp/><sp/>maddD(P_pred,Q,3,3);</highlight></codeline>
221<codeline lineno="77"><highlight class="normal">}</highlight></codeline>
222<codeline lineno="78"><highlight class="normal"></highlight></codeline>
223<codeline lineno="79"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>EKFfixed::correction(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">)</highlight></codeline>
224<codeline lineno="80"><highlight class="normal">{</highlight></codeline>
225<codeline lineno="81"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Y_error[2];</highlight></codeline>
226<codeline lineno="82"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">long</highlight><highlight class="normal"><sp/>temp30a[4];<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[2,2]<sp/>-<sp/>temporary<sp/>matrix<sp/>for<sp/>inversion<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
227<codeline lineno="83"><highlight class="normal"></highlight></codeline>
228<codeline lineno="84"><highlight class="normal"><sp/><sp/>choice_P(P_pred,temp15a,3);</highlight></codeline>
229<codeline lineno="85"><highlight class="normal"><sp/><sp/>maddD(temp15a,R,1,1);</highlight></codeline>
230<codeline lineno="86"><highlight class="normal"><sp/><sp/>minv2(temp15a,temp30a);</highlight></codeline>
231<codeline lineno="87"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Ry(0,0)=zprevod(temp15a[0],15);</highlight></codeline>
232<codeline lineno="88"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Ry(0,1)=zprevod(temp15a[1],15);</highlight></codeline>
233<codeline lineno="89"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Ry(1,0)=zprevod(temp15a[2],15);</highlight></codeline>
234<codeline lineno="90"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Ry(1,1)=zprevod(temp15a[3],15);</highlight></codeline>
235<codeline lineno="91"><highlight class="normal"><sp/><sp/></highlight></codeline>
236<codeline lineno="92"><highlight class="normal"><sp/><sp/>mmultDr(P_pred,temp15a,3,3,1,1);</highlight></codeline>
237<codeline lineno="93"><highlight class="normal"><sp/><sp/>mmult1530(temp15a,temp30a,Kalm,3,1,1);</highlight></codeline>
238<codeline lineno="94"><highlight class="normal"></highlight></codeline>
239<codeline lineno="95"><highlight class="normal"></highlight></codeline>
240<codeline lineno="96"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>estimate<sp/>the<sp/>state<sp/>system<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
241<codeline lineno="97"><highlight class="normal"><sp/><sp/>choice_x(x_pred,<sp/>temp15a);</highlight></codeline>
242<codeline lineno="98"><highlight class="normal"><sp/><sp/>msub(Y_mes,temp15a,Y_error,1,0);</highlight></codeline>
243<codeline lineno="99"><highlight class="normal"><sp/><sp/>mmult(Kalm,Y_error,temp15a,3,1,0);</highlight></codeline>
244<codeline lineno="100"><highlight class="normal"><sp/><sp/>madd(x_pred,temp15a,x_est,3,0);</highlight></codeline>
245<codeline lineno="101"><highlight class="normal"></highlight></codeline>
246<codeline lineno="102"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>of<sp/>covariances<sp/>-<sp/>version<sp/>without<sp/>MMULTDL()<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
247<codeline lineno="103"><highlight class="normal"></highlight></codeline>
248<codeline lineno="104"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Version<sp/>with<sp/>MMULTDL()<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
249<codeline lineno="105"><highlight class="normal"><sp/><sp/>mmultDl(P_pred,temp15a,1,3,3,1);</highlight></codeline>
250<codeline lineno="106"><highlight class="normal"></highlight></codeline>
251<codeline lineno="107"><highlight class="normal"><sp/><sp/>mmult(Kalm,temp15a,P_est,3,1,3);</highlight></codeline>
252<codeline lineno="108"><highlight class="normal"><sp/><sp/>msub(P_pred,P_est,P_est,3,3);</highlight></codeline>
253<codeline lineno="109"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>END<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
254<codeline lineno="110"><highlight class="normal">}</highlight></codeline>
255<codeline lineno="111"><highlight class="normal"></highlight></codeline>
256<codeline lineno="112"><highlight class="normal"></highlight></codeline>
257<codeline lineno="113"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>EKFfixed::ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>ux,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>uy,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>isxd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>isyd)</highlight></codeline>
258<codeline lineno="114"><highlight class="normal">{</highlight></codeline>
259<codeline lineno="115"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>vypocet<sp/>napeti<sp/>v<sp/>systemu<sp/>(x,y)</highlight><highlight class="normal"></highlight></codeline>
260<codeline lineno="116"><highlight class="normal"><sp/><sp/>ukalm[0]=prevod(ux/Uref,Qm);</highlight></codeline>
261<codeline lineno="117"><highlight class="normal"><sp/><sp/>ukalm[1]=prevod(uy/Uref,Qm);</highlight></codeline>
262<codeline lineno="118"><highlight class="normal"></highlight></codeline>
263<codeline lineno="119"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>zadani<sp/>mereni</highlight><highlight class="normal"></highlight></codeline>
264<codeline lineno="120"><highlight class="normal"><sp/><sp/>Y_mes[0]=prevod(isxd/Iref,Qm);</highlight></codeline>
265<codeline lineno="121"><highlight class="normal"><sp/><sp/>Y_mes[1]=prevod(isyd/Iref,Qm);</highlight></codeline>
266<codeline lineno="122"><highlight class="normal"></highlight></codeline>
267<codeline lineno="124"><highlight class="normal"><sp/><sp/>prediction(ukalm);</highlight></codeline>
268<codeline lineno="125"><highlight class="normal"><sp/><sp/>correction();</highlight></codeline>
269<codeline lineno="126"><highlight class="normal"></highlight></codeline>
270<codeline lineno="127"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>navrat<sp/>estimovanych<sp/>hodnot<sp/>regulatoru</highlight><highlight class="normal"></highlight></codeline>
271<codeline lineno="128"><highlight class="normal"><sp/><sp/>vec&amp;<sp/>mu<sp/>=<sp/>E.<ref refid="classenorm_10b8cb284e5af920a1b64a21d057ec5ac" kindref="member" tooltip="returns a pointer to the internal mean value. Use with Care!">_mu</ref>();</highlight></codeline>
272<codeline lineno="129"><highlight class="normal"><sp/><sp/>(mu)(0)=zprevod(x_est[0],Qm)*Iref;</highlight></codeline>
273<codeline lineno="130"><highlight class="normal"><sp/><sp/>(mu)(1)=zprevod(x_est[1],Qm)*Iref;</highlight></codeline>
274<codeline lineno="131"><highlight class="normal"><sp/><sp/>(mu)(2)=zprevod(x_est[2],Qm)*Wref;</highlight></codeline>
275<codeline lineno="132"><highlight class="normal"><sp/><sp/>(mu)(3)=zprevod(x_est[3],15)*Thetaref;</highlight></codeline>
276<codeline lineno="133"><highlight class="normal">}</highlight></codeline>
277<codeline lineno="134"><highlight class="normal"></highlight></codeline>
278<codeline lineno="135"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>EKFfixed::init_ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Tv)</highlight></codeline>
279<codeline lineno="136"><highlight class="normal">{</highlight></codeline>
280<codeline lineno="137"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>Q</highlight><highlight class="normal"></highlight></codeline>
281<codeline lineno="138"><highlight class="normal"><sp/><sp/>Q[0]=prevod(.01,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>0.05</highlight><highlight class="normal"></highlight></codeline>
282<codeline lineno="139"><highlight class="normal"><sp/><sp/>Q[5]=Q[0];</highlight></codeline>
283<codeline lineno="140"><highlight class="normal"><sp/><sp/>Q[10]=prevod(0.0001,15);<sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>1e-3</highlight><highlight class="normal"></highlight></codeline>
284<codeline lineno="141"><highlight class="normal"><sp/><sp/>Q[15]=prevod(0.0001,15);<sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>1e-3</highlight><highlight class="normal"></highlight></codeline>
285<codeline lineno="142"><highlight class="normal"></highlight></codeline>
286<codeline lineno="143"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>R</highlight><highlight class="normal"></highlight></codeline>
287<codeline lineno="144"><highlight class="normal"><sp/><sp/>R[0]=prevod(0.05,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>0.05</highlight><highlight class="normal"></highlight></codeline>
288<codeline lineno="145"><highlight class="normal"><sp/><sp/>R[3]=R[0];</highlight></codeline>
289<codeline lineno="146"><highlight class="normal"></highlight></codeline>
290<codeline lineno="147"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Motor<sp/>model<sp/>parameters<sp/></highlight><highlight class="normal"></highlight></codeline>
291<codeline lineno="148"><highlight class="normal"><sp/><sp/>cA=prevod(1-Tv*Rs/Ls,15);</highlight></codeline>
292<codeline lineno="149"><highlight class="normal"><sp/><sp/>cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);</highlight></codeline>
293<codeline lineno="150"><highlight class="normal"><sp/><sp/>cC=prevod(Tv/Ls/Iref*Uref,15);</highlight></codeline>
294<codeline lineno="151"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cD=prevod(1-Tv*Bf/J,15);</highlight><highlight class="normal"></highlight></codeline>
295<codeline lineno="152"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);</highlight><highlight class="normal"></highlight></codeline>
296<codeline lineno="153"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cF=prevod(p*Tv*Mref/J/Wref,15);</highlight><highlight class="normal"></highlight></codeline>
297<codeline lineno="154"><highlight class="normal"><sp/><sp/>cG=prevod(Tv*Wref*4/Thetaref,15);</highlight></codeline>
298<codeline lineno="155"><highlight class="normal"><sp/><sp/>cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);</highlight></codeline>
299<codeline lineno="156"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);</highlight><highlight class="normal"></highlight></codeline>
300<codeline lineno="157"><highlight class="normal"></highlight></codeline>
301<codeline lineno="158"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>Init<sp/>matrix<sp/>PSI<sp/>with<sp/>permanently<sp/>constant<sp/>terms<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
302<codeline lineno="159"><highlight class="normal"><sp/><sp/>PSI[0]=cA;</highlight></codeline>
303<codeline lineno="160"><highlight class="normal"><sp/><sp/>PSI[5]=PSI[0];</highlight></codeline>
304<codeline lineno="161"><highlight class="normal"><sp/><sp/>PSI[10]=0x7FFF;</highlight></codeline>
305<codeline lineno="162"><highlight class="normal"><sp/><sp/>PSI[14]=cG;</highlight></codeline>
306<codeline lineno="163"><highlight class="normal"><sp/><sp/>PSI[15]=0x7FFF;</highlight></codeline>
307<codeline lineno="164"><highlight class="normal">}</highlight></codeline>
308    </programlisting>
309    <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp"/>
310  </compounddef>
311</doxygen>
Note: See TracBrowser for help on using the browser.