[187] | 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="testKF_8cpp" kind="file"> |
---|
| 4 | <compoundname>testKF.cpp</compoundname> |
---|
| 5 | <includes local="no">itpp/itbase.h</includes> |
---|
| 6 | <includes refid="libKF_8h" local="no">estim/libKF.h</includes> |
---|
| 7 | <incdepgraph> |
---|
| 8 | <node id="872"> |
---|
| 9 | <label>libBM.h</label> |
---|
| 10 | <link refid="libBM_8h"/> |
---|
| 11 | <childnode refid="869" relation="include"> |
---|
| 12 | </childnode> |
---|
| 13 | </node> |
---|
| 14 | <node id="875"> |
---|
| 15 | <label>../itpp_ext.h</label> |
---|
| 16 | <link refid="itpp__ext_8h-source"/> |
---|
| 17 | <childnode refid="869" relation="include"> |
---|
| 18 | </childnode> |
---|
| 19 | </node> |
---|
| 20 | <node id="873"> |
---|
| 21 | <label>../stat/libEF.h</label> |
---|
| 22 | <link refid="libEF_8h"/> |
---|
| 23 | <childnode refid="869" relation="include"> |
---|
| 24 | </childnode> |
---|
| 25 | <childnode refid="874" relation="include"> |
---|
| 26 | </childnode> |
---|
| 27 | <childnode refid="872" relation="include"> |
---|
| 28 | </childnode> |
---|
| 29 | <childnode refid="875" relation="include"> |
---|
| 30 | </childnode> |
---|
| 31 | </node> |
---|
| 32 | <node id="871"> |
---|
| 33 | <label>../stat/libFN.h</label> |
---|
| 34 | <link refid="libFN_8h-source"/> |
---|
| 35 | <childnode refid="869" relation="include"> |
---|
| 36 | </childnode> |
---|
| 37 | <childnode refid="872" relation="include"> |
---|
| 38 | </childnode> |
---|
| 39 | </node> |
---|
| 40 | <node id="869"> |
---|
| 41 | <label>itpp/itbase.h</label> |
---|
| 42 | </node> |
---|
| 43 | <node id="876"> |
---|
| 44 | <label>../math/chmat.h</label> |
---|
| 45 | <link refid="chmat_8h"/> |
---|
| 46 | <childnode refid="869" relation="include"> |
---|
| 47 | </childnode> |
---|
| 48 | <childnode refid="874" relation="include"> |
---|
| 49 | </childnode> |
---|
| 50 | <childnode refid="875" relation="include"> |
---|
| 51 | </childnode> |
---|
| 52 | </node> |
---|
| 53 | <node id="870"> |
---|
| 54 | <label>estim/libKF.h</label> |
---|
| 55 | <link refid="libKF_8h"/> |
---|
| 56 | <childnode refid="869" relation="include"> |
---|
| 57 | </childnode> |
---|
| 58 | <childnode refid="871" relation="include"> |
---|
| 59 | </childnode> |
---|
| 60 | <childnode refid="873" relation="include"> |
---|
| 61 | </childnode> |
---|
| 62 | <childnode refid="876" relation="include"> |
---|
| 63 | </childnode> |
---|
| 64 | </node> |
---|
| 65 | <node id="874"> |
---|
| 66 | <label>../math/libDC.h</label> |
---|
| 67 | <link refid="libDC_8h"/> |
---|
| 68 | <childnode refid="869" relation="include"> |
---|
| 69 | </childnode> |
---|
| 70 | </node> |
---|
| 71 | <node id="868"> |
---|
| 72 | <label>work/git/mixpp/tests/testKF.cpp</label> |
---|
| 73 | <link refid="testKF.cpp"/> |
---|
| 74 | <childnode refid="869" relation="include"> |
---|
| 75 | </childnode> |
---|
| 76 | <childnode refid="870" relation="include"> |
---|
| 77 | </childnode> |
---|
| 78 | </node> |
---|
| 79 | </incdepgraph> |
---|
| 80 | <sectiondef kind="func"> |
---|
| 81 | <memberdef kind="function" id="testKF_8cpp_1e66f6b31b5ad750f1fe042a706a4e3d4" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual"> |
---|
| 82 | <type>int</type> |
---|
| 83 | <definition>int main</definition> |
---|
| 84 | <argsstring>()</argsstring> |
---|
| 85 | <name>main</name> |
---|
| 86 | <briefdescription> |
---|
| 87 | </briefdescription> |
---|
| 88 | <detaileddescription> |
---|
| 89 | </detaileddescription> |
---|
| 90 | <inbodydescription> |
---|
| 91 | </inbodydescription> |
---|
| 92 | <location file="/home/smidl/work/git/mixpp/tests/testKF.cpp" line="10" bodyfile="/home/smidl/work/git/mixpp/tests/testKF.cpp" bodystart="10" bodyend="130"/> |
---|
| 93 | </memberdef> |
---|
| 94 | </sectiondef> |
---|
| 95 | <briefdescription> |
---|
| 96 | </briefdescription> |
---|
| 97 | <detaileddescription> |
---|
| 98 | </detaileddescription> |
---|
| 99 | <programlisting> |
---|
| 100 | <codeline lineno="1"><highlight class="preprocessor">#include<sp/><itpp/itbase.h></highlight><highlight class="normal"></highlight></codeline> |
---|
| 101 | <codeline lineno="2"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/><<ref refid="libKF_8h" kindref="compound" tooltip="Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions.">estim/libKF.h</ref>></highlight><highlight class="normal"></highlight></codeline> |
---|
| 102 | <codeline lineno="3"><highlight class="normal"></highlight></codeline> |
---|
| 103 | <codeline lineno="4"><highlight class="normal"></highlight><highlight class="keyword">using<sp/>namespace<sp/></highlight><highlight class="normal">itpp;</highlight></codeline> |
---|
| 104 | <codeline lineno="5"><highlight class="normal"></highlight></codeline> |
---|
| 105 | <codeline lineno="6"><highlight class="normal"></highlight><highlight class="comment">//These<sp/>lines<sp/>are<sp/>needed<sp/>for<sp/>use<sp/>of<sp/>cout<sp/>and<sp/>endl</highlight><highlight class="normal"></highlight></codeline> |
---|
| 106 | <codeline lineno="7"><highlight class="normal"></highlight><highlight class="keyword">using</highlight><highlight class="normal"><sp/>std::cout;</highlight></codeline> |
---|
| 107 | <codeline lineno="8"><highlight class="normal"></highlight><highlight class="keyword">using</highlight><highlight class="normal"><sp/>std::endl;</highlight></codeline> |
---|
| 108 | <codeline lineno="9"><highlight class="normal"></highlight></codeline> |
---|
| 109 | <codeline lineno="10"><highlight class="normal"></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/><ref refid="sim__var_8cpp_1e66f6b31b5ad750f1fe042a706a4e3d4" kindref="member" tooltip="Extended Kalman filter with unknown Q.">main</ref>()<sp/>{</highlight></codeline> |
---|
| 110 | <codeline lineno="11"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Kalman<sp/>filter</highlight><highlight class="normal"></highlight></codeline> |
---|
| 111 | <codeline lineno="12"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>A,<sp/>B,C,D,R,Q,P0;</highlight></codeline> |
---|
| 112 | <codeline lineno="13"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>mu0;</highlight></codeline> |
---|
| 113 | <codeline lineno="14"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Mu0;;</highlight></codeline> |
---|
| 114 | <codeline lineno="15"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>input<sp/>from<sp/>Matlab</highlight><highlight class="normal"></highlight></codeline> |
---|
| 115 | <codeline lineno="16"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_file<sp/>fin(<sp/></highlight><highlight class="stringliteral">"testKF.it"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 116 | <codeline lineno="17"><highlight class="normal"></highlight></codeline> |
---|
| 117 | <codeline lineno="18"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Dt;</highlight></codeline> |
---|
| 118 | <codeline lineno="19"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Ndat;</highlight></codeline> |
---|
| 119 | <codeline lineno="20"><highlight class="normal"></highlight></codeline> |
---|
| 120 | <codeline lineno="21"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordtype">bool</highlight><highlight class="normal"><sp/>xxx=<sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"d"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 121 | <codeline lineno="22"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(!xxx){<sp/>it_error(</highlight><highlight class="stringliteral">"testKF.it<sp/>not<sp/>found"</highlight><highlight class="normal">);}</highlight></codeline> |
---|
| 122 | <codeline lineno="23"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin<sp/>>>Dt;</highlight></codeline> |
---|
| 123 | <codeline lineno="24"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"A"</highlight><highlight class="normal"><sp/>);<sp/></highlight></codeline> |
---|
| 124 | <codeline lineno="25"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin<sp/>>><sp/>A;</highlight></codeline> |
---|
| 125 | <codeline lineno="26"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"B"</highlight><highlight class="normal"><sp/>);<sp/></highlight></codeline> |
---|
| 126 | <codeline lineno="27"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin<sp/>>><sp/>B;</highlight></codeline> |
---|
| 127 | <codeline lineno="28"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"C"</highlight><highlight class="normal"><sp/>);<sp/></highlight></codeline> |
---|
| 128 | <codeline lineno="29"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin<sp/>>><sp/>C;</highlight></codeline> |
---|
| 129 | <codeline lineno="30"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"D"</highlight><highlight class="normal"><sp/>);<sp/></highlight></codeline> |
---|
| 130 | <codeline lineno="31"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin<sp/>>><sp/>D;</highlight></codeline> |
---|
| 131 | <codeline lineno="32"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"R"</highlight><highlight class="normal"><sp/>);<sp/></highlight></codeline> |
---|
| 132 | <codeline lineno="33"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin<sp/>>><sp/>R;</highlight></codeline> |
---|
| 133 | <codeline lineno="34"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"Q"</highlight><highlight class="normal"><sp/>);<sp/>fin<sp/>>><sp/>Q;</highlight></codeline> |
---|
| 134 | <codeline lineno="35"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"P0"</highlight><highlight class="normal"><sp/>);<sp/>fin<sp/>>><sp/>P0;</highlight></codeline> |
---|
| 135 | <codeline lineno="36"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fin.seek(<sp/></highlight><highlight class="stringliteral">"mu0"</highlight><highlight class="normal"><sp/>);<sp/>fin<sp/>>><sp/>Mu0;<sp/></highlight></codeline> |
---|
| 136 | <codeline lineno="37"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mu0=Mu0.get_col(0);</highlight></codeline> |
---|
| 137 | <codeline lineno="38"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 138 | <codeline lineno="39"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Ndat<sp/>=<sp/>10;</highlight><highlight class="comment">//Dt.cols();</highlight><highlight class="normal"></highlight></codeline> |
---|
| 139 | <codeline lineno="40"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>dimx<sp/>=<sp/>A.rows();</highlight></codeline> |
---|
| 140 | <codeline lineno="41"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 141 | <codeline lineno="42"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Prepare<sp/>for<sp/>Kalman<sp/>filters<sp/>in<sp/>BDM:</highlight><highlight class="normal"></highlight></codeline> |
---|
| 142 | <codeline lineno="43"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>rx(</highlight><highlight class="stringliteral">"{x<sp/>}"</highlight><highlight class="normal">,vec_1(A.cols()));</highlight></codeline> |
---|
| 143 | <codeline lineno="44"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>ru(</highlight><highlight class="stringliteral">"{u<sp/>}"</highlight><highlight class="normal">,vec_1(B.cols()));</highlight></codeline> |
---|
| 144 | <codeline lineno="45"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>ry(</highlight><highlight class="stringliteral">"{y<sp/>}"</highlight><highlight class="normal">,vec_1(C.rows()));</highlight></codeline> |
---|
| 145 | <codeline lineno="46"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 146 | <codeline lineno="47"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>LDMAT</highlight><highlight class="normal"></highlight></codeline> |
---|
| 147 | <codeline lineno="48"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>Kalman<ldmat><sp/>KF(rx,ry,ru);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 148 | <codeline lineno="49"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));</highlight><highlight class="normal"></highlight></codeline> |
---|
| 149 | <codeline lineno="50"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>KF.set_est(mu0,ldmat(P0)<sp/>);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 150 | <codeline lineno="51"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>epdf&<sp/>KFep<sp/>=<sp/>KF._epdf();</highlight><highlight class="normal"></highlight></codeline> |
---|
| 151 | <codeline lineno="52"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Xt(2,Ndat);<sp/></highlight><highlight class="normal"></highlight></codeline> |
---|
| 152 | <codeline lineno="53"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>Xt.set_col(<sp/>0,KFep.mean()<sp/>);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 153 | <codeline lineno="54"><highlight class="normal"></highlight></codeline> |
---|
| 154 | <codeline lineno="55"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Chol</highlight><highlight class="normal"></highlight></codeline> |
---|
| 155 | <codeline lineno="56"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh" kindref="compound" tooltip="Kalman filter in square root form.">KalmanCh</ref><sp/>KF(rx,ry,ru);</highlight></codeline> |
---|
| 156 | <codeline lineno="57"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KF.set_parameters(A,B,C,D,<ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref>(R),<ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref>(Q));</highlight></codeline> |
---|
| 157 | <codeline lineno="58"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KF.set_est(mu0,<ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref>(P0)<sp/>);<sp/></highlight><highlight class="comment">//prediction!</highlight><highlight class="normal"></highlight></codeline> |
---|
| 158 | <codeline lineno="59"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classepdf" kindref="compound" tooltip="Probability density function with numerical statistics, e.g. posterior density.">epdf</ref>&<sp/>KFep<sp/>=<sp/>KF._epdf();</highlight></codeline> |
---|
| 159 | <codeline lineno="60"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Xt(dimx,Ndat);<sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 160 | <codeline lineno="61"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Xt.set_col(<sp/>0,KFep.<ref refid="classepdf_1bf0a070a8f3e67a93604ca724638b870" kindref="member" tooltip="return expected value">mean</ref>()<sp/>);</highlight></codeline> |
---|
| 161 | <codeline lineno="62"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 162 | <codeline lineno="63"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="normal"></highlight></codeline> |
---|
| 163 | <codeline lineno="64"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>FSQMAT</highlight><highlight class="normal"></highlight></codeline> |
---|
| 164 | <codeline lineno="65"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman" kindref="compound" tooltip="Kalman filter with covariance matrices in square root form.">Kalman<ldmat></ref><sp/>KFf(rx,ry,ru);</highlight></codeline> |
---|
| 165 | <codeline lineno="66"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KFf.set_parameters(A,B,C,D,<ref refid="classldmat" kindref="compound" tooltip="Matrix stored in LD form, (typically known as UD).">ldmat</ref>(R),<ref refid="classldmat" kindref="compound" tooltip="Matrix stored in LD form, (typically known as UD).">ldmat</ref>(Q));</highlight></codeline> |
---|
| 166 | <codeline lineno="67"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KFf.set_est(mu0,<ref refid="classldmat" kindref="compound" tooltip="Matrix stored in LD form, (typically known as UD).">ldmat</ref>(P0)<sp/>);</highlight></codeline> |
---|
| 167 | <codeline lineno="68"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classepdf" kindref="compound" tooltip="Probability density function with numerical statistics, e.g. posterior density.">epdf</ref>&<sp/>KFfep<sp/>=<sp/>KFf._epdf();</highlight></codeline> |
---|
| 168 | <codeline lineno="69"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Xtf(dimx,Ndat);<sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 169 | <codeline lineno="70"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Xtf.set_col(<sp/>0,KFfep.<ref refid="classepdf_1bf0a070a8f3e67a93604ca724638b870" kindref="member" tooltip="return expected value">mean</ref>()<sp/>);</highlight></codeline> |
---|
| 170 | <codeline lineno="71"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 171 | <codeline lineno="72"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>FULL</highlight><highlight class="normal"></highlight></codeline> |
---|
| 172 | <codeline lineno="73"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull" kindref="compound" tooltip="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</ref><sp/>KF2(<sp/>A,B,C,D,R,Q,P0,mu0<sp/>);</highlight></codeline> |
---|
| 173 | <codeline lineno="74"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Xt2(dimx,Ndat);<sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 174 | <codeline lineno="75"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Xt2.set_col(<sp/>0,mu0);</highlight></codeline> |
---|
| 175 | <codeline lineno="76"><highlight class="normal"></highlight></codeline> |
---|
| 176 | <codeline lineno="77"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 177 | <codeline lineno="78"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>EKF</highlight><highlight class="normal"></highlight></codeline> |
---|
| 178 | <codeline lineno="79"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classbilinfn" kindref="compound" tooltip="Class representing function .">bilinfn</ref><sp/>fxu(rx,ru,A,B);</highlight></codeline> |
---|
| 179 | <codeline lineno="80"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classbilinfn" kindref="compound" tooltip="Class representing function .">bilinfn</ref><sp/>hxu(rx,ru,C,D);</highlight></codeline> |
---|
| 180 | <codeline lineno="81"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classEKFCh" kindref="compound" tooltip="Extended Kalman Filter in Square root.">EKFCh</ref><sp/>KFE(rx,ry,ru);</highlight></codeline> |
---|
| 181 | <codeline lineno="82"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KFE.set_parameters(&fxu,&hxu,Q,R);</highlight></codeline> |
---|
| 182 | <codeline lineno="83"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KFE.set_est(mu0,<ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref>(P0));</highlight></codeline> |
---|
| 183 | <codeline lineno="84"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classepdf" kindref="compound" tooltip="Probability density function with numerical statistics, e.g. posterior density.">epdf</ref>&<sp/>KFEep<sp/>=<sp/>KFE._epdf();</highlight></codeline> |
---|
| 184 | <codeline lineno="85"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>XtE(dimx,Ndat);<sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 185 | <codeline lineno="86"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>XtE.set_col(<sp/>0,KFEep.<ref refid="classepdf_1bf0a070a8f3e67a93604ca724638b870" kindref="member" tooltip="return expected value">mean</ref>()<sp/>);</highlight></codeline> |
---|
| 186 | <codeline lineno="87"><highlight class="normal"></highlight></codeline> |
---|
| 187 | <codeline lineno="88"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//test<sp/>performance<sp/>of<sp/>each<sp/>filter</highlight><highlight class="normal"></highlight></codeline> |
---|
| 188 | <codeline lineno="89"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Real_Timer<sp/>tt;</highlight></codeline> |
---|
| 189 | <codeline lineno="90"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>exec_times(4);<sp/></highlight><highlight class="comment">//<sp/>KF,<sp/>KFf<sp/>KF2,<sp/>KFE</highlight><highlight class="normal"></highlight></codeline> |
---|
| 190 | <codeline lineno="91"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 191 | <codeline lineno="92"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>tt.tic();</highlight></codeline> |
---|
| 192 | <codeline lineno="93"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal"><sp/>(<sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t=1;t<Ndat;t++<sp/>)<sp/>{</highlight></codeline> |
---|
| 193 | <codeline lineno="94"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KF.bayes(<sp/>Dt.get_col(<sp/>t<sp/>));</highlight></codeline> |
---|
| 194 | <codeline lineno="95"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Xt.set_col(<sp/>t,KFep.<ref refid="classepdf_1bf0a070a8f3e67a93604ca724638b870" kindref="member" tooltip="return expected value">mean</ref>()<sp/>);</highlight></codeline> |
---|
| 195 | <codeline lineno="96"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 196 | <codeline lineno="97"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>exec_times(0)<sp/>=<sp/>tt.toc();</highlight></codeline> |
---|
| 197 | <codeline lineno="98"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 198 | <codeline lineno="99"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>tt.tic();</highlight></codeline> |
---|
| 199 | <codeline lineno="100"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal"><sp/>(<sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t=1;t<Ndat;t++<sp/>)<sp/>{</highlight></codeline> |
---|
| 200 | <codeline lineno="101"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KFf.bayes(<sp/>Dt.get_col(<sp/>t<sp/>));</highlight></codeline> |
---|
| 201 | <codeline lineno="102"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Xtf.set_col(<sp/>t,KFfep.<ref refid="classepdf_1bf0a070a8f3e67a93604ca724638b870" kindref="member" tooltip="return expected value">mean</ref>()<sp/>);</highlight></codeline> |
---|
| 202 | <codeline lineno="103"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 203 | <codeline lineno="104"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>exec_times(1)<sp/>=<sp/>tt.toc();</highlight></codeline> |
---|
| 204 | <codeline lineno="105"><highlight class="normal"></highlight></codeline> |
---|
| 205 | <codeline lineno="106"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>tt.tic();</highlight></codeline> |
---|
| 206 | <codeline lineno="107"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal"><sp/>(<sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t=1;t<Ndat;t++<sp/>)<sp/>{</highlight></codeline> |
---|
| 207 | <codeline lineno="108"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KF2.bayes(<sp/>Dt.get_col(<sp/>t<sp/>));</highlight></codeline> |
---|
| 208 | <codeline lineno="109"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Xt2.set_col(<sp/>t,KF2.mu);</highlight></codeline> |
---|
| 209 | <codeline lineno="110"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 210 | <codeline lineno="111"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>exec_times(2)<sp/>=<sp/>tt.toc();</highlight></codeline> |
---|
| 211 | <codeline lineno="112"><highlight class="normal"></highlight></codeline> |
---|
| 212 | <codeline lineno="113"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>tt.tic();</highlight></codeline> |
---|
| 213 | <codeline lineno="114"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal"><sp/>(<sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t=1;t<Ndat;t++<sp/>)<sp/>{</highlight></codeline> |
---|
| 214 | <codeline lineno="115"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KFE.bayes(<sp/>Dt.get_col(<sp/>t<sp/>));</highlight></codeline> |
---|
| 215 | <codeline lineno="116"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>XtE.set_col(<sp/>t,KFEep.<ref refid="classepdf_1bf0a070a8f3e67a93604ca724638b870" kindref="member" tooltip="return expected value">mean</ref>()<sp/>);</highlight></codeline> |
---|
| 216 | <codeline lineno="117"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 217 | <codeline lineno="118"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>exec_times(3)<sp/>=<sp/>tt.toc();</highlight></codeline> |
---|
| 218 | <codeline lineno="119"><highlight class="normal"></highlight></codeline> |
---|
| 219 | <codeline lineno="120"><highlight class="normal"></highlight></codeline> |
---|
| 220 | <codeline lineno="121"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_file<sp/>fou(<sp/></highlight><highlight class="stringliteral">"testKF_res.it"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 221 | <codeline lineno="122"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fou<sp/><<<sp/>Name(</highlight><highlight class="stringliteral">"xth"</highlight><highlight class="normal">)<sp/><<<sp/>Xt;</highlight></codeline> |
---|
| 222 | <codeline lineno="123"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fou<sp/><<<sp/>Name(</highlight><highlight class="stringliteral">"xthf"</highlight><highlight class="normal">)<sp/><<<sp/>Xtf;</highlight></codeline> |
---|
| 223 | <codeline lineno="124"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fou<sp/><<<sp/>Name(</highlight><highlight class="stringliteral">"xth2"</highlight><highlight class="normal">)<sp/><<<sp/>Xt2;</highlight></codeline> |
---|
| 224 | <codeline lineno="125"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fou<sp/><<<sp/>Name(</highlight><highlight class="stringliteral">"xthE"</highlight><highlight class="normal">)<sp/><<<sp/>XtE;</highlight></codeline> |
---|
| 225 | <codeline lineno="126"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>fou<sp/><<<sp/>Name(</highlight><highlight class="stringliteral">"exec_times"</highlight><highlight class="normal">)<sp/><<<sp/>exec_times;</highlight></codeline> |
---|
| 226 | <codeline lineno="127"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Exit<sp/>program:</highlight><highlight class="normal"></highlight></codeline> |
---|
| 227 | <codeline lineno="128"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>0;</highlight></codeline> |
---|
| 228 | <codeline lineno="129"><highlight class="normal"></highlight></codeline> |
---|
| 229 | <codeline lineno="130"><highlight class="normal">}</highlight></codeline> |
---|
| 230 | </programlisting> |
---|
| 231 | <location file="/home/smidl/work/git/mixpp/tests/testKF.cpp"/> |
---|
| 232 | </compounddef> |
---|
| 233 | </doxygen> |
---|