root/doc/xml/ekf_8cpp.xml @ 259

Revision 187, 59.9 kB (checked in by smidl, 16 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_8cpp" kind="file">
4    <compoundname>ekf.cpp</compoundname>
5    <includes local="no">math.h</includes>
6    <includes refid="fixed_8h" local="yes">fixed.h</includes>
7    <includes refid="reference_8h" local="yes">reference.h</includes>
8    <includes refid="matrix_8h" local="yes">matrix.h</includes>
9    <includes refid="ekf_8h" local="yes">ekf.h</includes>
10    <incdepgraph>
11      <node id="717">
12        <label>ekf.h</label>
13        <link refid="ekf_8h-source"/>
14      </node>
15      <node id="712">
16        <label>work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp</label>
17        <link refid="ekf.cpp"/>
18        <childnode refid="713" relation="include">
19        </childnode>
20        <childnode refid="714" relation="include">
21        </childnode>
22        <childnode refid="715" relation="include">
23        </childnode>
24        <childnode refid="716" relation="include">
25        </childnode>
26        <childnode refid="717" relation="include">
27        </childnode>
28      </node>
29      <node id="713">
30        <label>math.h</label>
31      </node>
32      <node id="715">
33        <label>reference.h</label>
34        <link refid="reference_8h-source"/>
35      </node>
36      <node id="714">
37        <label>fixed.h</label>
38        <link refid="fixed_8h-source"/>
39      </node>
40      <node id="716">
41        <label>matrix.h</label>
42        <link refid="matrix_8h-source"/>
43      </node>
44    </incdepgraph>
45      <sectiondef kind="define">
46      <memberdef kind="define" id="ekf_8cpp_105679eb18ff6f0bd4ba53411a0fe0afb" prot="public" static="no">
47        <name>K_PREVOD_FORM</name>
48        <initializer>4</initializer>
49        <briefdescription>
50        </briefdescription>
51        <detaileddescription>
52        </detaileddescription>
53        <inbodydescription>
54        </inbodydescription>
55        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="24" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="24" bodyend="-1"/>
56      </memberdef>
57      </sectiondef>
58      <sectiondef kind="var">
59      <memberdef kind="variable" id="ekf_8cpp_1415bdef5e664918e4e4b773c36f4e103" prot="public" static="no" mutable="no">
60        <type>double</type>
61        <definition>double ladeni_EKF[10]</definition>
62        <argsstring>[10]</argsstring>
63        <name>ladeni_EKF</name>
64        <briefdescription>
65        </briefdescription>
66        <detaileddescription>
67        </detaileddescription>
68        <inbodydescription>
69        </inbodydescription>
70        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="36" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="36" bodyend="-1"/>
71      </memberdef>
72      <memberdef kind="variable" id="ekf_8cpp_1f66eddc642fe2a42f062f5cd834102a5" prot="public" static="yes" mutable="no">
73        <type>int</type>
74        <definition>int H[8]</definition>
75        <argsstring>[8]</argsstring>
76        <name>H</name>
77        <initializer>{0x7FFF,0,0,0,0,0x7FFF,0,0}</initializer>
78        <briefdescription>
79        </briefdescription>
80        <detaileddescription>
81        </detaileddescription>
82        <inbodydescription>
83        </inbodydescription>
84        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="39" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="39" bodyend="-1"/>
85      </memberdef>
86      <memberdef kind="variable" id="ekf_8cpp_1f5a21f73c9dd58ca98b25b4525fedb11" prot="public" static="yes" mutable="no">
87        <type>int</type>
88        <definition>int Ht[8]</definition>
89        <argsstring>[8]</argsstring>
90        <name>Ht</name>
91        <initializer>{0x7FFF,0,0,0x7FFF,0,0,0,0}</initializer>
92        <briefdescription>
93        </briefdescription>
94        <detaileddescription>
95        </detaileddescription>
96        <inbodydescription>
97        </inbodydescription>
98        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="40" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="40" bodyend="-1"/>
99      </memberdef>
100      <memberdef kind="variable" id="ekf_8cpp_1893628019411e1d74b2ca67f76d647d9" prot="public" static="yes" mutable="no">
101        <type>int</type>
102        <definition>int Q[16]</definition>
103        <argsstring>[16]</argsstring>
104        <name>Q</name>
105        <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer>
106        <briefdescription>
107        </briefdescription>
108        <detaileddescription>
109        </detaileddescription>
110        <inbodydescription>
111        </inbodydescription>
112        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="43" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="43" bodyend="-1"/>
113      </memberdef>
114      <memberdef kind="variable" id="ekf_8cpp_167b63ae29fb23d1a034f9ba7e0787745" prot="public" static="yes" mutable="no">
115        <type>int</type>
116        <definition>int R[4]</definition>
117        <argsstring>[4]</argsstring>
118        <name>R</name>
119        <initializer>{0,0,0,0}</initializer>
120        <briefdescription>
121        </briefdescription>
122        <detaileddescription>
123        </detaileddescription>
124        <inbodydescription>
125        </inbodydescription>
126        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="44" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="44" bodyend="-1"/>
127      </memberdef>
128      <memberdef kind="variable" id="ekf_8cpp_1db2f2b0f082a8ce53b3e61a9ce7e6c94" prot="public" static="yes" mutable="no">
129        <type>int</type>
130        <definition>int x_est[4]</definition>
131        <argsstring>[4]</argsstring>
132        <name>x_est</name>
133        <initializer>{0,0,0,0}</initializer>
134        <briefdescription>
135        </briefdescription>
136        <detaileddescription>
137        </detaileddescription>
138        <inbodydescription>
139        </inbodydescription>
140        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="47" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="47" bodyend="-1"/>
141      </memberdef>
142      <memberdef kind="variable" id="ekf_8cpp_1b5b26d093bdcad628aef9156a376174f" prot="public" static="yes" mutable="no">
143        <type>int</type>
144        <definition>int x_pred[4]</definition>
145        <argsstring>[4]</argsstring>
146        <name>x_pred</name>
147        <initializer>{0,0,0,0}</initializer>
148        <briefdescription>
149        </briefdescription>
150        <detaileddescription>
151        </detaileddescription>
152        <inbodydescription>
153        </inbodydescription>
154        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="48" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="48" bodyend="-1"/>
155      </memberdef>
156      <memberdef kind="variable" id="ekf_8cpp_1683087dfe47600d1a450302a2ccbb836" prot="public" static="yes" mutable="no">
157        <type>int</type>
158        <definition>int P_pred[16]</definition>
159        <argsstring>[16]</argsstring>
160        <name>P_pred</name>
161        <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer>
162        <briefdescription>
163        </briefdescription>
164        <detaileddescription>
165        </detaileddescription>
166        <inbodydescription>
167        </inbodydescription>
168        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="49" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="49" bodyend="-1"/>
169      </memberdef>
170      <memberdef kind="variable" id="ekf_8cpp_198aa9c8e8546c3990a668fa2376bd02f" prot="public" static="yes" mutable="no">
171        <type>int</type>
172        <definition>int P_est[16]</definition>
173        <argsstring>[16]</argsstring>
174        <name>P_est</name>
175        <initializer>{0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF}</initializer>
176        <briefdescription>
177        </briefdescription>
178        <detaileddescription>
179        </detaileddescription>
180        <inbodydescription>
181        </inbodydescription>
182        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="51" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="51" bodyend="-1"/>
183      </memberdef>
184      <memberdef kind="variable" id="ekf_8cpp_1af019cae3f3f03d594a8f549353c99e9" prot="public" static="yes" mutable="no">
185        <type>int</type>
186        <definition>int Y_mes[2]</definition>
187        <argsstring>[2]</argsstring>
188        <name>Y_mes</name>
189        <initializer>{0,0}</initializer>
190        <briefdescription>
191        </briefdescription>
192        <detaileddescription>
193        </detaileddescription>
194        <inbodydescription>
195        </inbodydescription>
196        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="52" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="52" bodyend="-1"/>
197      </memberdef>
198      <memberdef kind="variable" id="ekf_8cpp_16cccd275c0b11a7fac94937f2021045b" prot="public" static="yes" mutable="no">
199        <type>int</type>
200        <definition>int ukalm[2]</definition>
201        <argsstring>[2]</argsstring>
202        <name>ukalm</name>
203        <initializer>{0,0}</initializer>
204        <briefdescription>
205        </briefdescription>
206        <detaileddescription>
207        </detaileddescription>
208        <inbodydescription>
209        </inbodydescription>
210        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="53" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="53" bodyend="-1"/>
211      </memberdef>
212      <memberdef kind="variable" id="ekf_8cpp_1f79d2220030336cad022d159b3f8c8bb" prot="public" static="yes" mutable="no">
213        <type>int</type>
214        <definition>int Kalm[8]</definition>
215        <argsstring>[8]</argsstring>
216        <name>Kalm</name>
217        <briefdescription>
218        </briefdescription>
219        <detaileddescription>
220        </detaileddescription>
221        <inbodydescription>
222        </inbodydescription>
223        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="54" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="54" bodyend="-1"/>
224      </memberdef>
225      <memberdef kind="variable" id="ekf_8cpp_10db030b4590561a5e0a5d70223a860dd" prot="public" static="yes" mutable="no">
226        <type>int</type>
227        <definition>int PSI[16]</definition>
228        <argsstring>[16]</argsstring>
229        <name>PSI</name>
230        <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer>
231        <briefdescription>
232        </briefdescription>
233        <detaileddescription>
234        </detaileddescription>
235        <inbodydescription>
236        </inbodydescription>
237        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="56" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="56" bodyend="-1"/>
238      </memberdef>
239      <memberdef kind="variable" id="ekf_8cpp_17a8fbda3ee1da60e70499a79b4d3e3df" prot="public" static="yes" mutable="no">
240        <type>int</type>
241        <definition>int temp15a[16]</definition>
242        <argsstring>[16]</argsstring>
243        <name>temp15a</name>
244        <briefdescription>
245        </briefdescription>
246        <detaileddescription>
247        </detaileddescription>
248        <inbodydescription>
249        </inbodydescription>
250        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="59" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="59" bodyend="-1"/>
251      </memberdef>
252      <memberdef kind="variable" id="ekf_8cpp_1f82c96cc9810ce86d1edec2fbf1deb6d" prot="public" static="yes" mutable="no">
253        <type>int</type>
254        <definition>int cA</definition>
255        <argsstring></argsstring>
256        <name>cA</name>
257        <briefdescription>
258        </briefdescription>
259        <detaileddescription>
260        </detaileddescription>
261        <inbodydescription>
262        </inbodydescription>
263        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/>
264      </memberdef>
265      <memberdef kind="variable" id="ekf_8cpp_1026fd8d02ce266d7a9f9bb1e233f6242" prot="public" static="yes" mutable="no">
266        <type>int</type>
267        <definition>int cB</definition>
268        <argsstring></argsstring>
269        <name>cB</name>
270        <briefdescription>
271        </briefdescription>
272        <detaileddescription>
273        </detaileddescription>
274        <inbodydescription>
275        </inbodydescription>
276        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/>
277      </memberdef>
278      <memberdef kind="variable" id="ekf_8cpp_1da52f4f6674e0f993ee24e61553e2eae" prot="public" static="yes" mutable="no">
279        <type>int</type>
280        <definition>int cC</definition>
281        <argsstring></argsstring>
282        <name>cC</name>
283        <briefdescription>
284        </briefdescription>
285        <detaileddescription>
286        </detaileddescription>
287        <inbodydescription>
288        </inbodydescription>
289        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/>
290      </memberdef>
291      <memberdef kind="variable" id="ekf_8cpp_13a4546b83055800c4658329cb2ca0b4e" prot="public" static="yes" mutable="no">
292        <type>int</type>
293        <definition>int cG</definition>
294        <argsstring></argsstring>
295        <name>cG</name>
296        <briefdescription>
297        </briefdescription>
298        <detaileddescription>
299        </detaileddescription>
300        <inbodydescription>
301        </inbodydescription>
302        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/>
303      </memberdef>
304      <memberdef kind="variable" id="ekf_8cpp_119c038d1a31ea4257f137481c835ea96" prot="public" static="yes" mutable="no">
305        <type>int</type>
306        <definition>int cH</definition>
307        <argsstring></argsstring>
308        <name>cH</name>
309        <briefdescription>
310        </briefdescription>
311        <detaileddescription>
312        </detaileddescription>
313        <inbodydescription>
314        </inbodydescription>
315        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/>
316      </memberdef>
317      </sectiondef>
318      <sectiondef kind="func">
319      <memberdef kind="function" id="ekf_8cpp_1956dd7a9713816704f8ff5760e26e903" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
320        <type>void</type>
321        <definition>void init_ekf</definition>
322        <argsstring>(double Tv, double *param)</argsstring>
323        <name>init_ekf</name>
324        <param>
325          <type>double</type>
326          <declname>Tv</declname>
327        </param>
328        <param>
329          <type>double *</type>
330          <declname>param</declname>
331        </param>
332        <briefdescription>
333        </briefdescription>
334        <detaileddescription>
335        </detaileddescription>
336        <inbodydescription>
337<para>KONEC inicializace pro PC - az sem vyhodit v DSP /// </para>        </inbodydescription>
338        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="27" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="65" bodyend="125"/>
339      </memberdef>
340      <memberdef kind="function" id="ekf_8cpp_1267ca49dd712a2b06af569b1e108976a" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
341        <type>void</type>
342        <definition>void ekf</definition>
343        <argsstring>(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd)</argsstring>
344        <name>ekf</name>
345        <param>
346          <type>double *</type>
347          <declname>x_estimation</declname>
348        </param>
349        <param>
350          <type>double</type>
351          <declname>Umd</declname>
352        </param>
353        <param>
354          <type>double</type>
355          <declname>beta</declname>
356        </param>
357        <param>
358          <type>double</type>
359          <declname>Ucnd</declname>
360        </param>
361        <param>
362          <type>double</type>
363          <declname>Ucd</declname>
364        </param>
365        <param>
366          <type>double</type>
367          <declname>isxd</declname>
368        </param>
369        <param>
370          <type>double</type>
371          <declname>isyd</declname>
372        </param>
373        <briefdescription>
374        </briefdescription>
375        <detaileddescription>
376        </detaileddescription>
377        <inbodydescription>
378<para>prechod ze simulace do fixed-pointu - vyradit v DSP ///////////</para><para>KONEC emulace FIXED-POINTu - az sem vyradit v DSP //////////// </para>        </inbodydescription>
379        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="28" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="216" bodyend="254"/>
380      </memberdef>
381      <memberdef kind="function" id="ekf_8cpp_1bdd51eee3a5eb0a70f3d1e13b5cf2697" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual">
382        <type>void</type>
383        <definition>void prediction</definition>
384        <argsstring>(int *ux)</argsstring>
385        <name>prediction</name>
386        <param>
387          <type>int *</type>
388          <declname>ux</declname>
389        </param>
390        <briefdescription>
391        </briefdescription>
392        <detaileddescription>
393        </detaileddescription>
394        <inbodydescription>
395        </inbodydescription>
396        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="31" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="148" bodyend="172"/>
397      </memberdef>
398      <memberdef kind="function" id="ekf_8cpp_188642818810b8018ecbb525cad1d5c6f" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual">
399        <type>void</type>
400        <definition>void correction</definition>
401        <argsstring>(void)</argsstring>
402        <name>correction</name>
403        <param>
404          <type>void</type>
405        </param>
406        <briefdescription>
407        </briefdescription>
408        <detaileddescription>
409        </detaileddescription>
410        <inbodydescription>
411        </inbodydescription>
412        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="32" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="174" bodyend="213"/>
413      </memberdef>
414      <memberdef kind="function" id="ekf_8cpp_195d9e77bdeaf5408a69d540731fd629f" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual">
415        <type>void</type>
416        <definition>static void update_psi</definition>
417        <argsstring>(void)</argsstring>
418        <name>update_psi</name>
419        <param>
420          <type>void</type>
421        </param>
422        <briefdescription>
423        </briefdescription>
424        <detaileddescription>
425        </detaileddescription>
426        <inbodydescription>
427        </inbodydescription>
428        <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="33" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="128" bodyend="145"/>
429      </memberdef>
430      </sectiondef>
431    <briefdescription>
432    </briefdescription>
433    <detaileddescription>
434    </detaileddescription>
435    <programlisting>
436<codeline lineno="1"><highlight class="comment">/************************************</highlight></codeline>
437<codeline lineno="2"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Extended<sp/>Kalman<sp/>Filter</highlight></codeline>
438<codeline lineno="3"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Kalman<sp/>Observer</highlight></codeline>
439<codeline lineno="4"><highlight class="comment"></highlight></codeline>
440<codeline lineno="5"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Z.<sp/>Peroutka</highlight></codeline>
441<codeline lineno="6"><highlight class="comment"></highlight></codeline>
442<codeline lineno="7"><highlight class="comment">Rev.<sp/>15.3.2008</highlight></codeline>
443<codeline lineno="8"><highlight class="comment"></highlight></codeline>
444<codeline lineno="9"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>EKF<sp/>pocitan<sp/>s<sp/>daty<sp/>ve<sp/>formatu<sp/>Q15,<sp/>zatimco<sp/>regulace<sp/>pracuje<sp/>v<sp/>Q13<sp/>-&gt;</highlight></codeline>
445<codeline lineno="10"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>resenim<sp/>je<sp/>vynasobit<sp/>referencni<sp/>hodnoty<sp/>v<sp/>EKF<sp/>4-mi<sp/>(tim<sp/>je<sp/>automaticky</highlight></codeline>
446<codeline lineno="11"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>zajisten<sp/>prechod<sp/>mezi<sp/>formaty<sp/>Q13<sp/>a<sp/>Q15).<sp/>Realizovano<sp/>konstantou<sp/>K_PREVOD_FORM.</highlight></codeline>
447<codeline lineno="12"><highlight class="comment"></highlight></codeline>
448<codeline lineno="13"><highlight class="comment">15.3.2008<sp/><sp/><sp/><sp/><sp/><sp/><sp/>Kontrola<sp/>kodu<sp/>+<sp/>zamena<sp/>datovych<sp/>typu<sp/>q15-&gt;int<sp/>a<sp/>q30-&gt;long.</highlight></codeline>
449<codeline lineno="14"><highlight class="comment"></highlight></codeline>
450<codeline lineno="15"><highlight class="comment">*************************************/</highlight><highlight class="normal"></highlight></codeline>
451<codeline lineno="16"><highlight class="normal"></highlight></codeline>
452<codeline lineno="17"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;math.h&gt;</highlight><highlight class="normal"></highlight></codeline>
453<codeline lineno="18"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;fixed.h&quot;</highlight><highlight class="normal"></highlight></codeline>
454<codeline lineno="19"><highlight class="normal"></highlight><highlight class="comment">//#include<sp/>&quot;parametry_motoru.h&quot;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>aktivovat<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline>
455<codeline lineno="20"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;reference.h&quot;</highlight><highlight class="normal"></highlight></codeline>
456<codeline lineno="21"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;matrix.h&quot;</highlight><highlight class="normal"></highlight></codeline>
457<codeline lineno="22"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;ekf.h&quot;</highlight><highlight class="normal"></highlight></codeline>
458<codeline lineno="23"><highlight class="normal"></highlight></codeline>
459<codeline lineno="24"><highlight class="normal"></highlight><highlight class="preprocessor">#define<sp/>K_PREVOD_FORM<sp/><sp/><sp/>4<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>2^(15-Qm)</highlight></codeline>
460<codeline lineno="25"><highlight class="preprocessor"></highlight><highlight class="normal"></highlight></codeline>
461<codeline lineno="26"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Declaration<sp/>of<sp/>global<sp/>functions<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
462<codeline lineno="27"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>init_ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Tv,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*param);</highlight></codeline>
463<codeline lineno="28"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*x_estimation,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Umd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>beta,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucnd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucd,<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>
464<codeline lineno="29"><highlight class="normal"></highlight></codeline>
465<codeline lineno="30"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Declaration<sp/>of<sp/>local<sp/>functions<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
466<codeline lineno="31"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>prediction(</highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>*ux);</highlight></codeline>
467<codeline lineno="32"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>correction(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">);</highlight></codeline>
468<codeline lineno="33"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>update_psi(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">);</highlight></codeline>
469<codeline lineno="34"><highlight class="normal"></highlight></codeline>
470<codeline lineno="35"><highlight class="normal"></highlight><highlight class="comment">//<sp/>global<sp/>variables</highlight><highlight class="normal"></highlight></codeline>
471<codeline lineno="36"><highlight class="normal"></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>ladeni_EKF[10];</highlight></codeline>
472<codeline lineno="37"><highlight class="normal"></highlight></codeline>
473<codeline lineno="38"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Constants<sp/>-<sp/>definovat<sp/>jako<sp/>konstanty<sp/>??<sp/>?kde<sp/>je<sp/>vyhodnejsi<sp/>aby<sp/>v<sp/>pameti<sp/>byli?*/</highlight><highlight class="normal"></highlight></codeline>
474<codeline lineno="39"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>H[8]={0x7FFF,0,0,0,0,0x7FFF,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[2,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
475<codeline lineno="40"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Ht[8]={0x7FFF,0,0,0x7FFF,0,0,0,0};</highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
476<codeline lineno="41"><highlight class="normal"></highlight><highlight class="comment">//static<sp/>int<sp/>Q[4][4]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/>/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
477<codeline lineno="42"><highlight class="normal"></highlight><highlight class="comment">//static<sp/>int<sp/>R[2][2]={0,0,0,0};<sp/>/*<sp/>matrix<sp/>[2,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
478<codeline lineno="43"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Q[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
479<codeline lineno="44"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>R[4]={0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[2,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
480<codeline lineno="45"><highlight class="normal"></highlight></codeline>
481<codeline lineno="46"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Initial<sp/>conditions<sp/>and<sp/>variables<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
482<codeline lineno="47"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>x_est[4]={0,0,0,0};</highlight></codeline>
483<codeline lineno="48"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>x_pred[4]={0,0,0,0};</highlight></codeline>
484<codeline lineno="49"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>P_pred[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
485<codeline lineno="50"><highlight class="normal"></highlight><highlight class="comment">//static<sp/>int<sp/>P_est[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/>/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
486<codeline lineno="51"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>P_est[16]={0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
487<codeline lineno="52"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Y_mes[2]={0,0};</highlight></codeline>
488<codeline lineno="53"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>ukalm[2]={0,0};</highlight></codeline>
489<codeline lineno="54"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Kalm[8];<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[5,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
490<codeline lineno="55"><highlight class="normal"></highlight></codeline>
491<codeline lineno="56"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>PSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
492<codeline lineno="57"><highlight class="normal"></highlight></codeline>
493<codeline lineno="58"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Temporary<sp/>variables<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
494<codeline lineno="59"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>temp15a[16];</highlight></codeline>
495<codeline lineno="60"><highlight class="normal"></highlight></codeline>
496<codeline lineno="61"><highlight class="normal"></highlight><highlight class="comment">//<sp/>constants<sp/>of<sp/>PMSM<sp/>mathematical<sp/>model</highlight><highlight class="normal"></highlight></codeline>
497<codeline lineno="62"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>cA,<sp/>cB,<sp/>cC,<sp/>cG,<sp/>cH;<sp/><sp/></highlight><highlight class="comment">//<sp/>cD,<sp/>cE,<sp/>cF,<sp/>cI<sp/>...<sp/>nepouzivane</highlight><highlight class="normal"></highlight></codeline>
498<codeline lineno="63"><highlight class="normal"></highlight></codeline>
499<codeline lineno="65"><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>init_ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Tv,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*param)</highlight></codeline>
500<codeline lineno="66"><highlight class="normal">{</highlight></codeline>
501<codeline lineno="67"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>parametry<sp/>simulace<sp/>-<sp/>v<sp/>DSP<sp/>vyhodit<sp/>(nahrazeno<sp/>&quot;parametry_motoru.h&quot;)</highlight><highlight class="normal"></highlight></codeline>
502<codeline lineno="68"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Rs,<sp/>Ls,<sp/>Fmag,<sp/>kp,<sp/>p;</highlight></codeline>
503<codeline lineno="69"><highlight class="normal"></highlight></codeline>
504<codeline lineno="70"><highlight class="normal"><sp/></highlight><highlight class="comment">//<sp/>param=[Rs,<sp/>Ls,<sp/>Fmag,<sp/>Bf,<sp/>p,<sp/>kp,<sp/>J<sp/>=<sp/>0.04?];</highlight><highlight class="normal"></highlight></codeline>
505<codeline lineno="71"><highlight class="normal"><sp/><sp/>Rs=*param;</highlight></codeline>
506<codeline lineno="72"><highlight class="normal"><sp/><sp/>Ls=*(param+1);</highlight></codeline>
507<codeline lineno="73"><highlight class="normal"><sp/><sp/>Fmag=*(param+2);</highlight></codeline>
508<codeline lineno="74"><highlight class="normal"><sp/><sp/>p=*(param+4);</highlight></codeline>
509<codeline lineno="75"><highlight class="normal"><sp/><sp/>kp=*(param+5);</highlight></codeline>
510<codeline lineno="77"><highlight class="normal"></highlight></codeline>
511<codeline lineno="78"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>Q</highlight><highlight class="normal"></highlight></codeline>
512<codeline lineno="79"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>Q[0][0]=prevod(.05,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>1e-2</highlight></codeline>
513<codeline lineno="80"><highlight class="comment"><sp/><sp/>Q[1][1]=Q[0][0];</highlight></codeline>
514<codeline lineno="81"><highlight class="comment"><sp/><sp/>Q[2][2]=prevod(1e-3,15);<sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>10e-5</highlight></codeline>
515<codeline lineno="82"><highlight class="comment"><sp/><sp/>Q[3][3]=prevod(1e-3,15);<sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>model(ureg)<sp/>...<sp/>4e-3</highlight></codeline>
516<codeline lineno="83"><highlight class="comment"></highlight></codeline>
517<codeline lineno="84"><highlight class="comment"><sp/><sp/>//<sp/>Tuning<sp/>of<sp/>matrix<sp/>R</highlight></codeline>
518<codeline lineno="85"><highlight class="comment"><sp/><sp/>R[0][0]=prevod(0.05,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>model(ureg)<sp/>...<sp/>0.1</highlight></codeline>
519<codeline lineno="86"><highlight class="comment"><sp/><sp/>R[1][1]=R[0][0];</highlight></codeline>
520<codeline lineno="87"><highlight class="comment">/**/</highlight><highlight class="normal"></highlight></codeline>
521<codeline lineno="88"><highlight class="normal"></highlight></codeline>
522<codeline lineno="89"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>Q</highlight><highlight class="normal"></highlight></codeline>
523<codeline lineno="90"><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>
524<codeline lineno="91"><highlight class="normal"><sp/><sp/>Q[5]=Q[0];</highlight></codeline>
525<codeline lineno="92"><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>
526<codeline lineno="93"><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>
527<codeline lineno="94"><highlight class="normal"></highlight></codeline>
528<codeline lineno="95"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>R</highlight><highlight class="normal"></highlight></codeline>
529<codeline lineno="96"><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>
530<codeline lineno="97"><highlight class="normal"><sp/><sp/>R[3]=R[0];</highlight></codeline>
531<codeline lineno="98"><highlight class="normal"></highlight></codeline>
532<codeline lineno="99"><highlight class="normal"></highlight></codeline>
533<codeline lineno="100"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>Tuning<sp/>of<sp/>matrix<sp/>P_est<sp/>-<sp/>initial<sp/>values<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
534<codeline lineno="101"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/><sp/><sp/>P_est[0][0]=prevod(0.999,15);</highlight></codeline>
535<codeline lineno="102"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[1][1]=prevod(0.999,15);</highlight></codeline>
536<codeline lineno="103"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[2][2]=prevod(0.999,15);</highlight></codeline>
537<codeline lineno="104"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[3][3]=prevod(0.999,15);</highlight></codeline>
538<codeline lineno="105"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[4][4]=prevod(0.999,15);</highlight></codeline>
539<codeline lineno="106"><highlight class="comment">/**/</highlight><highlight class="normal"></highlight></codeline>
540<codeline lineno="107"><highlight class="normal"></highlight></codeline>
541<codeline lineno="108"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Motor<sp/>model<sp/>parameters<sp/></highlight><highlight class="normal"></highlight></codeline>
542<codeline lineno="109"><highlight class="normal"><sp/><sp/>cA=prevod(1-Tv*Rs/Ls,15);</highlight></codeline>
543<codeline lineno="110"><highlight class="normal"><sp/><sp/>cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);</highlight></codeline>
544<codeline lineno="111"><highlight class="normal"><sp/><sp/>cC=prevod(Tv/Ls/Iref*Uref,15);</highlight></codeline>
545<codeline lineno="112"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cD=prevod(1-Tv*Bf/J,15);</highlight><highlight class="normal"></highlight></codeline>
546<codeline lineno="113"><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>
547<codeline lineno="114"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cF=prevod(p*Tv*Mref/J/Wref,15);</highlight><highlight class="normal"></highlight></codeline>
548<codeline lineno="115"><highlight class="normal"><sp/><sp/>cG=prevod(Tv*Wref*K_PREVOD_FORM/Thetaref,15);</highlight></codeline>
549<codeline lineno="116"><highlight class="normal"><sp/><sp/>cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);</highlight></codeline>
550<codeline lineno="117"><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>
551<codeline lineno="118"><highlight class="normal"></highlight></codeline>
552<codeline lineno="119"><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>
553<codeline lineno="120"><highlight class="normal"><sp/><sp/>PSI[0]=cA;</highlight></codeline>
554<codeline lineno="121"><highlight class="normal"><sp/><sp/>PSI[5]=PSI[0];</highlight></codeline>
555<codeline lineno="122"><highlight class="normal"><sp/><sp/>PSI[10]=0x7FFF;</highlight></codeline>
556<codeline lineno="123"><highlight class="normal"><sp/><sp/>PSI[14]=cG;</highlight></codeline>
557<codeline lineno="124"><highlight class="normal"><sp/><sp/>PSI[15]=0x7FFF;</highlight></codeline>
558<codeline lineno="125"><highlight class="normal">}</highlight></codeline>
559<codeline lineno="126"><highlight class="normal"></highlight></codeline>
560<codeline lineno="127"><highlight class="normal"></highlight></codeline>
561<codeline lineno="128"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>update_psi(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">)</highlight></codeline>
562<codeline lineno="129"><highlight class="normal">{</highlight></codeline>
563<codeline lineno="130"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t_sin,t_cos,tmp;</highlight></codeline>
564<codeline lineno="131"><highlight class="normal"></highlight></codeline>
565<codeline lineno="132"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline>
566<codeline lineno="133"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
567<codeline lineno="134"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
568<codeline lineno="135"><highlight class="normal"></highlight></codeline>
569<codeline lineno="136"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline>
570<codeline lineno="137"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_sin=qsin(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
571<codeline lineno="138"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_cos=qcos(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
572<codeline lineno="139"><highlight class="normal"></highlight></codeline>
573<codeline lineno="140"><highlight class="normal"><sp/><sp/>PSI[2]=((long)cB*t_sin)&gt;&gt;15;</highlight></codeline>
574<codeline lineno="141"><highlight class="normal"><sp/><sp/>tmp=((long)cH*x_est[2])&gt;&gt;15;</highlight></codeline>
575<codeline lineno="142"><highlight class="normal"><sp/><sp/>PSI[3]=((long)tmp*t_cos)&gt;&gt;15;</highlight></codeline>
576<codeline lineno="143"><highlight class="normal"><sp/><sp/>PSI[6]=-((long)cB*t_cos)&gt;&gt;15;</highlight></codeline>
577<codeline lineno="144"><highlight class="normal"><sp/><sp/>PSI[7]=((long)tmp*t_sin)&gt;&gt;15;</highlight></codeline>
578<codeline lineno="145"><highlight class="normal">}</highlight></codeline>
579<codeline lineno="146"><highlight class="normal"></highlight></codeline>
580<codeline lineno="147"><highlight class="normal"></highlight></codeline>
581<codeline lineno="148"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>prediction(</highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>*ux)</highlight></codeline>
582<codeline lineno="149"><highlight class="normal">{</highlight></codeline>
583<codeline lineno="150"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t_sin,t_cos,<sp/>tmp;</highlight></codeline>
584<codeline lineno="151"><highlight class="normal"></highlight></codeline>
585<codeline lineno="152"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline>
586<codeline lineno="153"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
587<codeline lineno="154"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
588<codeline lineno="155"><highlight class="normal"></highlight></codeline>
589<codeline lineno="156"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline>
590<codeline lineno="157"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_sin=qsin(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
591<codeline lineno="158"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_cos=qcos(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
592<codeline lineno="159"><highlight class="normal"></highlight></codeline>
593<codeline lineno="160"><highlight class="normal"><sp/><sp/>tmp=((long)cB*x_est[2])&gt;&gt;15;</highlight></codeline>
594<codeline lineno="161"><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>
595<codeline lineno="162"><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>
596<codeline lineno="163"><highlight class="normal"><sp/><sp/>x_pred[2]=x_est[2];</highlight></codeline>
597<codeline lineno="164"><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>
598<codeline lineno="165"><highlight class="normal"></highlight></codeline>
599<codeline lineno="166"><highlight class="normal"><sp/><sp/>update_psi();</highlight></codeline>
600<codeline lineno="167"><highlight class="normal"></highlight></codeline>
601<codeline lineno="168"><highlight class="normal"><sp/><sp/>mmult(PSI,P_est,temp15a,3,3,3);</highlight></codeline>
602<codeline lineno="169"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mtrans(PSI,temp15b,5,5);</highlight><highlight class="normal"></highlight></codeline>
603<codeline lineno="170"><highlight class="normal"><sp/><sp/>mmultt(temp15a,PSI,P_pred,3,3,3);</highlight></codeline>
604<codeline lineno="171"><highlight class="normal"><sp/><sp/>maddD(P_pred,Q,3,3);</highlight></codeline>
605<codeline lineno="172"><highlight class="normal">}</highlight></codeline>
606<codeline lineno="173"><highlight class="normal"></highlight></codeline>
607<codeline lineno="174"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>correction(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">)</highlight></codeline>
608<codeline lineno="175"><highlight class="normal">{</highlight></codeline>
609<codeline lineno="176"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Y_error[2];</highlight></codeline>
610<codeline lineno="177"><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>
611<codeline lineno="178"><highlight class="normal"></highlight></codeline>
612<codeline lineno="179"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>Kalman<sp/>gain<sp/>calculation<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
613<codeline lineno="180"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mmult(P_pred,Ht,temp15a,5,5,2);</highlight><highlight class="normal"></highlight></codeline>
614<codeline lineno="181"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmultt(P_pred,H,temp15a,4,4,1);</highlight></codeline>
615<codeline lineno="182"><highlight class="comment"><sp/><sp/>mmult(H,temp15a,temp15b,1,4,1);<sp/><sp/><sp/><sp/><sp/><sp/><sp/>these<sp/>lines<sp/>are<sp/>replaced<sp/>by<sp/>choice_P<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
616<codeline lineno="183"><highlight class="normal"></highlight></codeline>
617<codeline lineno="184"><highlight class="normal"><sp/><sp/>choice_P(P_pred,temp15a,3);</highlight></codeline>
618<codeline lineno="185"><highlight class="normal"><sp/><sp/>maddD(temp15a,R,1,1);</highlight></codeline>
619<codeline lineno="186"><highlight class="normal"><sp/><sp/>minv2(temp15a,temp30a);</highlight></codeline>
620<codeline lineno="187"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmultt(P_pred,H,temp15a,4,4,1);<sp/><sp/><sp/><sp/><sp/>/*<sp/>remove<sp/>this<sp/>line<sp/>if<sp/>choice_P<sp/>is<sp/>not<sp/>used<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
621<codeline lineno="188"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mmultDr15(P_pred,Ht,temp15a,4,4,1,1);</highlight><highlight class="normal"></highlight></codeline>
622<codeline lineno="189"><highlight class="normal"><sp/><sp/>mmultDr(P_pred,temp15a,3,3,1,1);</highlight></codeline>
623<codeline lineno="190"><highlight class="normal"><sp/><sp/>mmult1530(temp15a,temp30a,Kalm,3,1,1);</highlight></codeline>
624<codeline lineno="191"><highlight class="normal"></highlight></codeline>
625<codeline lineno="192"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>estimate<sp/>the<sp/>state<sp/>system<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
626<codeline lineno="193"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mmult(H,x_pred,temp15a,1,4,0);</highlight><highlight class="normal"></highlight></codeline>
627<codeline lineno="194"><highlight class="normal"><sp/><sp/>choice_x(x_pred,<sp/>temp15a);</highlight></codeline>
628<codeline lineno="195"><highlight class="normal"><sp/><sp/>msub(Y_mes,temp15a,Y_error,1,0);</highlight></codeline>
629<codeline lineno="196"><highlight class="normal"><sp/><sp/>mmult(Kalm,Y_error,temp15a,3,1,0);</highlight></codeline>
630<codeline lineno="197"><highlight class="normal"><sp/><sp/>madd(x_pred,temp15a,x_est,3,0);</highlight></codeline>
631<codeline lineno="198"><highlight class="normal"></highlight></codeline>
632<codeline lineno="199"><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>
633<codeline lineno="200"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmult(Kalm,H,temp15a,4,1,4);</highlight></codeline>
634<codeline lineno="201"><highlight class="comment"><sp/><sp/>mmult(temp15a,P_pred,P_est,4,4,4);</highlight></codeline>
635<codeline lineno="202"><highlight class="comment"><sp/><sp/>msub(P_pred,P_est,P_est,4,4);</highlight></codeline>
636<codeline lineno="203"><highlight class="comment">/*<sp/>END<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
637<codeline lineno="204"><highlight class="normal"></highlight></codeline>
638<codeline lineno="205"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Version<sp/>with<sp/>MMULTDL()<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
639<codeline lineno="206"><highlight class="normal"><sp/><sp/>mmultDl(P_pred,temp15a,1,3,3,1);</highlight></codeline>
640<codeline lineno="207"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>if<sp/>result<sp/>matrix<sp/>has<sp/>more<sp/>terms<sp/>than<sp/>DIAG<sp/>matrix,it<sp/>is<sp/>necessary<sp/>to<sp/>enable</highlight></codeline>
641<codeline lineno="208"><highlight class="comment">erase<sp/>sequence<sp/>in<sp/>definition<sp/>of<sp/>function<sp/>MMULTDL()<sp/>that<sp/>is<sp/>currently<sp/>disabled.<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
642<codeline lineno="209"><highlight class="normal"></highlight></codeline>
643<codeline lineno="210"><highlight class="normal"><sp/><sp/>mmult(Kalm,temp15a,P_est,3,1,3);</highlight></codeline>
644<codeline lineno="211"><highlight class="normal"><sp/><sp/>msub(P_pred,P_est,P_est,3,3);</highlight></codeline>
645<codeline lineno="212"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>END<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
646<codeline lineno="213"><highlight class="normal">}</highlight></codeline>
647<codeline lineno="214"><highlight class="normal"></highlight></codeline>
648<codeline lineno="215"><highlight class="normal"></highlight></codeline>
649<codeline lineno="216"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*x_estimation,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Umd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>beta,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucnd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucd,<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>
650<codeline lineno="217"><highlight class="normal">{</highlight></codeline>
651<codeline lineno="218"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Umk,<sp/>ua,<sp/>ub;</highlight></codeline>
652<codeline lineno="219"><highlight class="normal"></highlight></codeline>
653<codeline lineno="221"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Um,<sp/>Ucn,<sp/>Uc,<sp/>isx,<sp/>isy,<sp/>t_cos;</highlight></codeline>
654<codeline lineno="222"><highlight class="normal"></highlight></codeline>
655<codeline lineno="223"><highlight class="normal"><sp/><sp/>Um=prevod(Umd/Uref,Qm);</highlight></codeline>
656<codeline lineno="224"><highlight class="normal"><sp/><sp/>Ucn=prevod(Ucnd/Uref,Qm);</highlight></codeline>
657<codeline lineno="225"><highlight class="normal"><sp/><sp/>Uc=prevod(Ucd/Uref,Qm);</highlight></codeline>
658<codeline lineno="226"><highlight class="normal"><sp/><sp/>isx=prevod(isxd/Iref,Qm);</highlight></codeline>
659<codeline lineno="227"><highlight class="normal"><sp/><sp/>isy=prevod(isyd/Iref,Qm);</highlight></codeline>
660<codeline lineno="229"><highlight class="normal"></highlight></codeline>
661<codeline lineno="230"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>prepocet<sp/>napeti<sp/>pro<sp/>EKF<sp/>dle<sp/>skutecneho<sp/>napeti<sp/>v<sp/>ss<sp/>obvodu</highlight><highlight class="normal"></highlight></codeline>
662<codeline lineno="231"><highlight class="normal"><sp/><sp/>Umk=((long)Um*Uc)/Ucn;</highlight></codeline>
663<codeline lineno="232"><highlight class="normal"></highlight></codeline>
664<codeline lineno="233"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>vypocet<sp/>fazovych<sp/>napeti<sp/>stridace</highlight><highlight class="normal"></highlight></codeline>
665<codeline lineno="234"><highlight class="normal"><sp/><sp/>ua=((long)Umk*prevod(cos(beta),15))&gt;&gt;15;</highlight></codeline>
666<codeline lineno="235"><highlight class="normal"><sp/><sp/>ub=((long)Umk*prevod(cos(beta-2./3.*M_PI),15))&gt;&gt;15;</highlight></codeline>
667<codeline lineno="236"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>uc=((long)Umk*prevod(cos(beta+2./3.*M_PI),15))&gt;&gt;15;</highlight><highlight class="normal"></highlight></codeline>
668<codeline lineno="237"><highlight class="normal"></highlight></codeline>
669<codeline lineno="238"><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>
670<codeline lineno="239"><highlight class="normal"><sp/><sp/>ukalm[0]=ua;</highlight></codeline>
671<codeline lineno="240"><highlight class="normal"><sp/><sp/>ukalm[1]=(((long)ua+ub&lt;&lt;1)*18917)&gt;&gt;15;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>usy=(ua+2*ub)/sqrt(3)</highlight><highlight class="normal"></highlight></codeline>
672<codeline lineno="241"><highlight class="normal"></highlight></codeline>
673<codeline lineno="242"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>zadani<sp/>mereni</highlight><highlight class="normal"></highlight></codeline>
674<codeline lineno="243"><highlight class="normal"><sp/><sp/>Y_mes[0]=isx;</highlight></codeline>
675<codeline lineno="244"><highlight class="normal"><sp/><sp/>Y_mes[1]=isy;</highlight></codeline>
676<codeline lineno="245"><highlight class="normal"></highlight></codeline>
677<codeline lineno="247"><highlight class="normal"><sp/><sp/>prediction(ukalm);</highlight></codeline>
678<codeline lineno="248"><highlight class="normal"><sp/><sp/>correction();</highlight></codeline>
679<codeline lineno="249"><highlight class="normal"></highlight></codeline>
680<codeline lineno="250"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>navrat<sp/>estimovanych<sp/>hodnot<sp/>regulatoru</highlight><highlight class="normal"></highlight></codeline>
681<codeline lineno="251"><highlight class="normal"><sp/><sp/>*x_estimation=zprevod(x_est[2],Qm)*Wref;</highlight></codeline>
682<codeline lineno="252"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>x_est[3]=(short<sp/>int)x_est[3];</highlight><highlight class="normal"></highlight></codeline>
683<codeline lineno="253"><highlight class="normal"><sp/><sp/>*(x_estimation+1)=zprevod((</highlight><highlight class="keywordtype">short</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal">)x_est[3],15)*Thetaref;</highlight></codeline>
684<codeline lineno="254"><highlight class="normal">}</highlight></codeline>
685    </programlisting>
686    <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp"/>
687  </compounddef>
688</doxygen>
Note: See TracBrowser for help on using the browser.