<?xml version='1.0' encoding='UTF-8' standalone='no'?>
<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.5.6">
  <compounddef id="ekf_8cpp" kind="file">
    <compoundname>ekf.cpp</compoundname>
    <includes local="no">math.h</includes>
    <includes refid="fixed_8h" local="yes">fixed.h</includes>
    <includes refid="reference_8h" local="yes">reference.h</includes>
    <includes refid="matrix_8h" local="yes">matrix.h</includes>
    <includes refid="ekf_8h" local="yes">ekf.h</includes>
    <incdepgraph>
      <node id="717">
        <label>ekf.h</label>
        <link refid="ekf_8h-source"/>
      </node>
      <node id="712">
        <label>work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp</label>
        <link refid="ekf.cpp"/>
        <childnode refid="713" relation="include">
        </childnode>
        <childnode refid="714" relation="include">
        </childnode>
        <childnode refid="715" relation="include">
        </childnode>
        <childnode refid="716" relation="include">
        </childnode>
        <childnode refid="717" relation="include">
        </childnode>
      </node>
      <node id="713">
        <label>math.h</label>
      </node>
      <node id="715">
        <label>reference.h</label>
        <link refid="reference_8h-source"/>
      </node>
      <node id="714">
        <label>fixed.h</label>
        <link refid="fixed_8h-source"/>
      </node>
      <node id="716">
        <label>matrix.h</label>
        <link refid="matrix_8h-source"/>
      </node>
    </incdepgraph>
      <sectiondef kind="define">
      <memberdef kind="define" id="ekf_8cpp_105679eb18ff6f0bd4ba53411a0fe0afb" prot="public" static="no">
        <name>K_PREVOD_FORM</name>
        <initializer>4</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      </sectiondef>
      <sectiondef kind="var">
      <memberdef kind="variable" id="ekf_8cpp_1415bdef5e664918e4e4b773c36f4e103" prot="public" static="no" mutable="no">
        <type>double</type>
        <definition>double ladeni_EKF[10]</definition>
        <argsstring>[10]</argsstring>
        <name>ladeni_EKF</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1f66eddc642fe2a42f062f5cd834102a5" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int H[8]</definition>
        <argsstring>[8]</argsstring>
        <name>H</name>
        <initializer>{0x7FFF,0,0,0,0,0x7FFF,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1f5a21f73c9dd58ca98b25b4525fedb11" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int Ht[8]</definition>
        <argsstring>[8]</argsstring>
        <name>Ht</name>
        <initializer>{0x7FFF,0,0,0x7FFF,0,0,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1893628019411e1d74b2ca67f76d647d9" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int Q[16]</definition>
        <argsstring>[16]</argsstring>
        <name>Q</name>
        <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_167b63ae29fb23d1a034f9ba7e0787745" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int R[4]</definition>
        <argsstring>[4]</argsstring>
        <name>R</name>
        <initializer>{0,0,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1db2f2b0f082a8ce53b3e61a9ce7e6c94" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int x_est[4]</definition>
        <argsstring>[4]</argsstring>
        <name>x_est</name>
        <initializer>{0,0,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1b5b26d093bdcad628aef9156a376174f" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int x_pred[4]</definition>
        <argsstring>[4]</argsstring>
        <name>x_pred</name>
        <initializer>{0,0,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1683087dfe47600d1a450302a2ccbb836" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int P_pred[16]</definition>
        <argsstring>[16]</argsstring>
        <name>P_pred</name>
        <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_198aa9c8e8546c3990a668fa2376bd02f" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int P_est[16]</definition>
        <argsstring>[16]</argsstring>
        <name>P_est</name>
        <initializer>{0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1af019cae3f3f03d594a8f549353c99e9" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int Y_mes[2]</definition>
        <argsstring>[2]</argsstring>
        <name>Y_mes</name>
        <initializer>{0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_16cccd275c0b11a7fac94937f2021045b" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int ukalm[2]</definition>
        <argsstring>[2]</argsstring>
        <name>ukalm</name>
        <initializer>{0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1f79d2220030336cad022d159b3f8c8bb" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int Kalm[8]</definition>
        <argsstring>[8]</argsstring>
        <name>Kalm</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_10db030b4590561a5e0a5d70223a860dd" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int PSI[16]</definition>
        <argsstring>[16]</argsstring>
        <name>PSI</name>
        <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_17a8fbda3ee1da60e70499a79b4d3e3df" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int temp15a[16]</definition>
        <argsstring>[16]</argsstring>
        <name>temp15a</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1f82c96cc9810ce86d1edec2fbf1deb6d" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int cA</definition>
        <argsstring></argsstring>
        <name>cA</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1026fd8d02ce266d7a9f9bb1e233f6242" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int cB</definition>
        <argsstring></argsstring>
        <name>cB</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_1da52f4f6674e0f993ee24e61553e2eae" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int cC</definition>
        <argsstring></argsstring>
        <name>cC</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_13a4546b83055800c4658329cb2ca0b4e" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int cG</definition>
        <argsstring></argsstring>
        <name>cG</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="variable" id="ekf_8cpp_119c038d1a31ea4257f137481c835ea96" prot="public" static="yes" mutable="no">
        <type>int</type>
        <definition>int cH</definition>
        <argsstring></argsstring>
        <name>cH</name>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      </sectiondef>
      <sectiondef kind="func">
      <memberdef kind="function" id="ekf_8cpp_1956dd7a9713816704f8ff5760e26e903" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
        <type>void</type>
        <definition>void init_ekf</definition>
        <argsstring>(double Tv, double *param)</argsstring>
        <name>init_ekf</name>
        <param>
          <type>double</type>
          <declname>Tv</declname>
        </param>
        <param>
          <type>double *</type>
          <declname>param</declname>
        </param>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
<para>KONEC inicializace pro PC - az sem vyhodit v DSP /// </para>        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="function" id="ekf_8cpp_1267ca49dd712a2b06af569b1e108976a" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
        <type>void</type>
        <definition>void ekf</definition>
        <argsstring>(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd)</argsstring>
        <name>ekf</name>
        <param>
          <type>double *</type>
          <declname>x_estimation</declname>
        </param>
        <param>
          <type>double</type>
          <declname>Umd</declname>
        </param>
        <param>
          <type>double</type>
          <declname>beta</declname>
        </param>
        <param>
          <type>double</type>
          <declname>Ucnd</declname>
        </param>
        <param>
          <type>double</type>
          <declname>Ucd</declname>
        </param>
        <param>
          <type>double</type>
          <declname>isxd</declname>
        </param>
        <param>
          <type>double</type>
          <declname>isyd</declname>
        </param>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
<para>prechod ze simulace do fixed-pointu - vyradit v DSP ///////////</para><para>KONEC emulace FIXED-POINTu - az sem vyradit v DSP //////////// </para>        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="function" id="ekf_8cpp_1bdd51eee3a5eb0a70f3d1e13b5cf2697" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual">
        <type>void</type>
        <definition>void prediction</definition>
        <argsstring>(int *ux)</argsstring>
        <name>prediction</name>
        <param>
          <type>int *</type>
          <declname>ux</declname>
        </param>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="function" id="ekf_8cpp_188642818810b8018ecbb525cad1d5c6f" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual">
        <type>void</type>
        <definition>void correction</definition>
        <argsstring>(void)</argsstring>
        <name>correction</name>
        <param>
          <type>void</type>
        </param>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      <memberdef kind="function" id="ekf_8cpp_195d9e77bdeaf5408a69d540731fd629f" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual">
        <type>void</type>
        <definition>static void update_psi</definition>
        <argsstring>(void)</argsstring>
        <name>update_psi</name>
        <param>
          <type>void</type>
        </param>
        <briefdescription>
        </briefdescription>
        <detaileddescription>
        </detaileddescription>
        <inbodydescription>
        </inbodydescription>
        <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"/>
      </memberdef>
      </sectiondef>
    <briefdescription>
    </briefdescription>
    <detaileddescription>
    </detaileddescription>
    <programlisting>
<codeline lineno="1"><highlight class="comment">/************************************</highlight></codeline>
<codeline lineno="2"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Extended<sp/>Kalman<sp/>Filter</highlight></codeline>
<codeline lineno="3"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Kalman<sp/>Observer</highlight></codeline>
<codeline lineno="4"><highlight class="comment"></highlight></codeline>
<codeline lineno="5"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Z.<sp/>Peroutka</highlight></codeline>
<codeline lineno="6"><highlight class="comment"></highlight></codeline>
<codeline lineno="7"><highlight class="comment">Rev.<sp/>15.3.2008</highlight></codeline>
<codeline lineno="8"><highlight class="comment"></highlight></codeline>
<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>
<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>
<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>
<codeline lineno="12"><highlight class="comment"></highlight></codeline>
<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>
<codeline lineno="14"><highlight class="comment"></highlight></codeline>
<codeline lineno="15"><highlight class="comment">*************************************/</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="16"><highlight class="normal"></highlight></codeline>
<codeline lineno="17"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;math.h&gt;</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="18"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;fixed.h&quot;</highlight><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="20"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;reference.h&quot;</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="21"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;matrix.h&quot;</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="22"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&quot;ekf.h&quot;</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="23"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="25"><highlight class="preprocessor"></highlight><highlight class="normal"></highlight></codeline>
<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>
<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>
<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>
<codeline lineno="29"><highlight class="normal"></highlight></codeline>
<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>
<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>
<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>
<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>
<codeline lineno="34"><highlight class="normal"></highlight></codeline>
<codeline lineno="35"><highlight class="normal"></highlight><highlight class="comment">//<sp/>global<sp/>variables</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="36"><highlight class="normal"></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>ladeni_EKF[10];</highlight></codeline>
<codeline lineno="37"><highlight class="normal"></highlight></codeline>
<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>
<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>
<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>
<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>
<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>
<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>
<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>
<codeline lineno="45"><highlight class="normal"></highlight></codeline>
<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>
<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>
<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>
<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>
<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>
<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>
<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>
<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>
<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>
<codeline lineno="55"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="57"><highlight class="normal"></highlight></codeline>
<codeline lineno="58"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Temporary<sp/>variables<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="60"><highlight class="normal"></highlight></codeline>
<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>
<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>
<codeline lineno="63"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="66"><highlight class="normal">{</highlight></codeline>
<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>
<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>
<codeline lineno="69"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="71"><highlight class="normal"><sp/><sp/>Rs=*param;</highlight></codeline>
<codeline lineno="72"><highlight class="normal"><sp/><sp/>Ls=*(param+1);</highlight></codeline>
<codeline lineno="73"><highlight class="normal"><sp/><sp/>Fmag=*(param+2);</highlight></codeline>
<codeline lineno="74"><highlight class="normal"><sp/><sp/>p=*(param+4);</highlight></codeline>
<codeline lineno="75"><highlight class="normal"><sp/><sp/>kp=*(param+5);</highlight></codeline>
<codeline lineno="77"><highlight class="normal"></highlight></codeline>
<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>
<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>
<codeline lineno="80"><highlight class="comment"><sp/><sp/>Q[1][1]=Q[0][0];</highlight></codeline>
<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>
<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>
<codeline lineno="83"><highlight class="comment"></highlight></codeline>
<codeline lineno="84"><highlight class="comment"><sp/><sp/>//<sp/>Tuning<sp/>of<sp/>matrix<sp/>R</highlight></codeline>
<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>
<codeline lineno="86"><highlight class="comment"><sp/><sp/>R[1][1]=R[0][0];</highlight></codeline>
<codeline lineno="87"><highlight class="comment">/**/</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="88"><highlight class="normal"></highlight></codeline>
<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>
<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>
<codeline lineno="91"><highlight class="normal"><sp/><sp/>Q[5]=Q[0];</highlight></codeline>
<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>
<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>
<codeline lineno="94"><highlight class="normal"></highlight></codeline>
<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>
<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>
<codeline lineno="97"><highlight class="normal"><sp/><sp/>R[3]=R[0];</highlight></codeline>
<codeline lineno="98"><highlight class="normal"></highlight></codeline>
<codeline lineno="99"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="101"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/><sp/><sp/>P_est[0][0]=prevod(0.999,15);</highlight></codeline>
<codeline lineno="102"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[1][1]=prevod(0.999,15);</highlight></codeline>
<codeline lineno="103"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[2][2]=prevod(0.999,15);</highlight></codeline>
<codeline lineno="104"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[3][3]=prevod(0.999,15);</highlight></codeline>
<codeline lineno="105"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[4][4]=prevod(0.999,15);</highlight></codeline>
<codeline lineno="106"><highlight class="comment">/**/</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="107"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="109"><highlight class="normal"><sp/><sp/>cA=prevod(1-Tv*Rs/Ls,15);</highlight></codeline>
<codeline lineno="110"><highlight class="normal"><sp/><sp/>cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);</highlight></codeline>
<codeline lineno="111"><highlight class="normal"><sp/><sp/>cC=prevod(Tv/Ls/Iref*Uref,15);</highlight></codeline>
<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>
<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>
<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>
<codeline lineno="115"><highlight class="normal"><sp/><sp/>cG=prevod(Tv*Wref*K_PREVOD_FORM/Thetaref,15);</highlight></codeline>
<codeline lineno="116"><highlight class="normal"><sp/><sp/>cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);</highlight></codeline>
<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>
<codeline lineno="118"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="120"><highlight class="normal"><sp/><sp/>PSI[0]=cA;</highlight></codeline>
<codeline lineno="121"><highlight class="normal"><sp/><sp/>PSI[5]=PSI[0];</highlight></codeline>
<codeline lineno="122"><highlight class="normal"><sp/><sp/>PSI[10]=0x7FFF;</highlight></codeline>
<codeline lineno="123"><highlight class="normal"><sp/><sp/>PSI[14]=cG;</highlight></codeline>
<codeline lineno="124"><highlight class="normal"><sp/><sp/>PSI[15]=0x7FFF;</highlight></codeline>
<codeline lineno="125"><highlight class="normal">}</highlight></codeline>
<codeline lineno="126"><highlight class="normal"></highlight></codeline>
<codeline lineno="127"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="129"><highlight class="normal">{</highlight></codeline>
<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>
<codeline lineno="131"><highlight class="normal"></highlight></codeline>
<codeline lineno="132"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="133"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
<codeline lineno="134"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
<codeline lineno="135"><highlight class="normal"></highlight></codeline>
<codeline lineno="136"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="137"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_sin=qsin(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="138"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_cos=qcos(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="139"><highlight class="normal"></highlight></codeline>
<codeline lineno="140"><highlight class="normal"><sp/><sp/>PSI[2]=((long)cB*t_sin)&gt;&gt;15;</highlight></codeline>
<codeline lineno="141"><highlight class="normal"><sp/><sp/>tmp=((long)cH*x_est[2])&gt;&gt;15;</highlight></codeline>
<codeline lineno="142"><highlight class="normal"><sp/><sp/>PSI[3]=((long)tmp*t_cos)&gt;&gt;15;</highlight></codeline>
<codeline lineno="143"><highlight class="normal"><sp/><sp/>PSI[6]=-((long)cB*t_cos)&gt;&gt;15;</highlight></codeline>
<codeline lineno="144"><highlight class="normal"><sp/><sp/>PSI[7]=((long)tmp*t_sin)&gt;&gt;15;</highlight></codeline>
<codeline lineno="145"><highlight class="normal">}</highlight></codeline>
<codeline lineno="146"><highlight class="normal"></highlight></codeline>
<codeline lineno="147"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="149"><highlight class="normal">{</highlight></codeline>
<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>
<codeline lineno="151"><highlight class="normal"></highlight></codeline>
<codeline lineno="152"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="153"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
<codeline lineno="154"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline>
<codeline lineno="155"><highlight class="normal"></highlight></codeline>
<codeline lineno="156"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="157"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_sin=qsin(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="158"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_cos=qcos(x_est[3]);</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="159"><highlight class="normal"></highlight></codeline>
<codeline lineno="160"><highlight class="normal"><sp/><sp/>tmp=((long)cB*x_est[2])&gt;&gt;15;</highlight></codeline>
<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>
<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>
<codeline lineno="163"><highlight class="normal"><sp/><sp/>x_pred[2]=x_est[2];</highlight></codeline>
<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>
<codeline lineno="165"><highlight class="normal"></highlight></codeline>
<codeline lineno="166"><highlight class="normal"><sp/><sp/>update_psi();</highlight></codeline>
<codeline lineno="167"><highlight class="normal"></highlight></codeline>
<codeline lineno="168"><highlight class="normal"><sp/><sp/>mmult(PSI,P_est,temp15a,3,3,3);</highlight></codeline>
<codeline lineno="169"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mtrans(PSI,temp15b,5,5);</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="170"><highlight class="normal"><sp/><sp/>mmultt(temp15a,PSI,P_pred,3,3,3);</highlight></codeline>
<codeline lineno="171"><highlight class="normal"><sp/><sp/>maddD(P_pred,Q,3,3);</highlight></codeline>
<codeline lineno="172"><highlight class="normal">}</highlight></codeline>
<codeline lineno="173"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="175"><highlight class="normal">{</highlight></codeline>
<codeline lineno="176"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Y_error[2];</highlight></codeline>
<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>
<codeline lineno="178"><highlight class="normal"></highlight></codeline>
<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>
<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>
<codeline lineno="181"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmultt(P_pred,H,temp15a,4,4,1);</highlight></codeline>
<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>
<codeline lineno="183"><highlight class="normal"></highlight></codeline>
<codeline lineno="184"><highlight class="normal"><sp/><sp/>choice_P(P_pred,temp15a,3);</highlight></codeline>
<codeline lineno="185"><highlight class="normal"><sp/><sp/>maddD(temp15a,R,1,1);</highlight></codeline>
<codeline lineno="186"><highlight class="normal"><sp/><sp/>minv2(temp15a,temp30a);</highlight></codeline>
<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>
<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>
<codeline lineno="189"><highlight class="normal"><sp/><sp/>mmultDr(P_pred,temp15a,3,3,1,1);</highlight></codeline>
<codeline lineno="190"><highlight class="normal"><sp/><sp/>mmult1530(temp15a,temp30a,Kalm,3,1,1);</highlight></codeline>
<codeline lineno="191"><highlight class="normal"></highlight></codeline>
<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>
<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>
<codeline lineno="194"><highlight class="normal"><sp/><sp/>choice_x(x_pred,<sp/>temp15a);</highlight></codeline>
<codeline lineno="195"><highlight class="normal"><sp/><sp/>msub(Y_mes,temp15a,Y_error,1,0);</highlight></codeline>
<codeline lineno="196"><highlight class="normal"><sp/><sp/>mmult(Kalm,Y_error,temp15a,3,1,0);</highlight></codeline>
<codeline lineno="197"><highlight class="normal"><sp/><sp/>madd(x_pred,temp15a,x_est,3,0);</highlight></codeline>
<codeline lineno="198"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="200"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmult(Kalm,H,temp15a,4,1,4);</highlight></codeline>
<codeline lineno="201"><highlight class="comment"><sp/><sp/>mmult(temp15a,P_pred,P_est,4,4,4);</highlight></codeline>
<codeline lineno="202"><highlight class="comment"><sp/><sp/>msub(P_pred,P_est,P_est,4,4);</highlight></codeline>
<codeline lineno="203"><highlight class="comment">/*<sp/>END<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="204"><highlight class="normal"></highlight></codeline>
<codeline lineno="205"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Version<sp/>with<sp/>MMULTDL()<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="206"><highlight class="normal"><sp/><sp/>mmultDl(P_pred,temp15a,1,3,3,1);</highlight></codeline>
<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>
<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>
<codeline lineno="209"><highlight class="normal"></highlight></codeline>
<codeline lineno="210"><highlight class="normal"><sp/><sp/>mmult(Kalm,temp15a,P_est,3,1,3);</highlight></codeline>
<codeline lineno="211"><highlight class="normal"><sp/><sp/>msub(P_pred,P_est,P_est,3,3);</highlight></codeline>
<codeline lineno="212"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>END<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="213"><highlight class="normal">}</highlight></codeline>
<codeline lineno="214"><highlight class="normal"></highlight></codeline>
<codeline lineno="215"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="217"><highlight class="normal">{</highlight></codeline>
<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>
<codeline lineno="219"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="222"><highlight class="normal"></highlight></codeline>
<codeline lineno="223"><highlight class="normal"><sp/><sp/>Um=prevod(Umd/Uref,Qm);</highlight></codeline>
<codeline lineno="224"><highlight class="normal"><sp/><sp/>Ucn=prevod(Ucnd/Uref,Qm);</highlight></codeline>
<codeline lineno="225"><highlight class="normal"><sp/><sp/>Uc=prevod(Ucd/Uref,Qm);</highlight></codeline>
<codeline lineno="226"><highlight class="normal"><sp/><sp/>isx=prevod(isxd/Iref,Qm);</highlight></codeline>
<codeline lineno="227"><highlight class="normal"><sp/><sp/>isy=prevod(isyd/Iref,Qm);</highlight></codeline>
<codeline lineno="229"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="231"><highlight class="normal"><sp/><sp/>Umk=((long)Um*Uc)/Ucn;</highlight></codeline>
<codeline lineno="232"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="234"><highlight class="normal"><sp/><sp/>ua=((long)Umk*prevod(cos(beta),15))&gt;&gt;15;</highlight></codeline>
<codeline lineno="235"><highlight class="normal"><sp/><sp/>ub=((long)Umk*prevod(cos(beta-2./3.*M_PI),15))&gt;&gt;15;</highlight></codeline>
<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>
<codeline lineno="237"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="239"><highlight class="normal"><sp/><sp/>ukalm[0]=ua;</highlight></codeline>
<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>
<codeline lineno="241"><highlight class="normal"></highlight></codeline>
<codeline lineno="242"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>zadani<sp/>mereni</highlight><highlight class="normal"></highlight></codeline>
<codeline lineno="243"><highlight class="normal"><sp/><sp/>Y_mes[0]=isx;</highlight></codeline>
<codeline lineno="244"><highlight class="normal"><sp/><sp/>Y_mes[1]=isy;</highlight></codeline>
<codeline lineno="245"><highlight class="normal"></highlight></codeline>
<codeline lineno="247"><highlight class="normal"><sp/><sp/>prediction(ukalm);</highlight></codeline>
<codeline lineno="248"><highlight class="normal"><sp/><sp/>correction();</highlight></codeline>
<codeline lineno="249"><highlight class="normal"></highlight></codeline>
<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>
<codeline lineno="251"><highlight class="normal"><sp/><sp/>*x_estimation=zprevod(x_est[2],Qm)*Wref;</highlight></codeline>
<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>
<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>
<codeline lineno="254"><highlight class="normal">}</highlight></codeline>
    </programlisting>
    <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp"/>
  </compounddef>
</doxygen>
