[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="libKF_8cpp" kind="file"> |
---|
| 4 | <compoundname>libKF.cpp</compoundname> |
---|
| 5 | <includes local="no">itpp/itbase.h</includes> |
---|
| 6 | <includes refid="libKF_8h" local="yes">libKF.h</includes> |
---|
| 7 | <incdepgraph> |
---|
| 8 | <node id="228"> |
---|
| 9 | <label>libBM.h</label> |
---|
| 10 | <link refid="libBM_8h"/> |
---|
| 11 | <childnode refid="225" relation="include"> |
---|
| 12 | </childnode> |
---|
| 13 | </node> |
---|
| 14 | <node id="231"> |
---|
| 15 | <label>../itpp_ext.h</label> |
---|
| 16 | <link refid="itpp__ext_8h-source"/> |
---|
| 17 | <childnode refid="225" relation="include"> |
---|
| 18 | </childnode> |
---|
| 19 | </node> |
---|
| 20 | <node id="229"> |
---|
| 21 | <label>../stat/libEF.h</label> |
---|
| 22 | <link refid="libEF_8h"/> |
---|
| 23 | <childnode refid="225" relation="include"> |
---|
| 24 | </childnode> |
---|
| 25 | <childnode refid="230" relation="include"> |
---|
| 26 | </childnode> |
---|
| 27 | <childnode refid="228" relation="include"> |
---|
| 28 | </childnode> |
---|
| 29 | <childnode refid="231" relation="include"> |
---|
| 30 | </childnode> |
---|
| 31 | </node> |
---|
| 32 | <node id="227"> |
---|
| 33 | <label>../stat/libFN.h</label> |
---|
| 34 | <link refid="libFN_8h-source"/> |
---|
| 35 | <childnode refid="225" relation="include"> |
---|
| 36 | </childnode> |
---|
| 37 | <childnode refid="228" relation="include"> |
---|
| 38 | </childnode> |
---|
| 39 | </node> |
---|
| 40 | <node id="225"> |
---|
| 41 | <label>itpp/itbase.h</label> |
---|
| 42 | </node> |
---|
| 43 | <node id="224"> |
---|
| 44 | <label>work/git/mixpp/bdm/estim/libKF.cpp</label> |
---|
| 45 | <link refid="libKF.cpp"/> |
---|
| 46 | <childnode refid="225" relation="include"> |
---|
| 47 | </childnode> |
---|
| 48 | <childnode refid="226" relation="include"> |
---|
| 49 | </childnode> |
---|
| 50 | </node> |
---|
| 51 | <node id="232"> |
---|
| 52 | <label>../math/chmat.h</label> |
---|
| 53 | <link refid="chmat_8h"/> |
---|
| 54 | <childnode refid="225" relation="include"> |
---|
| 55 | </childnode> |
---|
| 56 | <childnode refid="230" relation="include"> |
---|
| 57 | </childnode> |
---|
| 58 | <childnode refid="231" relation="include"> |
---|
| 59 | </childnode> |
---|
| 60 | </node> |
---|
| 61 | <node id="226"> |
---|
| 62 | <label>libKF.h</label> |
---|
| 63 | <link refid="libKF_8h"/> |
---|
| 64 | <childnode refid="225" relation="include"> |
---|
| 65 | </childnode> |
---|
| 66 | <childnode refid="227" relation="include"> |
---|
| 67 | </childnode> |
---|
| 68 | <childnode refid="229" relation="include"> |
---|
| 69 | </childnode> |
---|
| 70 | <childnode refid="232" relation="include"> |
---|
| 71 | </childnode> |
---|
| 72 | </node> |
---|
| 73 | <node id="230"> |
---|
| 74 | <label>../math/libDC.h</label> |
---|
| 75 | <link refid="libDC_8h"/> |
---|
| 76 | <childnode refid="225" relation="include"> |
---|
| 77 | </childnode> |
---|
| 78 | </node> |
---|
| 79 | </incdepgraph> |
---|
| 80 | <sectiondef kind="func"> |
---|
| 81 | <memberdef kind="function" id="libKF_8cpp_186ba216243ed95bb46d80d88775d16af" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual"> |
---|
| 82 | <type>std::ostream &</type> |
---|
| 83 | <definition>std::ostream& operator<<</definition> |
---|
| 84 | <argsstring>(std::ostream &os, const KalmanFull &kf)</argsstring> |
---|
| 85 | <name>operator<<</name> |
---|
| 86 | <param> |
---|
| 87 | <type>std::ostream &</type> |
---|
| 88 | <declname>os</declname> |
---|
| 89 | </param> |
---|
| 90 | <param> |
---|
| 91 | <type>const <ref refid="classKalmanFull" kindref="compound">KalmanFull</ref> &</type> |
---|
| 92 | <declname>kf</declname> |
---|
| 93 | </param> |
---|
| 94 | <briefdescription> |
---|
| 95 | </briefdescription> |
---|
| 96 | <detaileddescription> |
---|
| 97 | </detaileddescription> |
---|
| 98 | <inbodydescription> |
---|
| 99 | </inbodydescription> |
---|
| 100 | <location file="/home/smidl/work/git/mixpp/bdm/estim/libKF.cpp" line="56" bodyfile="/home/smidl/work/git/mixpp/bdm/estim/libKF.cpp" bodystart="56" bodyend="59"/> |
---|
| 101 | </memberdef> |
---|
| 102 | </sectiondef> |
---|
| 103 | <briefdescription> |
---|
| 104 | </briefdescription> |
---|
| 105 | <detaileddescription> |
---|
| 106 | </detaileddescription> |
---|
| 107 | <programlisting> |
---|
| 108 | <codeline lineno="1"><highlight class="preprocessor">#include<sp/><itpp/itbase.h></highlight><highlight class="normal"></highlight></codeline> |
---|
| 109 | <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.">libKF.h</ref>"</highlight><highlight class="normal"></highlight></codeline> |
---|
| 110 | <codeline lineno="3"><highlight class="normal"></highlight></codeline> |
---|
| 111 | <codeline lineno="4"><highlight class="normal"></highlight><highlight class="keyword">using<sp/>namespace<sp/></highlight><highlight class="normal">itpp;</highlight></codeline> |
---|
| 112 | <codeline lineno="5"><highlight class="normal"></highlight></codeline> |
---|
| 113 | <codeline lineno="6"><highlight class="normal"></highlight><highlight class="keyword">using</highlight><highlight class="normal"><sp/>std::endl;</highlight></codeline> |
---|
| 114 | <codeline lineno="7"><highlight class="normal"></highlight></codeline> |
---|
| 115 | <codeline lineno="8" refid="classKalmanFull_17197ab6e7380790006394eabd3b97043" refkind="member"><highlight class="normal"><ref refid="classKalmanFull_19d7cc2235b643d9662cd9c8b8469747d" kindref="member" tooltip="For EKFfull;.">KalmanFull::KalmanFull</ref><sp/>(<sp/>mat<sp/>A0,<sp/>mat<sp/>B0,<sp/>mat<sp/>C0,<sp/>mat<sp/>D0,<sp/>mat<sp/>R0,<sp/>mat<sp/>Q0,<sp/>mat<sp/>P0,<sp/>vec<sp/>mu0<sp/>)<sp/>{</highlight></codeline> |
---|
| 116 | <codeline lineno="9"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>dimx<sp/>=<sp/>A0.rows();</highlight></codeline> |
---|
| 117 | <codeline lineno="10"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>dimu<sp/>=<sp/>B0.cols();</highlight></codeline> |
---|
| 118 | <codeline lineno="11"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>dimy<sp/>=<sp/>C0.rows();</highlight></codeline> |
---|
| 119 | <codeline lineno="12"><highlight class="normal"></highlight></codeline> |
---|
| 120 | <codeline lineno="13"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>A0.cols()<sp/>==dimx,<sp/></highlight><highlight class="stringliteral">"KalmanFull:<sp/>A<sp/>is<sp/>not<sp/>square"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 121 | <codeline lineno="14"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>B0.rows()<sp/>==dimx,<sp/></highlight><highlight class="stringliteral">"KalmanFull:<sp/>B<sp/>is<sp/>not<sp/>compatible"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 122 | <codeline lineno="15"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>C0.cols()<sp/>==dimx,<sp/></highlight><highlight class="stringliteral">"KalmanFull:<sp/>C<sp/>is<sp/>not<sp/>square"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 123 | <codeline lineno="16"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>(<sp/>D0.rows()<sp/>==dimy<sp/>)<sp/>||<sp/>(<sp/>D0.cols()<sp/>==dimu<sp/>),<sp/></highlight><highlight class="stringliteral">"KalmanFull:<sp/>D<sp/>is<sp/>not<sp/>compatible"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 124 | <codeline lineno="17"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>(<sp/>R0.cols()<sp/>==dimy<sp/>)<sp/>||<sp/>(<sp/>R0.rows()<sp/>==dimy<sp/>),<sp/></highlight><highlight class="stringliteral">"KalmanFull:<sp/>R<sp/>is<sp/>not<sp/>compatible"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 125 | <codeline lineno="18"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>(<sp/>Q0.cols()<sp/>==dimx<sp/>)<sp/>||<sp/>(<sp/>Q0.rows()<sp/>==dimx<sp/>),<sp/></highlight><highlight class="stringliteral">"KalmanFull:<sp/>Q<sp/>is<sp/>not<sp/>compatible"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 126 | <codeline lineno="19"><highlight class="normal"></highlight></codeline> |
---|
| 127 | <codeline lineno="20"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>A<sp/>=<sp/>A0;</highlight></codeline> |
---|
| 128 | <codeline lineno="21"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>B<sp/>=<sp/>B0;</highlight></codeline> |
---|
| 129 | <codeline lineno="22"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>C<sp/>=<sp/>C0;</highlight></codeline> |
---|
| 130 | <codeline lineno="23"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>D<sp/>=<sp/>D0;</highlight></codeline> |
---|
| 131 | <codeline lineno="24"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>R<sp/>=<sp/>R0;</highlight></codeline> |
---|
| 132 | <codeline lineno="25"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Q<sp/>=<sp/>Q0;</highlight></codeline> |
---|
| 133 | <codeline lineno="26"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref><sp/>=<sp/>mu0;</highlight></codeline> |
---|
| 134 | <codeline lineno="27"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref><sp/>=<sp/>P0;</highlight></codeline> |
---|
| 135 | <codeline lineno="28"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 136 | <codeline lineno="29"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>ll<sp/>=<sp/>0.0;</highlight></codeline> |
---|
| 137 | <codeline lineno="30"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>evalll=</highlight><highlight class="keyword">true</highlight><highlight class="normal">;</highlight></codeline> |
---|
| 138 | <codeline lineno="31"><highlight class="normal">}</highlight></codeline> |
---|
| 139 | <codeline lineno="32"><highlight class="normal"></highlight></codeline> |
---|
| 140 | <codeline lineno="33" refid="classKalmanFull_113a041cd98ff157703766be275a657bb" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classKalmanFull_113a041cd98ff157703766be275a657bb" kindref="member" tooltip="Here dt = [yt;ut] of appropriate dimensions.">KalmanFull::bayes</ref><sp/>(<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>vec<sp/>&dt<sp/>)<sp/>{</highlight></codeline> |
---|
| 141 | <codeline lineno="34"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>dt.length()<sp/>==<sp/>(<sp/>dimy+dimu<sp/>),</highlight><highlight class="stringliteral">"KalmanFull::bayes<sp/>wrong<sp/>size<sp/>of<sp/>dt"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 142 | <codeline lineno="35"><highlight class="normal"></highlight></codeline> |
---|
| 143 | <codeline lineno="36"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>u<sp/>=<sp/>dt.get<sp/>(<sp/>dimy,dimy+dimu-1<sp/>);</highlight></codeline> |
---|
| 144 | <codeline lineno="37"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>y<sp/>=<sp/>dt.get<sp/>(<sp/>0,dimy-1<sp/>);</highlight></codeline> |
---|
| 145 | <codeline lineno="38"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Time<sp/>update</highlight><highlight class="normal"></highlight></codeline> |
---|
| 146 | <codeline lineno="39"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref><sp/>=<sp/>A*<ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref><sp/>+<sp/>B*u;</highlight></codeline> |
---|
| 147 | <codeline lineno="40"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref><sp/><sp/>=<sp/>A*<ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>*A.transpose()<sp/>+<sp/>Q;</highlight></codeline> |
---|
| 148 | <codeline lineno="41"><highlight class="normal"></highlight></codeline> |
---|
| 149 | <codeline lineno="42"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Data<sp/>update</highlight><highlight class="normal"></highlight></codeline> |
---|
| 150 | <codeline lineno="43"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>_Ry<sp/>=<sp/>C*<ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>*C.transpose()<sp/>+<sp/>R;</highlight></codeline> |
---|
| 151 | <codeline lineno="44"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>_iRy<sp/>=<sp/>inv<sp/>(<sp/>_Ry<sp/>);</highlight></codeline> |
---|
| 152 | <codeline lineno="45"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>_K<sp/>=<sp/><ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>*C.transpose()<sp/>*_iRy;</highlight></codeline> |
---|
| 153 | <codeline lineno="46"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref><sp/>-=<sp/>_K*C*<ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>;<sp/></highlight><highlight class="comment">//<sp/>P<sp/>=<sp/>P<sp/>-KCP;</highlight><highlight class="normal"></highlight></codeline> |
---|
| 154 | <codeline lineno="47"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref><sp/>+=<sp/>_K*<sp/>(<sp/>y-C*<ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>-D*u<sp/>);</highlight></codeline> |
---|
| 155 | <codeline lineno="48"><highlight class="normal"></highlight></codeline> |
---|
| 156 | <codeline lineno="49"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(<sp/>evalll<sp/>)<sp/>{</highlight></codeline> |
---|
| 157 | <codeline lineno="50"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//from<sp/>enorm</highlight><highlight class="normal"></highlight></codeline> |
---|
| 158 | <codeline lineno="51"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>x=y-C*<ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>-D*u;</highlight></codeline> |
---|
| 159 | <codeline lineno="52"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>ll<sp/>=<sp/>-0.5*<sp/>(<sp/>R.cols()<sp/>*<sp/>1.83787706640935<sp/>+log<sp/>(<sp/>det<sp/>(<sp/>_Ry<sp/>)<sp/>)<sp/>+x*<sp/>(<sp/>_iRy*x<sp/>)<sp/>);</highlight></codeline> |
---|
| 160 | <codeline lineno="53"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 161 | <codeline lineno="54"><highlight class="normal">};</highlight></codeline> |
---|
| 162 | <codeline lineno="55"><highlight class="normal"></highlight></codeline> |
---|
| 163 | <codeline lineno="56" refid="classKalmanFull_186ba216243ed95bb46d80d88775d16af" refkind="member"><highlight class="normal">std::ostream<sp/>&<ref refid="classKalmanFull_186ba216243ed95bb46d80d88775d16af" kindref="member" tooltip="print elements of KF">operator<< </ref>(<sp/>std::ostream<sp/>&os,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/><ref refid="classKalmanFull" kindref="compound" tooltip="Basic Kalman filter with full matrices (education purpose only)! Will be deleted...">KalmanFull</ref><sp/>&kf<sp/>)<sp/>{</highlight></codeline> |
---|
| 164 | <codeline lineno="57"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>os<sp/><<<sp/></highlight><highlight class="stringliteral">"mu:"</highlight><highlight class="normal"><sp/><<<sp/>kf.<ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref><sp/><<<sp/>endl<sp/><<<sp/></highlight><highlight class="stringliteral">"P:"</highlight><highlight class="normal"><sp/><<<sp/>kf.<ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref><sp/><<endl;</highlight></codeline> |
---|
| 165 | <codeline lineno="58"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>os;</highlight></codeline> |
---|
| 166 | <codeline lineno="59"><highlight class="normal">}</highlight></codeline> |
---|
| 167 | <codeline lineno="60"><highlight class="normal"></highlight></codeline> |
---|
| 168 | <codeline lineno="61"><highlight class="normal"></highlight></codeline> |
---|
| 169 | <codeline lineno="62"><highlight class="normal"></highlight></codeline> |
---|
| 170 | <codeline lineno="64" refid="classEKFfull_167ac4de96fd025197da767fe0472c7f7" refkind="member"><highlight class="normal"><ref refid="classEKFfull_167ac4de96fd025197da767fe0472c7f7" kindref="member" tooltip="Default constructor.">EKFfull::EKFfull</ref><sp/>(<sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>rvx0,<sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>rvy0,<sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>rvu0<sp/>)<sp/>:<sp/><ref refid="classBM" kindref="compound" tooltip="Bayesian Model of the world, i.e. all uncertainty is modeled by probabilities.">BM</ref><sp/>(<sp/>rvx0<sp/>),E(rvx0)<sp/>{};</highlight></codeline> |
---|
| 171 | <codeline lineno="65"><highlight class="normal"></highlight></codeline> |
---|
| 172 | <codeline lineno="66" refid="classEKFfull_1fc753106e0d4cf68e4f2160fd54458c0" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classEKFfull_1fc753106e0d4cf68e4f2160fd54458c0" kindref="member" tooltip="Set nonlinear functions for mean values and covariance matrices.">EKFfull::set_parameters</ref><sp/>(<sp/><ref refid="classdiffbifn" kindref="compound" tooltip="Class representing a differentiable function of two variables .">diffbifn</ref>*<sp/>pfxu0,<sp/><sp/><ref refid="classdiffbifn" kindref="compound" tooltip="Class representing a differentiable function of two variables .">diffbifn</ref>*<sp/>phxu0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>mat<sp/>Q0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>mat<sp/>R0<sp/>)<sp/>{</highlight></codeline> |
---|
| 173 | <codeline lineno="67"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>pfxu<sp/>=<sp/>pfxu0;</highlight></codeline> |
---|
| 174 | <codeline lineno="68"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>phxu<sp/>=<sp/>phxu0;</highlight></codeline> |
---|
| 175 | <codeline lineno="69"><highlight class="normal"></highlight></codeline> |
---|
| 176 | <codeline lineno="70"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>dimx<sp/>=<sp/><ref refid="classBM_1af00f0612fabe66241dd507188cdbf88" kindref="member" tooltip="Random variable of the posterior.">rv</ref>.<ref refid="classRV_1f5c7b8bd589eef09ccdf3329a0addea0" kindref="member" tooltip="Return number of scalars in the RV.">count</ref>();</highlight></codeline> |
---|
| 177 | <codeline lineno="71"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>dimy<sp/>=<sp/>phxu0-><ref refid="classfnc_1a8891973d0ca48ce38e1886df45ca298" kindref="member" tooltip="access function">_dimy</ref>();</highlight></codeline> |
---|
| 178 | <codeline lineno="72"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>dimu<sp/>=<sp/>phxu0-><ref refid="classdiffbifn_1fc8779acbff170611aff0ee70cee3879" kindref="member" tooltip="access function">_dimu</ref>();</highlight></codeline> |
---|
| 179 | <codeline lineno="73"><highlight class="normal"></highlight></codeline> |
---|
| 180 | <codeline lineno="74"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>A.set_size(dimx,dimx);</highlight></codeline> |
---|
| 181 | <codeline lineno="75"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>C.set_size(dimy,dimx);</highlight></codeline> |
---|
| 182 | <codeline lineno="76"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//initialize<sp/>matrices<sp/>A<sp/>C,<sp/>later,<sp/>these<sp/>will<sp/>be<sp/>only<sp/>updated!</highlight><highlight class="normal"></highlight></codeline> |
---|
| 183 | <codeline lineno="77"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>pfxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>,zeros<sp/>(<sp/>dimu<sp/>),A,</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 184 | <codeline lineno="78"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>B.clear();</highlight></codeline> |
---|
| 185 | <codeline lineno="79"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>phxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>,zeros<sp/>(<sp/>dimu<sp/>),C,</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 186 | <codeline lineno="80"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>D.clear();</highlight></codeline> |
---|
| 187 | <codeline lineno="81"><highlight class="normal"></highlight></codeline> |
---|
| 188 | <codeline lineno="82"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>R<sp/>=<sp/>R0;</highlight></codeline> |
---|
| 189 | <codeline lineno="83"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Q<sp/>=<sp/>Q0;</highlight></codeline> |
---|
| 190 | <codeline lineno="84"><highlight class="normal">}</highlight></codeline> |
---|
| 191 | <codeline lineno="85"><highlight class="normal"></highlight></codeline> |
---|
| 192 | <codeline lineno="86" refid="classEKFfull_18ca46f177e395fa714bbd8bd29ea43e0" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classEKFfull_18ca46f177e395fa714bbd8bd29ea43e0" kindref="member" tooltip="Here dt = [yt;ut] of appropriate dimensions.">EKFfull::bayes</ref><sp/>(<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>vec<sp/>&dt<sp/>)<sp/>{</highlight></codeline> |
---|
| 193 | <codeline lineno="87"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>dt.length()<sp/>==<sp/>(<sp/>dimy+dimu<sp/>),</highlight><highlight class="stringliteral">"KalmanFull::bayes<sp/>wrong<sp/>size<sp/>of<sp/>dt"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 194 | <codeline lineno="88"><highlight class="normal"></highlight></codeline> |
---|
| 195 | <codeline lineno="89"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>u<sp/>=<sp/>dt.get<sp/>(<sp/>dimy,dimy+dimu-1<sp/>);</highlight></codeline> |
---|
| 196 | <codeline lineno="90"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>y<sp/>=<sp/>dt.get<sp/>(<sp/>0,dimy-1<sp/>);</highlight></codeline> |
---|
| 197 | <codeline lineno="91"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 198 | <codeline lineno="92"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>pfxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>,zeros<sp/>(<sp/>dimu<sp/>),A,</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 199 | <codeline lineno="93"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>phxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>,zeros<sp/>(<sp/>dimu<sp/>),C,</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 200 | <codeline lineno="94"><highlight class="normal"></highlight></codeline> |
---|
| 201 | <codeline lineno="95"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Time<sp/>update</highlight><highlight class="normal"></highlight></codeline> |
---|
| 202 | <codeline lineno="96"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref><sp/>=<sp/>pfxu-><ref refid="classdiffbifn_1ad7673e16aa1a046b131b24c731c4632" kindref="member" tooltip="Evaluates (VS: Do we really need common eval? ).">eval</ref>(<ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>,u);</highlight><highlight class="comment">//<sp/>A*mu<sp/>+<sp/>B*u;</highlight><highlight class="normal"></highlight></codeline> |
---|
| 203 | <codeline lineno="97"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref><sp/><sp/>=<sp/>A*<ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>*A.transpose()<sp/>+<sp/>Q;</highlight></codeline> |
---|
| 204 | <codeline lineno="98"><highlight class="normal"></highlight></codeline> |
---|
| 205 | <codeline lineno="99"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Data<sp/>update</highlight><highlight class="normal"></highlight></codeline> |
---|
| 206 | <codeline lineno="100"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>_Ry<sp/>=<sp/>C*<ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>*C.transpose()<sp/>+<sp/>R;</highlight></codeline> |
---|
| 207 | <codeline lineno="101"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>_iRy<sp/>=<sp/>inv<sp/>(<sp/>_Ry<sp/>);</highlight></codeline> |
---|
| 208 | <codeline lineno="102"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>_K<sp/>=<sp/><ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>*C.transpose()<sp/>*_iRy;</highlight></codeline> |
---|
| 209 | <codeline lineno="103"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref><sp/>-=<sp/>_K*C*<ref refid="classKalmanFull_1b75dc059e84fa8ffc076203b30f926cc" kindref="member" tooltip="Variance of the posterior density.">P</ref>;<sp/></highlight><highlight class="comment">//<sp/>P<sp/>=<sp/>P<sp/>-KCP;</highlight><highlight class="normal"></highlight></codeline> |
---|
| 210 | <codeline lineno="104"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>yp<sp/>=<sp/>phxu-><ref refid="classdiffbifn_1ad7673e16aa1a046b131b24c731c4632" kindref="member" tooltip="Evaluates (VS: Do we really need common eval? ).">eval</ref>(<ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>,u);</highlight></codeline> |
---|
| 211 | <codeline lineno="105"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref><sp/>+=<sp/>_K*<sp/>(<sp/>y-yp<sp/>);</highlight></codeline> |
---|
| 212 | <codeline lineno="106"><highlight class="normal"></highlight></codeline> |
---|
| 213 | <codeline lineno="107"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>E.<ref refid="classenorm_1d892a38f03be12e572ea57d9689cef6b" kindref="member" tooltip="access function">set_mu</ref>(<ref refid="classKalmanFull_1fb5aec635e2720cc5ac31bc01c18a68a" kindref="member" tooltip="Mean value of the posterior density.">mu</ref>);</highlight></codeline> |
---|
| 214 | <codeline lineno="108"><highlight class="normal"></highlight></codeline> |
---|
| 215 | <codeline lineno="109"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(<sp/><ref refid="classBM_1bf6fb59b30141074f8ee1e2f43d03129" kindref="member" tooltip="If true, the filter will compute likelihood of the data record and store it in ll...">BM::evalll</ref><sp/>)<sp/>{</highlight></codeline> |
---|
| 216 | <codeline lineno="110"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//from<sp/>enorm</highlight><highlight class="normal"></highlight></codeline> |
---|
| 217 | <codeline lineno="111"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>x=y-yp;</highlight></codeline> |
---|
| 218 | <codeline lineno="112"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classBM_15623fef6572a08c2b53b8c87b82dc979" kindref="member" tooltip="Logarithm of marginalized data likelihood.">BM::ll</ref><sp/>=<sp/>-0.5*<sp/>(<sp/>R.cols()<sp/>*<sp/>1.83787706640935<sp/>+log<sp/>(<sp/>det<sp/>(<sp/>_Ry<sp/>)<sp/>)<sp/>+x*<sp/>(<sp/>_iRy*x<sp/>)<sp/>);</highlight></codeline> |
---|
| 219 | <codeline lineno="113"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 220 | <codeline lineno="114"><highlight class="normal">};</highlight></codeline> |
---|
| 221 | <codeline lineno="115"><highlight class="normal"></highlight></codeline> |
---|
| 222 | <codeline lineno="116"><highlight class="normal"></highlight></codeline> |
---|
| 223 | <codeline lineno="117"><highlight class="normal"></highlight></codeline> |
---|
| 224 | <codeline lineno="118" refid="classKalmanCh_192fb227287af05c9f0078d523c7c9793" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classKalmanCh_192fb227287af05c9f0078d523c7c9793" kindref="member" tooltip="Set parameters with check of relevance.">KalmanCh::set_parameters</ref><sp/>(<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>mat<sp/>&A0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>mat<sp/>&B0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>mat<sp/>&C0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>mat<sp/>&D0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/><ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref><sp/>&R0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/><ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref><sp/>&Q0<sp/>)<sp/>{</highlight></codeline> |
---|
| 225 | <codeline lineno="119"><highlight class="normal"></highlight></codeline> |
---|
| 226 | <codeline lineno="120"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>(<sp/>(<sp/><ref refid="classKalman" kindref="compound" tooltip="Kalman filter with covariance matrices in square root form.">Kalman<chmat></ref>*<sp/>)<sp/></highlight><highlight class="keyword">this</highlight><highlight class="normal"><sp/>)->set_parameters<sp/>(<sp/>A0,B0,C0,D0,R0,Q0<sp/>);</highlight></codeline> |
---|
| 227 | <codeline lineno="121"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Cholesky<sp/>special!</highlight><highlight class="normal"></highlight></codeline> |
---|
| 228 | <codeline lineno="122"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.clear();</highlight></codeline> |
---|
| 229 | <codeline lineno="123"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/>0,0,<ref refid="classKalman_111d171dc0e0ab111c56a70f98b97b3ec" kindref="member" tooltip="Matrix R in square-root form.">R</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>);</highlight></codeline> |
---|
| 230 | <codeline lineno="124"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_19b69015c800eb93f3ee49da23a6f55d9" kindref="member" tooltip="Matrix Q in square-root form.">Q</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>);</highlight></codeline> |
---|
| 231 | <codeline lineno="125"><highlight class="normal">}</highlight></codeline> |
---|
| 232 | <codeline lineno="126"><highlight class="normal"></highlight></codeline> |
---|
| 233 | <codeline lineno="127"><highlight class="normal"></highlight></codeline> |
---|
| 234 | <codeline lineno="128" refid="classKalmanCh_1cca758192846940409822b9bd778d4e1" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classKalmanCh_1cca758192846940409822b9bd778d4e1" kindref="member" tooltip="Here dt = [yt;ut] of appropriate dimensions.">KalmanCh::bayes</ref><sp/>(<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>vec<sp/>&dt<sp/>)<sp/>{</highlight></codeline> |
---|
| 235 | <codeline lineno="129"><highlight class="normal"></highlight></codeline> |
---|
| 236 | <codeline lineno="130"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>u<sp/>=<sp/>dt.get<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_1b0153795a1444b6968a86409c778d9ce" kindref="member" tooltip="cache of rvu.count()">dimu</ref>-1<sp/>);</highlight></codeline> |
---|
| 237 | <codeline lineno="131"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>y<sp/>=<sp/>dt.get<sp/>(<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1<sp/>);</highlight></codeline> |
---|
| 238 | <codeline lineno="132"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>pom(<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>);</highlight></codeline> |
---|
| 239 | <codeline lineno="133"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 240 | <codeline lineno="134"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//TODO<sp/>get<sp/>rid<sp/>of<sp/>Q<sp/>in<sp/>qr()!</highlight><highlight class="normal"></highlight></codeline> |
---|
| 241 | <codeline lineno="135"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Q;</highlight><highlight class="normal"></highlight></codeline> |
---|
| 242 | <codeline lineno="136"><highlight class="normal"></highlight></codeline> |
---|
| 243 | <codeline lineno="137"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//R<sp/>and<sp/>Q<sp/>are<sp/>already<sp/>set<sp/>in<sp/>set_parameters()</highlight><highlight class="normal"></highlight></codeline> |
---|
| 244 | <codeline lineno="138"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,0,<sp/>(<sp/><ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>*<ref refid="classKalman_186a805cd6515872d1132ad0d6eb5dc13" kindref="member" tooltip="Matrix C.">C</ref>.T()<sp/>);</highlight></codeline> |
---|
| 245 | <codeline lineno="139"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Fixme<sp/>can<sp/>be<sp/>more<sp/>efficient<sp/>if<sp/>.T()<sp/>is<sp/>not<sp/>used</highlight><highlight class="normal"></highlight></codeline> |
---|
| 246 | <codeline lineno="140"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<sp/>(<sp/><ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>*<ref refid="classKalman_15e02efe86ee91e9c74b93b425fe060b9" kindref="member" tooltip="Matrix A.">A</ref>.T()<sp/>);</highlight></codeline> |
---|
| 247 | <codeline lineno="141"><highlight class="normal"></highlight></codeline> |
---|
| 248 | <codeline lineno="142"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>if<sp/>(<sp/>!qr<sp/>(<sp/>preA,Q,postA<sp/>)<sp/>)<sp/>it_warning<sp/>(<sp/>"QR<sp/>in<sp/>kalman<sp/>unstable!"<sp/>);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 249 | <codeline lineno="143"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(<sp/>!qr<sp/>(<sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>,<ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>)<sp/>)<sp/>{it_warning<sp/>(<sp/></highlight><highlight class="stringliteral">"QR<sp/>in<sp/>kalman<sp/>unstable!"</highlight><highlight class="normal"><sp/>);}</highlight></codeline> |
---|
| 250 | <codeline lineno="144"><highlight class="normal"></highlight></codeline> |
---|
| 251 | <codeline lineno="145"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>(<sp/><ref refid="classKalman_145c9f928d2d62e0c884900fb3380f904" kindref="member" tooltip="cache of fy.R">_Ry</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>=<ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>(<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1,<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1<sp/>);</highlight></codeline> |
---|
| 252 | <codeline lineno="146"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1d422f51467c7a06174af2476d2826132" kindref="member" tooltip="placeholder for Kalman gain">_K</ref><sp/>=<sp/>inv(<ref refid="classKalman_15e02efe86ee91e9c74b93b425fe060b9" kindref="member" tooltip="Matrix A.">A</ref>)*(<sp/><ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>(<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1<sp/>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>-1<sp/>)<sp/>).T();</highlight></codeline> |
---|
| 253 | <codeline lineno="147"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>(<sp/><ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>=<sp/><ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>-1,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>-1<sp/>);</highlight></codeline> |
---|
| 254 | <codeline lineno="148"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 255 | <codeline lineno="149"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref><sp/>=<sp/><ref refid="classKalman_15e02efe86ee91e9c74b93b425fe060b9" kindref="member" tooltip="Matrix A.">A</ref>*<sp/>(<sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref><sp/>)<sp/>+<sp/><ref refid="classKalman_1dc87704284a6c0bca13bf51f4345a50a" kindref="member" tooltip="Matrix B.">B</ref>*u;</highlight></codeline> |
---|
| 256 | <codeline lineno="150"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1764bbc95238eda11fc81c5ebd0b1dcfd" kindref="member" tooltip="cache of fy.mu">_yp</ref><sp/>=<sp/><ref refid="classKalman_186a805cd6515872d1132ad0d6eb5dc13" kindref="member" tooltip="Matrix C.">C</ref>*<ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref><sp/>-<ref refid="classKalman_1d69f774ba3335c970c1c5b1d182f4dd1" kindref="member" tooltip="Matrix D.">D</ref>*u;</highlight></codeline> |
---|
| 257 | <codeline lineno="151"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 258 | <codeline lineno="152"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>backward_substitution(<ref refid="classKalman_145c9f928d2d62e0c884900fb3380f904" kindref="member" tooltip="cache of fy.R">_Ry</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>(),(<sp/>y-_yp),pom);</highlight></codeline> |
---|
| 259 | <codeline lineno="153"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref><sp/>+=<sp/><sp/>(<sp/><ref refid="classKalman_1d422f51467c7a06174af2476d2826132" kindref="member" tooltip="placeholder for Kalman gain">_K</ref><sp/>)<sp/>*<sp/>pom;</highlight></codeline> |
---|
| 260 | <codeline lineno="154"><highlight class="normal"></highlight></codeline> |
---|
| 261 | <codeline lineno="155"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>cout<sp/><<<sp/></highlight><highlight class="stringliteral">"P:"</highlight><highlight class="normal"><sp/><<<ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_1a37e2c726e4fc3ad50b26ac2ca6c1452" kindref="member" tooltip="Conversion to full matrix.">to_mat</ref>()<sp/><<endl;</highlight></codeline> |
---|
| 262 | <codeline lineno="156"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>cout<sp/><<<sp/></highlight><highlight class="stringliteral">"Ry:"</highlight><highlight class="normal"><sp/><<<ref refid="classKalman_145c9f928d2d62e0c884900fb3380f904" kindref="member" tooltip="cache of fy.R">_Ry</ref>.<ref refid="classchmat_1a37e2c726e4fc3ad50b26ac2ca6c1452" kindref="member" tooltip="Conversion to full matrix.">to_mat</ref>()<sp/><<endl;</highlight></codeline> |
---|
| 263 | <codeline lineno="157"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>cout<sp/><<<sp/></highlight><highlight class="stringliteral">"_K:"</highlight><highlight class="normal"><sp/><<<ref refid="classKalman_1d422f51467c7a06174af2476d2826132" kindref="member" tooltip="placeholder for Kalman gain">_K</ref><sp/><<endl;</highlight></codeline> |
---|
| 264 | <codeline lineno="158"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 265 | <codeline lineno="159"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(<sp/><ref refid="classBM_1bf6fb59b30141074f8ee1e2f43d03129" kindref="member" tooltip="If true, the filter will compute likelihood of the data record and store it in ll...">evalll</ref>==</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>)<sp/>{<sp/></highlight><highlight class="comment">//likelihood<sp/>of<sp/>observation<sp/>y</highlight><highlight class="normal"></highlight></codeline> |
---|
| 266 | <codeline lineno="160"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classBM_15623fef6572a08c2b53b8c87b82dc979" kindref="member" tooltip="Logarithm of marginalized data likelihood.">ll</ref>=<ref refid="classKalman_1e580ab06483952bd03f2e651763e184f" kindref="member" tooltip="preditive density on $y_t$">fy</ref>.<ref refid="classenorm_1609a7c33dbb4fdfab050f3bdd1122401" kindref="member" tooltip="Compute log-probability of argument val.">evalpdflog</ref><sp/>(<sp/>y<sp/>);</highlight></codeline> |
---|
| 267 | <codeline lineno="161"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 268 | <codeline lineno="162"><highlight class="normal">}</highlight></codeline> |
---|
| 269 | <codeline lineno="163"><highlight class="normal"></highlight></codeline> |
---|
| 270 | <codeline lineno="164"><highlight class="normal"></highlight></codeline> |
---|
| 271 | <codeline lineno="165" refid="classEKFCh_1e9e39a9204db3dda88d06e47c1e19064" refkind="member"><highlight class="normal"><ref refid="classEKFCh_1e9e39a9204db3dda88d06e47c1e19064" kindref="member" tooltip="Default constructor.">EKFCh::EKFCh</ref><sp/>(<sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>rvx0,<sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>rvy0,<sp/><ref refid="classRV" kindref="compound" tooltip="Class representing variables, most often random variables.">RV</ref><sp/>rvu0<sp/>)<sp/>:<sp/><ref refid="classKalmanCh" kindref="compound" tooltip="Kalman filter in square root form.">KalmanCh</ref><sp/>(<sp/>rvx0,rvy0,rvu0<sp/>)<sp/>{}</highlight></codeline> |
---|
| 272 | <codeline lineno="166"><highlight class="normal"></highlight></codeline> |
---|
| 273 | <codeline lineno="167" refid="classEKFCh_10216bed270df59fe65d0d62d41f8257c" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classEKFCh_10216bed270df59fe65d0d62d41f8257c" kindref="member" tooltip="Set nonlinear functions for mean values and covariance matrices.">EKFCh::set_parameters</ref><sp/>(<sp/><ref refid="classdiffbifn" kindref="compound" tooltip="Class representing a differentiable function of two variables .">diffbifn</ref>*<sp/>pfxu0,<sp/><sp/><ref refid="classdiffbifn" kindref="compound" tooltip="Class representing a differentiable function of two variables .">diffbifn</ref>*<sp/>phxu0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/><ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref><sp/>Q0,</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/><ref refid="classchmat" kindref="compound" tooltip="Symmetric matrix stored in square root decomposition using upper cholesky.">chmat</ref><sp/>R0<sp/>)<sp/>{</highlight></codeline> |
---|
| 274 | <codeline lineno="168"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>pfxu<sp/>=<sp/>pfxu0;</highlight></codeline> |
---|
| 275 | <codeline lineno="169"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>phxu<sp/>=<sp/>phxu0;</highlight></codeline> |
---|
| 276 | <codeline lineno="170"><highlight class="normal"></highlight></codeline> |
---|
| 277 | <codeline lineno="171"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//initialize<sp/>matrices<sp/>A<sp/>C,<sp/>later,<sp/>these<sp/>will<sp/>be<sp/>only<sp/>updated!</highlight><highlight class="normal"></highlight></codeline> |
---|
| 278 | <codeline lineno="172"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>pfxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref>,zeros<sp/>(<sp/><ref refid="classKalman_1b0153795a1444b6968a86409c778d9ce" kindref="member" tooltip="cache of rvu.count()">dimu</ref><sp/>),<ref refid="classKalman_15e02efe86ee91e9c74b93b425fe060b9" kindref="member" tooltip="Matrix A.">A</ref>,</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 279 | <codeline lineno="173"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>pfxu->dfdu_cond<sp/>(<sp/>*_mu,zeros<sp/>(<sp/>dimu<sp/>),B,true<sp/>);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 280 | <codeline lineno="174"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1dc87704284a6c0bca13bf51f4345a50a" kindref="member" tooltip="Matrix B.">B</ref>.clear();</highlight></codeline> |
---|
| 281 | <codeline lineno="175"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>phxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref>,zeros<sp/>(<sp/><ref refid="classKalman_1b0153795a1444b6968a86409c778d9ce" kindref="member" tooltip="cache of rvu.count()">dimu</ref><sp/>),<ref refid="classKalman_186a805cd6515872d1132ad0d6eb5dc13" kindref="member" tooltip="Matrix C.">C</ref>,</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 282 | <codeline lineno="176"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>phxu->dfdu_cond<sp/>(<sp/>*_mu,zeros<sp/>(<sp/>dimu<sp/>),D,true<sp/>);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 283 | <codeline lineno="177"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1d69f774ba3335c970c1c5b1d182f4dd1" kindref="member" tooltip="Matrix D.">D</ref>.clear();</highlight></codeline> |
---|
| 284 | <codeline lineno="178"><highlight class="normal"></highlight></codeline> |
---|
| 285 | <codeline lineno="179"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_111d171dc0e0ab111c56a70f98b97b3ec" kindref="member" tooltip="Matrix R in square-root form.">R</ref><sp/>=<sp/>R0;</highlight></codeline> |
---|
| 286 | <codeline lineno="180"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_19b69015c800eb93f3ee49da23a6f55d9" kindref="member" tooltip="Matrix Q in square-root form.">Q</ref><sp/>=<sp/>Q0;</highlight></codeline> |
---|
| 287 | <codeline lineno="181"><highlight class="normal"></highlight></codeline> |
---|
| 288 | <codeline lineno="182"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Cholesky<sp/>special!</highlight><highlight class="normal"></highlight></codeline> |
---|
| 289 | <codeline lineno="183"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.clear();</highlight></codeline> |
---|
| 290 | <codeline lineno="184"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/>0,0,<ref refid="classKalman_111d171dc0e0ab111c56a70f98b97b3ec" kindref="member" tooltip="Matrix R in square-root form.">R</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>);</highlight></codeline> |
---|
| 291 | <codeline lineno="185"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_19b69015c800eb93f3ee49da23a6f55d9" kindref="member" tooltip="Matrix Q in square-root form.">Q</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>);</highlight></codeline> |
---|
| 292 | <codeline lineno="186"><highlight class="normal">}</highlight></codeline> |
---|
| 293 | <codeline lineno="187"><highlight class="normal"></highlight></codeline> |
---|
| 294 | <codeline lineno="188"><highlight class="normal"></highlight></codeline> |
---|
| 295 | <codeline lineno="189" refid="classEKFCh_196f6edda324a0b7ef8b4e86cc7af60c1" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classEKFCh_196f6edda324a0b7ef8b4e86cc7af60c1" kindref="member" tooltip="Here dt = [yt;ut] of appropriate dimensions.">EKFCh::bayes</ref><sp/>(<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>vec<sp/>&dt<sp/>)<sp/>{</highlight></codeline> |
---|
| 296 | <codeline lineno="190"><highlight class="normal"></highlight></codeline> |
---|
| 297 | <codeline lineno="191"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>pom(<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>);</highlight></codeline> |
---|
| 298 | <codeline lineno="192"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>u<sp/>=<sp/>dt.get<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_1b0153795a1444b6968a86409c778d9ce" kindref="member" tooltip="cache of rvu.count()">dimu</ref>-1<sp/>);</highlight></codeline> |
---|
| 299 | <codeline lineno="193"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>y<sp/>=<sp/>dt.get<sp/>(<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1<sp/>);</highlight></codeline> |
---|
| 300 | <codeline lineno="194"><highlight class="normal"></highlight></codeline> |
---|
| 301 | <codeline lineno="195"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>pfxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref>,u,<ref refid="classKalman_15e02efe86ee91e9c74b93b425fe060b9" kindref="member" tooltip="Matrix A.">A</ref>,</highlight><highlight class="keyword">false</highlight><highlight class="normal"><sp/>);<sp/></highlight><highlight class="comment">//update<sp/>A<sp/>by<sp/>a<sp/>derivative<sp/>of<sp/>fx</highlight><highlight class="normal"></highlight></codeline> |
---|
| 302 | <codeline lineno="196"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>phxu-><ref refid="classdiffbifn_16d217a02d4fa13931258d4bebdd0feb4" kindref="member" tooltip="Evaluates and writes result into A .">dfdx_cond</ref><sp/>(<sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref>,u,<ref refid="classKalman_186a805cd6515872d1132ad0d6eb5dc13" kindref="member" tooltip="Matrix C.">C</ref>,</highlight><highlight class="keyword">false</highlight><highlight class="normal"><sp/>);<sp/></highlight><highlight class="comment">//update<sp/>A<sp/>by<sp/>a<sp/>derivative<sp/>of<sp/>fx</highlight><highlight class="normal"></highlight></codeline> |
---|
| 303 | <codeline lineno="197"><highlight class="normal"></highlight></codeline> |
---|
| 304 | <codeline lineno="198"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//R<sp/>and<sp/>Q<sp/>are<sp/>already<sp/>set<sp/>in<sp/>set_parameters()</highlight><highlight class="normal"></highlight></codeline> |
---|
| 305 | <codeline lineno="199"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,0,<sp/>(<sp/><ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>*<ref refid="classKalman_186a805cd6515872d1132ad0d6eb5dc13" kindref="member" tooltip="Matrix C.">C</ref>.T()<sp/>);</highlight></codeline> |
---|
| 306 | <codeline lineno="200"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//Fixme<sp/>can<sp/>be<sp/>more<sp/>efficient<sp/>if<sp/>.T()<sp/>is<sp/>not<sp/>used</highlight><highlight class="normal"></highlight></codeline> |
---|
| 307 | <codeline lineno="201"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>.set_submatrix<sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<sp/>(<sp/><ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>*<ref refid="classKalman_15e02efe86ee91e9c74b93b425fe060b9" kindref="member" tooltip="Matrix A.">A</ref>.T()<sp/>);</highlight></codeline> |
---|
| 308 | <codeline lineno="202"><highlight class="normal"></highlight></codeline> |
---|
| 309 | <codeline lineno="203"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Sttm<sp/>=<sp/>_P->to_mat();</highlight><highlight class="normal"></highlight></codeline> |
---|
| 310 | <codeline lineno="204"><highlight class="normal"></highlight></codeline> |
---|
| 311 | <codeline lineno="205"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>if<sp/>(<sp/>!qr<sp/>(<sp/>preA,Q,postA<sp/>)<sp/>)<sp/>it_warning<sp/>(<sp/>"QR<sp/>in<sp/>kalman<sp/>unstable!"<sp/>);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 312 | <codeline lineno="206"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(<sp/>!qr<sp/>(<sp/><ref refid="classKalmanCh_194ee9da75b0e0f632e4a354988ca3798" kindref="member" tooltip="pre array (triangular matrix)">preA</ref>,<ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>)<sp/>)<sp/>{it_warning<sp/>(<sp/></highlight><highlight class="stringliteral">"QR<sp/>in<sp/>kalman<sp/>unstable!"</highlight><highlight class="normal"><sp/>);}</highlight></codeline> |
---|
| 313 | <codeline lineno="207"><highlight class="normal"></highlight></codeline> |
---|
| 314 | <codeline lineno="208"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>(<sp/><ref refid="classKalman_145c9f928d2d62e0c884900fb3380f904" kindref="member" tooltip="cache of fy.R">_Ry</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>=<ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>(<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1,<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1<sp/>);</highlight></codeline> |
---|
| 315 | <codeline lineno="209"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1d422f51467c7a06174af2476d2826132" kindref="member" tooltip="placeholder for Kalman gain">_K</ref><sp/>=<sp/>inv(<ref refid="classKalman_15e02efe86ee91e9c74b93b425fe060b9" kindref="member" tooltip="Matrix A.">A</ref>)*(<sp/><ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>(<sp/>0,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>-1<sp/>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>-1<sp/>)<sp/>).T();</highlight></codeline> |
---|
| 316 | <codeline lineno="210"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>(<sp/><ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>()<sp/>)<sp/>=<sp/><ref refid="classKalmanCh_10d31a26dc72b5846cfe5af3ccb63ac87" kindref="member" tooltip="post array (triangular matrix)">postA</ref><sp/>(<sp/><ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>-1,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>,<ref refid="classKalman_1ba17b956df1e38b31fbbc299c8213b6a" kindref="member" tooltip="cache of rvy.count()">dimy</ref>+<ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>-1<sp/>);</highlight></codeline> |
---|
| 317 | <codeline lineno="211"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 318 | <codeline lineno="212"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>iRY<sp/>=<sp/>inv(_Ry->to_mat());</highlight><highlight class="normal"></highlight></codeline> |
---|
| 319 | <codeline lineno="213"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>Stt<sp/>=<sp/>Sttm<sp/>-<sp/>Sttm<sp/>*<sp/>C.T()<sp/>*<sp/>iRY<sp/>*<sp/>C<sp/>*<sp/>Sttm;</highlight><highlight class="normal"></highlight></codeline> |
---|
| 320 | <codeline lineno="214"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/><sp/><sp/><sp/><sp/>mat<sp/>_K2<sp/>=<sp/>Stt*C.T()*inv(R.to_mat());</highlight><highlight class="normal"></highlight></codeline> |
---|
| 321 | <codeline lineno="215"><highlight class="normal"></highlight></codeline> |
---|
| 322 | <codeline lineno="216"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>prediction</highlight><highlight class="normal"></highlight></codeline> |
---|
| 323 | <codeline lineno="217"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref><sp/>=<sp/>pfxu-><ref refid="classdiffbifn_1ad7673e16aa1a046b131b24c731c4632" kindref="member" tooltip="Evaluates (VS: Do we really need common eval? ).">eval</ref><sp/>(<sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref><sp/>,u<sp/>);</highlight></codeline> |
---|
| 324 | <codeline lineno="218"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1764bbc95238eda11fc81c5ebd0b1dcfd" kindref="member" tooltip="cache of fy.mu">_yp</ref><sp/>=<sp/>phxu-><ref refid="classdiffbifn_1ad7673e16aa1a046b131b24c731c4632" kindref="member" tooltip="Evaluates (VS: Do we really need common eval? ).">eval</ref><sp/>(<sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref>,u<sp/>);</highlight></codeline> |
---|
| 325 | <codeline lineno="219"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 326 | <codeline lineno="220"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/><sp/><sp/><sp/><sp/>vec<sp/>mu2<sp/>=<sp/>*_mu<sp/>+<sp/>(<sp/>_K2<sp/>)<sp/>*<sp/>(<sp/>y-*_yp<sp/>);*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 327 | <codeline lineno="221"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 328 | <codeline lineno="222"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//correction<sp/>//=<sp/>initial<sp/>value<sp/>is<sp/>already<sp/>prediction!</highlight><highlight class="normal"></highlight></codeline> |
---|
| 329 | <codeline lineno="223"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>backward_substitution(<ref refid="classKalman_145c9f928d2d62e0c884900fb3380f904" kindref="member" tooltip="cache of fy.R">_Ry</ref>.<ref refid="classchmat_19c50d31c999d85d8e9d8cf2b69b6ac8c" kindref="member" tooltip="Access function.">_Ch</ref>(),(<sp/>y-<ref refid="classKalman_1764bbc95238eda11fc81c5ebd0b1dcfd" kindref="member" tooltip="cache of fy.mu">_yp</ref><sp/>),pom);</highlight></codeline> |
---|
| 330 | <codeline lineno="224"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_1fe803a81d2d847b0b1db3c6b29c18061" kindref="member" tooltip="cache of est.mu">_mu</ref><sp/>+=<sp/>(<sp/><ref refid="classKalman_1d422f51467c7a06174af2476d2826132" kindref="member" tooltip="placeholder for Kalman gain">_K</ref><sp/>)<sp/>*pom<sp/>;</highlight></codeline> |
---|
| 331 | <codeline lineno="225"><highlight class="normal"></highlight></codeline> |
---|
| 332 | <codeline lineno="226"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/><sp/><sp/><sp/><sp/>_K<sp/>=<sp/>(_P->to_mat())*C.transpose()<sp/>*<sp/>(<sp/>_iRy->to_mat()<sp/>);</highlight></codeline> |
---|
| 333 | <codeline lineno="227"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>*_mu<sp/>=<sp/>pfxu->eval<sp/>(<sp/>*_mu<sp/>,u<sp/>)<sp/>+<sp/>(<sp/>_K<sp/>)*<sp/>(<sp/>y-*_yp<sp/>);*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 334 | <codeline lineno="228"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight></codeline> |
---|
| 335 | <codeline lineno="229"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>cout<sp/><<<sp/></highlight><highlight class="stringliteral">"P:"</highlight><highlight class="normal"><sp/><<<ref refid="classKalman_19fb808cc94a4c2652e1fb93be9bb7dcf" kindref="member" tooltip="cache of est.R">_P</ref>.<ref refid="classchmat_1a37e2c726e4fc3ad50b26ac2ca6c1452" kindref="member" tooltip="Conversion to full matrix.">to_mat</ref>()<sp/><<endl;</highlight></codeline> |
---|
| 336 | <codeline lineno="230"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>cout<sp/><<<sp/></highlight><highlight class="stringliteral">"Ry:"</highlight><highlight class="normal"><sp/><<<ref refid="classKalman_145c9f928d2d62e0c884900fb3380f904" kindref="member" tooltip="cache of fy.R">_Ry</ref>.<ref refid="classchmat_1a37e2c726e4fc3ad50b26ac2ca6c1452" kindref="member" tooltip="Conversion to full matrix.">to_mat</ref>()<sp/><<endl;</highlight></codeline> |
---|
| 337 | <codeline lineno="231"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>cout<sp/><<<sp/></highlight><highlight class="stringliteral">"_K:"</highlight><highlight class="normal"><sp/><<<ref refid="classKalman_1d422f51467c7a06174af2476d2826132" kindref="member" tooltip="placeholder for Kalman gain">_K</ref><sp/><<endl;</highlight></codeline> |
---|
| 338 | <codeline lineno="232"><highlight class="normal"></highlight></codeline> |
---|
| 339 | <codeline lineno="233"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal"><sp/>(<sp/><ref refid="classBM_1bf6fb59b30141074f8ee1e2f43d03129" kindref="member" tooltip="If true, the filter will compute likelihood of the data record and store it in ll...">evalll</ref>==</highlight><highlight class="keyword">true</highlight><highlight class="normal"><sp/>)<sp/>{<sp/></highlight><highlight class="comment">//likelihood<sp/>of<sp/>observation<sp/>y</highlight><highlight class="normal"></highlight></codeline> |
---|
| 340 | <codeline lineno="234"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classBM_15623fef6572a08c2b53b8c87b82dc979" kindref="member" tooltip="Logarithm of marginalized data likelihood.">ll</ref>=<ref refid="classKalman_1e580ab06483952bd03f2e651763e184f" kindref="member" tooltip="preditive density on $y_t$">fy</ref>.<ref refid="classenorm_1609a7c33dbb4fdfab050f3bdd1122401" kindref="member" tooltip="Compute log-probability of argument val.">evalpdflog</ref><sp/>(<sp/>y<sp/>);</highlight></codeline> |
---|
| 341 | <codeline lineno="235"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline> |
---|
| 342 | <codeline lineno="236"><highlight class="normal">}</highlight></codeline> |
---|
| 343 | <codeline lineno="237"><highlight class="normal"></highlight></codeline> |
---|
| 344 | <codeline lineno="238" refid="classKFcondQR_1c9ecf292a85327aa6309c9fd70ceb606" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classKFcondQR_1c9ecf292a85327aa6309c9fd70ceb606" kindref="member" tooltip="Substitute val for rvc.">KFcondQR::condition</ref><sp/>(<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>vec<sp/>&QR<sp/>)<sp/>{</highlight></codeline> |
---|
| 345 | <codeline lineno="239"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>QR.length()<sp/>==<sp/>(<sp/><ref refid="classBMcond_19ba793c8ec453f04d372d17195ed8dec" kindref="member" tooltip="Identificator of the conditioning variable.">rvc</ref>.<ref refid="classRV_1f5c7b8bd589eef09ccdf3329a0addea0" kindref="member" tooltip="Return number of scalars in the RV.">count</ref>()<sp/>),</highlight><highlight class="stringliteral">"KFcondRQ:<sp/>conditioning<sp/>by<sp/>incompatible<sp/>vector"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 346 | <codeline lineno="240"><highlight class="normal"></highlight></codeline> |
---|
| 347 | <codeline lineno="241"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_19b69015c800eb93f3ee49da23a6f55d9" kindref="member" tooltip="Matrix Q in square-root form.">Q</ref>.<ref refid="classldmat_10884a613b94fde61bfc84288e73ce57f" kindref="member" tooltip="Access functions.">setD</ref><sp/>(<sp/>QR<sp/>(<sp/>0,<sp/><ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>-1<sp/>)<sp/>);</highlight></codeline> |
---|
| 348 | <codeline lineno="242"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_111d171dc0e0ab111c56a70f98b97b3ec" kindref="member" tooltip="Matrix R in square-root form.">R</ref>.<ref refid="classldmat_10884a613b94fde61bfc84288e73ce57f" kindref="member" tooltip="Access functions.">setD</ref><sp/>(<sp/>QR<sp/>(<sp/><ref refid="classKalman_139c8c403b46fa3b8c7da77cb2e3729eb" kindref="member" tooltip="cache of rv.count()">dimx</ref>,<sp/>-1<sp/>)<sp/>);</highlight></codeline> |
---|
| 349 | <codeline lineno="243"><highlight class="normal">};</highlight></codeline> |
---|
| 350 | <codeline lineno="244"><highlight class="normal"></highlight></codeline> |
---|
| 351 | <codeline lineno="245" refid="classKFcondR_18c0721e47879bb8840d829db7a174a7f" refkind="member"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/><ref refid="classKFcondR_18c0721e47879bb8840d829db7a174a7f" kindref="member" tooltip="Substitute val for rvc.">KFcondR::condition</ref><sp/>(<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>vec<sp/>&R0<sp/>)<sp/>{</highlight></codeline> |
---|
| 352 | <codeline lineno="246"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>it_assert_debug<sp/>(<sp/>R0.length()<sp/>==<sp/>(<sp/><ref refid="classBMcond_19ba793c8ec453f04d372d17195ed8dec" kindref="member" tooltip="Identificator of the conditioning variable.">rvc</ref>.<ref refid="classRV_1f5c7b8bd589eef09ccdf3329a0addea0" kindref="member" tooltip="Return number of scalars in the RV.">count</ref>()<sp/>),</highlight><highlight class="stringliteral">"KFcondR:<sp/>conditioning<sp/>by<sp/>incompatible<sp/>vector"</highlight><highlight class="normal"><sp/>);</highlight></codeline> |
---|
| 353 | <codeline lineno="247"><highlight class="normal"></highlight></codeline> |
---|
| 354 | <codeline lineno="248"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><ref refid="classKalman_111d171dc0e0ab111c56a70f98b97b3ec" kindref="member" tooltip="Matrix R in square-root form.">R</ref>.<ref refid="classldmat_10884a613b94fde61bfc84288e73ce57f" kindref="member" tooltip="Access functions.">setD</ref><sp/>(<sp/>R0<sp/>);</highlight></codeline> |
---|
| 355 | <codeline lineno="249"><highlight class="normal">};</highlight></codeline> |
---|
| 356 | <codeline lineno="250"><highlight class="normal"></highlight></codeline> |
---|
| 357 | <codeline lineno="251"><highlight class="normal"></highlight></codeline> |
---|
| 358 | </programlisting> |
---|
| 359 | <location file="/home/smidl/work/git/mixpp/bdm/estim/libKF.cpp"/> |
---|
| 360 | </compounddef> |
---|
| 361 | </doxygen> |
---|