| 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="Kalman_8m" kind="file"> |
|---|
| 4 | <compoundname>Kalman.m</compoundname> |
|---|
| 5 | <sectiondef kind="var"> |
|---|
| 6 | <memberdef kind="variable" id="Kalman_8m_1a2d0bade8b7ef0b56beda27488ea708d" prot="public" static="no" mutable="no"> |
|---|
| 7 | <type></type> |
|---|
| 8 | <definition>function[xt, Rx, ll]</definition> |
|---|
| 9 | <argsstring>[xt, Rx, ll]</argsstring> |
|---|
| 10 | <name>function</name> |
|---|
| 11 | <initializer><ref refid="classKalman" kindref="compound">Kalman</ref>(xt,yt,A,C,Rw,Rv,Rx) |
|---|
| 12 | % yp=C*xt</initializer> |
|---|
| 13 | <briefdescription> |
|---|
| 14 | </briefdescription> |
|---|
| 15 | <detaileddescription> |
|---|
| 16 | </detaileddescription> |
|---|
| 17 | <inbodydescription> |
|---|
| 18 | </inbodydescription> |
|---|
| 19 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="2" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="1" bodyend="-1"/> |
|---|
| 20 | </memberdef> |
|---|
| 21 | <memberdef kind="variable" id="Kalman_8m_188c32bd7de17dde57a9a412b9a000e11" prot="public" static="no" mutable="no"> |
|---|
| 22 | <type>predikovan v stupy</type> |
|---|
| 23 | <definition>Ry</definition> |
|---|
| 24 | <argsstring></argsstring> |
|---|
| 25 | <name>Ry</name> |
|---|
| 26 | <initializer>Rv+C*Rx*C'</initializer> |
|---|
| 27 | <briefdescription> |
|---|
| 28 | </briefdescription> |
|---|
| 29 | <detaileddescription> |
|---|
| 30 | </detaileddescription> |
|---|
| 31 | <inbodydescription> |
|---|
| 32 | </inbodydescription> |
|---|
| 33 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="3" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="3" bodyend="-1"/> |
|---|
| 34 | </memberdef> |
|---|
| 35 | <memberdef kind="variable" id="Kalman_8m_17385c34888ce96c459b2ab999e756e9b" prot="public" static="no" mutable="no"> |
|---|
| 36 | <type>kovariance inovace</type> |
|---|
| 37 | <definition>asov updt stavu Rx</definition> |
|---|
| 38 | <argsstring></argsstring> |
|---|
| 39 | <name>Rx</name> |
|---|
| 40 | <initializer>Rx-Rx*C'*inv(Ry)*C*Rx</initializer> |
|---|
| 41 | <briefdescription> |
|---|
| 42 | </briefdescription> |
|---|
| 43 | <detaileddescription> |
|---|
| 44 | </detaileddescription> |
|---|
| 45 | <inbodydescription> |
|---|
| 46 | </inbodydescription> |
|---|
| 47 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="4" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="4" bodyend="-1"/> |
|---|
| 48 | </memberdef> |
|---|
| 49 | <memberdef kind="variable" id="Kalman_8m_1109d43e4753ed77f192edf5a278d31e8" prot="public" static="no" mutable="no"> |
|---|
| 50 | <type>datov updt kov stavu</type> |
|---|
| 51 | <definition>datov updt kov stavu ey</definition> |
|---|
| 52 | <argsstring></argsstring> |
|---|
| 53 | <name>ey</name> |
|---|
| 54 | <initializer>yt-yp</initializer> |
|---|
| 55 | <briefdescription> |
|---|
| 56 | </briefdescription> |
|---|
| 57 | <detaileddescription> |
|---|
| 58 | </detaileddescription> |
|---|
| 59 | <inbodydescription> |
|---|
| 60 | </inbodydescription> |
|---|
| 61 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="5" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="5" bodyend="-1"/> |
|---|
| 62 | </memberdef> |
|---|
| 63 | <memberdef kind="variable" id="Kalman_8m_17ef544e75392f3a67bfc3e470f4ff3a4" prot="public" static="no" mutable="no"> |
|---|
| 64 | <type>inovace</type> |
|---|
| 65 | <definition>inovace KG</definition> |
|---|
| 66 | <argsstring></argsstring> |
|---|
| 67 | <name>KG</name> |
|---|
| 68 | <initializer>Rx*C'*inv(Rv)</initializer> |
|---|
| 69 | <briefdescription> |
|---|
| 70 | </briefdescription> |
|---|
| 71 | <detaileddescription> |
|---|
| 72 | </detaileddescription> |
|---|
| 73 | <inbodydescription> |
|---|
| 74 | </inbodydescription> |
|---|
| 75 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="7" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="7" bodyend="-1"/> |
|---|
| 76 | </memberdef> |
|---|
| 77 | <memberdef kind="variable" id="Kalman_8m_1e99e8f063b011dafba65cd94236b07b9" prot="public" static="no" mutable="no"> |
|---|
| 78 | <type><ref refid="classKalman" kindref="compound">Kalman</ref> v gain</type> |
|---|
| 79 | <definition>xt</definition> |
|---|
| 80 | <argsstring></argsstring> |
|---|
| 81 | <name>xt</name> |
|---|
| 82 | <initializer>xt+KG*ey</initializer> |
|---|
| 83 | <briefdescription> |
|---|
| 84 | </briefdescription> |
|---|
| 85 | <detaileddescription> |
|---|
| 86 | </detaileddescription> |
|---|
| 87 | <inbodydescription> |
|---|
| 88 | </inbodydescription> |
|---|
| 89 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="8" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="8" bodyend="-1"/> |
|---|
| 90 | </memberdef> |
|---|
| 91 | <memberdef kind="variable" id="Kalman_8m_1d228be362b719d6a232f91c2998779fa" prot="public" static="no" mutable="no"> |
|---|
| 92 | <type>datov updt stavu</type> |
|---|
| 93 | <definition>datov updt stavu yp</definition> |
|---|
| 94 | <argsstring></argsstring> |
|---|
| 95 | <name>yp</name> |
|---|
| 96 | <initializer>yt-C*xt</initializer> |
|---|
| 97 | <briefdescription> |
|---|
| 98 | </briefdescription> |
|---|
| 99 | <detaileddescription> |
|---|
| 100 | </detaileddescription> |
|---|
| 101 | <inbodydescription> |
|---|
| 102 | </inbodydescription> |
|---|
| 103 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="9" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="9" bodyend="-1"/> |
|---|
| 104 | </memberdef> |
|---|
| 105 | <memberdef kind="variable" id="Kalman_8m_116e4ef534cec559430e07e05eb71c719" prot="public" static="no" mutable="no"> |
|---|
| 106 | <type></type> |
|---|
| 107 | <definition>K</definition> |
|---|
| 108 | <argsstring></argsstring> |
|---|
| 109 | <name>K</name> |
|---|
| 110 | <initializer>Rx*C'*inv(Ry)</initializer> |
|---|
| 111 | <briefdescription> |
|---|
| 112 | </briefdescription> |
|---|
| 113 | <detaileddescription> |
|---|
| 114 | </detaileddescription> |
|---|
| 115 | <inbodydescription> |
|---|
| 116 | </inbodydescription> |
|---|
| 117 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="11" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="11" bodyend="-1"/> |
|---|
| 118 | </memberdef> |
|---|
| 119 | <memberdef kind="variable" id="Kalman_8m_1d5583c89e1658b14ba2a7dccd35666f7" prot="public" static="no" mutable="no"> |
|---|
| 120 | <type>asov updt kov stavu</type> |
|---|
| 121 | <definition>asov updt kov stavu ll</definition> |
|---|
| 122 | <argsstring></argsstring> |
|---|
| 123 | <name>ll</name> |
|---|
| 124 | <initializer>-1/2*log(det(Ry))-1/2*yp'*inv(Ry)*yp</initializer> |
|---|
| 125 | <briefdescription> |
|---|
| 126 | </briefdescription> |
|---|
| 127 | <detaileddescription> |
|---|
| 128 | </detaileddescription> |
|---|
| 129 | <inbodydescription> |
|---|
| 130 | </inbodydescription> |
|---|
| 131 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m" line="18" bodyfile="/home/smidl/work/mixpp/matlab/Kalman.m" bodystart="18" bodyend="-1"/> |
|---|
| 132 | </memberdef> |
|---|
| 133 | </sectiondef> |
|---|
| 134 | <briefdescription> |
|---|
| 135 | </briefdescription> |
|---|
| 136 | <detaileddescription> |
|---|
| 137 | </detaileddescription> |
|---|
| 138 | <programlisting> |
|---|
| 139 | <codeline lineno="1"><highlight class="normal">function<sp/>[xt,Rx,ll]=<ref refid="classKalman" kindref="compound" tooltip="Kalman filter with covariance matrices in square root form.">Kalman</ref>(xt,yt,A,C,Rw,Rv,Rx)</highlight></codeline> |
|---|
| 140 | <codeline lineno="2"><highlight class="normal">%<sp/>yp=C*xt;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>predikovan�<sp/>v�stupy</highlight></codeline> |
|---|
| 141 | <codeline lineno="3"><highlight class="normal">%<sp/>Ry=Rv+C*Rx*C';<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>kovariance<sp/>inovace</highlight></codeline> |
|---|
| 142 | <codeline lineno="4"><highlight class="normal">%<sp/>Rx=Rx-Rx*C'*inv(Ry)*C*Rx;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>datov�<sp/>updt<sp/>kov.<sp/>stavu</highlight></codeline> |
|---|
| 143 | <codeline lineno="5"><highlight class="normal">%<sp/>ey=yt-yp;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>inovace<sp/></highlight></codeline> |
|---|
| 144 | <codeline lineno="6"><highlight class="normal">%<sp/></highlight></codeline> |
|---|
| 145 | <codeline lineno="7"><highlight class="normal">%<sp/>KG=Rx*C</highlight><highlight class="stringliteral">'*inv(Rv);<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>Kalman�v<sp/>gain</highlight></codeline> |
|---|
| 146 | <codeline lineno="8"><highlight class="stringliteral">%<sp/>xt=xt+KG*ey;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>datov�<sp/>updt<sp/>stavu</highlight></codeline> |
|---|
| 147 | <codeline lineno="9"><highlight class="stringliteral">yp=yt-C*xt;</highlight></codeline> |
|---|
| 148 | <codeline lineno="10"><highlight class="stringliteral">Ry=C*Rx*C'</highlight><highlight class="normal">+Rv;</highlight></codeline> |
|---|
| 149 | <codeline lineno="11"><highlight class="normal">K=Rx*C</highlight><highlight class="stringliteral">'*inv(Ry);</highlight></codeline> |
|---|
| 150 | <codeline lineno="12"><highlight class="stringliteral">xt=xt+K*yp;</highlight></codeline> |
|---|
| 151 | <codeline lineno="13"><highlight class="stringliteral">Rx=(eye(length(C))-K*C)*Rx;</highlight></codeline> |
|---|
| 152 | <codeline lineno="14"><highlight class="stringliteral"></highlight></codeline> |
|---|
| 153 | <codeline lineno="15"><highlight class="stringliteral">xt=A*xt;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>�asov�<sp/>updt<sp/>stavu</highlight></codeline> |
|---|
| 154 | <codeline lineno="16"><highlight class="stringliteral">Rx=Rw+A*Rx*A'</highlight><highlight class="normal">;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>%<sp/>�asov�<sp/>updt<sp/>kov.<sp/>stavu</highlight></codeline> |
|---|
| 155 | <codeline lineno="17"><highlight class="normal"></highlight></codeline> |
|---|
| 156 | <codeline lineno="18"><highlight class="normal">ll=-1/2*log(det(Ry))-1/2*yp'*inv(Ry)*yp;</highlight></codeline> |
|---|
| 157 | <codeline lineno="19"><highlight class="normal">%keyboard</highlight></codeline> |
|---|
| 158 | </programlisting> |
|---|
| 159 | <location file="/home/smidl/work/mixpp/matlab/Kalman.m"/> |
|---|
| 160 | </compounddef> |
|---|
| 161 | </doxygen> |
|---|