<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
<title>CKalmanFilterCapable.h Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>
<div align="left"><a href="http://www.mrpt.org/">Main MRPT website</a> > <b>C++ reference</b> </div>
<div align="right">
<a href="index.html"><img border="0" src="mrpt_logo.png" alt="MRPT logo"></a>
</div>
<!-- Generated by Doxygen 1.7.5 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<div id="navrow1" class="tabs">
<ul class="tablist">
<li><a href="index.html"><span>Main Page</span></a></li>
<li><a href="pages.html"><span>Related Pages</span></a></li>
<li><a href="modules.html"><span>Modules</span></a></li>
<li><a href="namespaces.html"><span>Namespaces</span></a></li>
<li><a href="annotated.html"><span>Classes</span></a></li>
<li class="current"><a href="files.html"><span>Files</span></a></li>
<li>
<div id="MSearchBox" class="MSearchBoxInactive">
<div class="left">
<form id="FSearchBox" action="search.php" method="get">
<img id="MSearchSelect" src="search/mag.png" alt=""/>
<input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"/>
</form>
</div><div class="right"></div>
</div>
</li>
</ul>
</div>
<div id="navrow2" class="tabs2">
<ul class="tablist">
<li><a href="files.html"><span>File List</span></a></li>
<li><a href="globals.html"><span>File Members</span></a></li>
</ul>
</div>
<div class="header">
<div class="headertitle">
<div class="title">CKalmanFilterCapable.h</div> </div>
</div>
<div class="contents">
<a href="_c_kalman_filter_capable_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* +---------------------------------------------------------------------------+</span>
<a name="l00002"></a>00002 <span class="comment"> | The Mobile Robot Programming Toolkit (MRPT) C++ library |</span>
<a name="l00003"></a>00003 <span class="comment"> | |</span>
<a name="l00004"></a>00004 <span class="comment"> | http://www.mrpt.org/ |</span>
<a name="l00005"></a>00005 <span class="comment"> | |</span>
<a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2011 University of Malaga |</span>
<a name="l00007"></a>00007 <span class="comment"> | |</span>
<a name="l00008"></a>00008 <span class="comment"> | This software was written by the Machine Perception and Intelligent |</span>
<a name="l00009"></a>00009 <span class="comment"> | Robotics Lab, University of Malaga (Spain). |</span>
<a name="l00010"></a>00010 <span class="comment"> | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |</span>
<a name="l00011"></a>00011 <span class="comment"> | |</span>
<a name="l00012"></a>00012 <span class="comment"> | This file is part of the MRPT project. |</span>
<a name="l00013"></a>00013 <span class="comment"> | |</span>
<a name="l00014"></a>00014 <span class="comment"> | MRPT is free software: you can redistribute it and/or modify |</span>
<a name="l00015"></a>00015 <span class="comment"> | it under the terms of the GNU General Public License as published by |</span>
<a name="l00016"></a>00016 <span class="comment"> | the Free Software Foundation, either version 3 of the License, or |</span>
<a name="l00017"></a>00017 <span class="comment"> | (at your option) any later version. |</span>
<a name="l00018"></a>00018 <span class="comment"> | |</span>
<a name="l00019"></a>00019 <span class="comment"> | MRPT is distributed in the hope that it will be useful, |</span>
<a name="l00020"></a>00020 <span class="comment"> | but WITHOUT ANY WARRANTY; without even the implied warranty of |</span>
<a name="l00021"></a>00021 <span class="comment"> | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |</span>
<a name="l00022"></a>00022 <span class="comment"> | GNU General Public License for more details. |</span>
<a name="l00023"></a>00023 <span class="comment"> | |</span>
<a name="l00024"></a>00024 <span class="comment"> | You should have received a copy of the GNU General Public License |</span>
<a name="l00025"></a>00025 <span class="comment"> | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |</span>
<a name="l00026"></a>00026 <span class="comment"> | |</span>
<a name="l00027"></a>00027 <span class="comment"> +---------------------------------------------------------------------------+ */</span>
<a name="l00028"></a>00028 <span class="preprocessor">#ifndef CKalmanFilterCapable_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define CKalmanFilterCapable_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="_c_matrix_fixed_numeric_8h.html">mrpt/math/CMatrixFixedNumeric.h</a>></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="_c_matrix_template_numeric_8h.html">mrpt/math/CMatrixTemplateNumeric.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_array_8h.html">mrpt/math/CArray.h</a>></span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="base_2include_2mrpt_2math_2utils_8h.html">mrpt/math/utils.h</a>></span>
<a name="l00035"></a>00035
<a name="l00036"></a>00036 <span class="preprocessor">#include <<a class="code" href="_c_time_logger_8h.html">mrpt/utils/CTimeLogger.h</a>></span>
<a name="l00037"></a>00037 <span class="preprocessor">#include <<a class="code" href="_c_loadable_options_8h.html">mrpt/utils/CLoadableOptions.h</a>></span>
<a name="l00038"></a>00038 <span class="preprocessor">#include <<a class="code" href="_c_debug_output_capable_8h.html">mrpt/utils/CDebugOutputCapable.h</a>></span>
<a name="l00039"></a>00039 <span class="preprocessor">#include <<a class="code" href="stl__extensions_8h.html">mrpt/utils/stl_extensions.h</a>></span>
<a name="l00040"></a>00040 <span class="preprocessor">#include <<a class="code" href="os_8h.html">mrpt/system/os.h</a>></span>
<a name="l00041"></a>00041 <span class="preprocessor">#include <<a class="code" href="_c_tic_tac_8h.html">mrpt/utils/CTicTac.h</a>></span>
<a name="l00042"></a>00042 <span class="preprocessor">#include <<a class="code" href="_c_file_output_stream_8h.html">mrpt/utils/CFileOutputStream.h</a>></span>
<a name="l00043"></a>00043 <span class="preprocessor">#include <<a class="code" href="_t_enum_type_8h.html">mrpt/utils/TEnumType.h</a>></span>
<a name="l00044"></a>00044
<a name="l00045"></a>00045 <span class="preprocessor">#include <<a class="code" href="bayes_2include_2mrpt_2bayes_2link__pragmas_8h.html">mrpt/bayes/link_pragmas.h</a>></span>
<a name="l00046"></a>00046
<a name="l00047"></a>00047
<a name="l00048"></a>00048 <span class="keyword">namespace </span>mrpt
<a name="l00049"></a>00049 {
<a name="l00050"></a>00050 <span class="keyword">namespace </span>bayes
<a name="l00051"></a>00051 {
<a name="l00052"></a>00052 <span class="keyword">using namespace </span>mrpt::utils;
<a name="l00053"></a>00053 <span class="keyword">using namespace </span>mrpt::math;
<a name="l00054"></a>00054 <span class="keyword">using namespace </span>mrpt;
<a name="l00055"></a>00055 <span class="keyword">using namespace </span>std;
<a name="l00056"></a>00056 <span class="comment"></span>
<a name="l00057"></a>00057 <span class="comment"> /** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable</span>
<a name="l00058"></a>00058 <span class="comment"> * For further details on each algorithm see the tutorial: http://www.mrpt.org/Kalman_Filters</span>
<a name="l00059"></a>00059 <span class="comment"> * \sa bayes::CKalmanFilterCapable::KF_options</span>
<a name="l00060"></a>00060 <span class="comment"> * \ingroup mrpt_bayes_grp</span>
<a name="l00061"></a>00061 <span class="comment"> */</span>
<a name="l00062"></a><a class="code" href="group__mrpt__bayes__grp.html#ga93ff73c3a01619f01d97fd80dff40d14">00062</a> <span class="keyword">enum</span> <a class="code" href="group__mrpt__bayes__grp.html#ga93ff73c3a01619f01d97fd80dff40d14" title="The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...">TKFMethod</a> {
<a name="l00063"></a><a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a5ad91d60187b5cca61d25994f7827dc0">00063</a> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a5ad91d60187b5cca61d25994f7827dc0">kfEKFNaive</a> = 0,
<a name="l00064"></a><a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14acb25ab5d45035eb456de1c7250dbcf5a">00064</a> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14acb25ab5d45035eb456de1c7250dbcf5a">kfEKFAlaDavison</a>,
<a name="l00065"></a><a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14ac5aee4885e69b59edf793342e96acab3">00065</a> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14ac5aee4885e69b59edf793342e96acab3">kfIKFFull</a>,
<a name="l00066"></a><a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a0c3af1381c1d9b4b4ebb04370dc59ba9">00066</a> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a0c3af1381c1d9b4b4ebb04370dc59ba9">kfIKF</a>
<a name="l00067"></a>00067 };
<a name="l00068"></a>00068
<a name="l00069"></a>00069 <span class="comment">// Forward declaration:</span>
<a name="l00070"></a>00070 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> FEAT_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE> <span class="keyword">class </span>CKalmanFilterCapable;
<a name="l00071"></a>00071
<a name="l00072"></a><a class="code" href="namespacemrpt_1_1bayes_1_1detail.html">00072</a> <span class="keyword">namespace </span>detail {
<a name="l00073"></a>00073 <span class="keyword">struct </span><a class="code" href="structmrpt_1_1bayes_1_1detail_1_1_c_run_one_kalman_iteration__add_new_landmarks.html">CRunOneKalmanIteration_addNewLandmarks</a>;
<a name="l00074"></a>00074 }
<a name="l00075"></a>00075 <span class="comment"></span>
<a name="l00076"></a>00076 <span class="comment"> /** Generic options for the Kalman Filter algorithm in itself.</span>
<a name="l00077"></a>00077 <span class="comment"> * \ingroup mrpt_bayes_grp</span>
<a name="l00078"></a>00078 <span class="comment"> */</span>
<a name="l00079"></a>00079 <span class="keyword">struct </span><a class="code" href="bayes_2include_2mrpt_2bayes_2link__pragmas_8h.html#a7ae1d44be57c866339914ce9f771ff5a">BAYES_IMPEXP</a> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html" title="Generic options for the Kalman Filter algorithm in itself.">TKF_options</a> : <span class="keyword">public</span> utils::<a class="code" href="classmrpt_1_1utils_1_1_c_loadable_options.html" title="This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...">CLoadableOptions</a>
<a name="l00080"></a>00080 {
<a name="l00081"></a>00081 <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html" title="Generic options for the Kalman Filter algorithm in itself.">TKF_options</a>();
<a name="l00082"></a>00082
<a name="l00083"></a>00083 <span class="keywordtype">void</span> loadFromConfigFile(
<a name="l00084"></a>00084 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1utils_1_1_c_config_file_base.html" title="This class allows loading and storing values and vectors of different types from a configuration text...">mrpt::utils::CConfigFileBase</a> &source,
<a name="l00085"></a>00085 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &section);
<a name="l00086"></a>00086 <span class="comment"></span>
<a name="l00087"></a>00087 <span class="comment"> /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. */</span>
<a name="l00088"></a>00088 <span class="keywordtype">void</span> dumpToTextStream(<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">CStream</a> &out) <span class="keyword">const</span>;
<a name="l00089"></a>00089
<a name="l00090"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a136e211bf1136d8437ad907782c2967d">00090</a> <a class="code" href="group__mrpt__bayes__grp.html#ga93ff73c3a01619f01d97fd80dff40d14" title="The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...">TKFMethod</a> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a136e211bf1136d8437ad907782c2967d" title="The method to employ (default: kfEKFNaive)">method</a>; <span class="comment">//!< The method to employ (default: kfEKFNaive)</span>
<a name="l00091"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a79d277e2e9ac720106e7665883aeb81d">00091</a> <span class="comment"></span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a79d277e2e9ac720106e7665883aeb81d" title="If set to true timing and other information will be dumped during the execution (default=false)">verbose</a>; <span class="comment">//!< If set to true timing and other information will be dumped during the execution (default=false)</span>
<a name="l00092"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a512bf4ff5350affab3d5e94c03b7da1a">00092</a> <span class="comment"></span> <span class="keywordtype">int</span> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a512bf4ff5350affab3d5e94c03b7da1a" title="Number of refinement iterations, only for the IKF method.">IKF_iterations</a>; <span class="comment">//!< Number of refinement iterations, only for the IKF method.</span>
<a name="l00093"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a57b4a351be8de935f8c2c2357b3d8b5c">00093</a> <span class="comment"></span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a57b4a351be8de935f8c2c2357b3d8b5c" title="If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...">enable_profiler</a>;<span class="comment">//!< If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLog at the end of the execution.</span>
<a name="l00094"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#aa628121a80c83a8e7a75363df9536048">00094</a> <span class="comment"></span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#aa628121a80c83a8e7a75363df9536048" title="(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...">use_analytic_transition_jacobian</a>; <span class="comment">//!< (default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimated from a numeric approximation by calling several times to OnTransitionModel.</span>
<a name="l00095"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#abe0d010c6e68f8270175362293fbcc17">00095</a> <span class="comment"></span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#abe0d010c6e68f8270175362293fbcc17" title="(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...">use_analytic_observation_jacobian</a>; <span class="comment">//!< (default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estimated from a numeric approximation by calling several times to OnObservationModel.</span>
<a name="l00096"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a00d30cee24c38bfc6ec470a573646675">00096</a> <span class="comment"></span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a00d30cee24c38bfc6ec470a573646675" title="(default=false) If true, will compute all the Jacobians numerically and compare them to the analytica...">debug_verify_analytic_jacobians</a>; <span class="comment">//!< (default=false) If true, will compute all the Jacobians numerically and compare them to the analytical ones, throwing an exception on mismatch.</span>
<a name="l00097"></a><a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a5239885ee4c9ed75c1c3d6966226cae2">00097</a> <span class="comment"></span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html#a5239885ee4c9ed75c1c3d6966226cae2" title="(default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians...">debug_verify_analytic_jacobians_threshold</a>; <span class="comment">//!< (default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians</span>
<a name="l00098"></a>00098 <span class="comment"></span> };
<a name="l00099"></a>00099 <span class="comment"></span>
<a name="l00100"></a>00100 <span class="comment"> /** Auxiliary functions, for internal usage of MRPT classes */</span>
<a name="l00101"></a>00101 <span class="keyword">namespace </span>detail
<a name="l00102"></a>00102 {
<a name="l00103"></a>00103 <span class="comment">// Auxiliary functions.</span>
<a name="l00104"></a>00104 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> FEAT_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l00105"></a>00105 <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aeec894a458a3e7175e707871cd796f34">getNumberOfLandmarksInMap</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE></a> &obj);
<a name="l00106"></a>00106 <span class="comment">// Specialization:</span>
<a name="l00107"></a>00107 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l00108"></a>00108 <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aeec894a458a3e7175e707871cd796f34">getNumberOfLandmarksInMap</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE></a> &obj);
<a name="l00109"></a>00109
<a name="l00110"></a>00110 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> FEAT_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l00111"></a>00111 <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aafff1b2b519f6fb209de6ebde0830c19">isMapEmpty</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE></a> &obj);
<a name="l00112"></a>00112 <span class="comment">// Specialization:</span>
<a name="l00113"></a>00113 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l00114"></a>00114 <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aafff1b2b519f6fb209de6ebde0830c19">isMapEmpty</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE></a> &obj);
<a name="l00115"></a>00115 }
<a name="l00116"></a>00116
<a name="l00117"></a>00117 <span class="comment"></span>
<a name="l00118"></a>00118 <span class="comment"> /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.</span>
<a name="l00119"></a>00119 <span class="comment"> * This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed</span>
<a name="l00120"></a>00120 <span class="comment"> * by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which</span>
<a name="l00121"></a>00121 <span class="comment"> * should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class.</span>
<a name="l00122"></a>00122 <span class="comment"> * Note that the main entry point is protected, so derived classes must offer another method more specific to a given problem which, internally, calls runOneKalmanIteration.</span>
<a name="l00123"></a>00123 <span class="comment"> *</span>
<a name="l00124"></a>00124 <span class="comment"> * For further details and examples, check out the tutorial: http://www.mrpt.org/Kalman_Filters</span>
<a name="l00125"></a>00125 <span class="comment"> *</span>
<a name="l00126"></a>00126 <span class="comment"> * The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation</span>
<a name="l00127"></a>00127 <span class="comment"> * of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.</span>
<a name="l00128"></a>00128 <span class="comment"> *</span>
<a name="l00129"></a>00129 <span class="comment"> * The meaning of the template parameters is:</span>
<a name="l00130"></a>00130 <span class="comment"> * - VEH_SIZE: The dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.</span>
<a name="l00131"></a>00131 <span class="comment"> * - OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).</span>
<a name="l00132"></a>00132 <span class="comment"> * - FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).</span>
<a name="l00133"></a>00133 <span class="comment"> * - ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).</span>
<a name="l00134"></a>00134 <span class="comment"> * - KFTYPE: The numeric type of the matrices (default: double)</span>
<a name="l00135"></a>00135 <span class="comment"> *</span>
<a name="l00136"></a>00136 <span class="comment"> * Revisions:</span>
<a name="l00137"></a>00137 <span class="comment"> * - 2007: Antonio J. Ortiz de Galisteo (AJOGD)</span>
<a name="l00138"></a>00138 <span class="comment"> * - 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).</span>
<a name="l00139"></a>00139 <span class="comment"> * - 2008/MAR: Implemented IKF (JLBC).</span>
<a name="l00140"></a>00140 <span class="comment"> * - 2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).</span>
<a name="l00141"></a>00141 <span class="comment"> *</span>
<a name="l00142"></a>00142 <span class="comment"> * \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D</span>
<a name="l00143"></a>00143 <span class="comment"> * \ingroup mrpt_bayes_grp</span>
<a name="l00144"></a>00144 <span class="comment"> */</span>
<a name="l00145"></a>00145 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> FEAT_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE = <span class="keywordtype">double</span>>
<a name="l00146"></a>00146 <span class="keyword">class </span>CKalmanFilterCapable : <span class="keyword">public</span> mrpt::utils::<a class="code" href="classmrpt_1_1utils_1_1_c_debug_output_capable.html" title="This base class provides a common printf-like method to send debug information to std::cout...">CDebugOutputCapable</a>
<a name="l00147"></a>00147 {
<a name="l00148"></a>00148 <span class="keyword">public</span>:
<a name="l00149"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6229386413a59a767a5197fca67e56cf">00149</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6229386413a59a767a5197fca67e56cf">get_vehicle_size</a>() { <span class="keywordflow">return</span> VEH_SIZE; }
<a name="l00150"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ae1bc73d658a9a0e1ca3615a523624434">00150</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ae1bc73d658a9a0e1ca3615a523624434">get_observation_size</a>() { <span class="keywordflow">return</span> OBS_SIZE; }
<a name="l00151"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad19146ca0b12bed3c2aace9329ece0fd">00151</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad19146ca0b12bed3c2aace9329ece0fd">get_feature_size</a>() { <span class="keywordflow">return</span> FEAT_SIZE; }
<a name="l00152"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#abaeadbd68b3a68d35e530a29b7c193b2">00152</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#abaeadbd68b3a68d35e530a29b7c193b2">get_action_size</a>() { <span class="keywordflow">return</span> ACT_SIZE; }
<a name="l00153"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a1ac5b0adbbd3a0e5122ebb148ff3e7ad">00153</a> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a1ac5b0adbbd3a0e5122ebb148ff3e7ad">getNumberOfLandmarksInTheMap</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aeec894a458a3e7175e707871cd796f34">detail::getNumberOfLandmarksInMap</a>(*<span class="keyword">this</span>); }
<a name="l00154"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a48adbb9852e76624b16e7fa115c19e65">00154</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a48adbb9852e76624b16e7fa115c19e65">isMapEmpty</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aafff1b2b519f6fb209de6ebde0830c19">detail::isMapEmpty</a>(*<span class="keyword">this</span>); }
<a name="l00155"></a>00155
<a name="l00156"></a>00156
<a name="l00157"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad66228d2cc13705900669ccbab41f3d9">00157</a> <span class="keyword">typedef</span> KFTYPE <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad66228d2cc13705900669ccbab41f3d9" title="The numeric type used in the Kalman Filter (default=double)">kftype</a>; <span class="comment">//!< The numeric type used in the Kalman Filter (default=double)</span>
<a name="l00158"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6195a8efec17869133a1fd31105d6a64">00158</a> <span class="comment"></span> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6195a8efec17869133a1fd31105d6a64" title="My class, in a shorter name!">KFCLASS</a>; <span class="comment">//!< My class, in a shorter name!</span>
<a name="l00159"></a>00159 <span class="comment"></span>
<a name="l00160"></a>00160 <span class="comment">// ---------- Many useful typedefs to short the notation a bit... --------</span>
<a name="l00161"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a9a120b286d286048985b059aa721e0c1">00161</a> <span class="keyword">typedef</span> mrpt<a class="code" href="structmrpt_1_1dynamicsize__vector.html">::dynamicsize_vector<KFTYPE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a9a120b286d286048985b059aa721e0c1">KFVector</a>;
<a name="l00162"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad214112622ea8531c9a7c052f93600bd">00162</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixTemplateNumeric<KFTYPE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad214112622ea8531c9a7c052f93600bd">KFMatrix</a>;
<a name="l00163"></a>00163
<a name="l00164"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a8dd3e63dd847fd98ef59b472e46cad2c">00164</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,VEH_SIZE,VEH_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a8dd3e63dd847fd98ef59b472e46cad2c">KFMatrix_VxV</a>;
<a name="l00165"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a521a75ae040b78365ebf57f97967bc84">00165</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,OBS_SIZE,OBS_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a521a75ae040b78365ebf57f97967bc84">KFMatrix_OxO</a>;
<a name="l00166"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad8a2b4b92969f736bf6c147804b50614">00166</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,FEAT_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad8a2b4b92969f736bf6c147804b50614">KFMatrix_FxF</a>;
<a name="l00167"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#af20d435cc094724e00949ea137aa832b">00167</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,ACT_SIZE,ACT_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#af20d435cc094724e00949ea137aa832b">KFMatrix_AxA</a>;
<a name="l00168"></a>00168
<a name="l00169"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a60912a7a33cd8e77605e4761848609b2">00169</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,VEH_SIZE,OBS_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a60912a7a33cd8e77605e4761848609b2">KFMatrix_VxO</a>;
<a name="l00170"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a7277a24dd3aae42b96708b4669e74a12">00170</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,VEH_SIZE,FEAT_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a7277a24dd3aae42b96708b4669e74a12">KFMatrix_VxF</a>;
<a name="l00171"></a>00171
<a name="l00172"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#affb2aa897e6434f6572d48ff1be1988e">00172</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,VEH_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#affb2aa897e6434f6572d48ff1be1988e">KFMatrix_FxV</a>;
<a name="l00173"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a253bf3d53d25e20ec0592868efdba607">00173</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,OBS_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a253bf3d53d25e20ec0592868efdba607">KFMatrix_FxO</a>;
<a name="l00174"></a>00174
<a name="l00175"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ada78f649a58aa64c3db6c8b436b5ceba">00175</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,OBS_SIZE,FEAT_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ada78f649a58aa64c3db6c8b436b5ceba">KFMatrix_OxF</a>;
<a name="l00176"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ab0ddb67c05616e7fc1fcabe9cc04c753">00176</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<KFTYPE,OBS_SIZE,VEH_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ab0ddb67c05616e7fc1fcabe9cc04c753">KFMatrix_OxV</a>;
<a name="l00177"></a>00177
<a name="l00178"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad4f1667845ca7553925160142821cff0">00178</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">CArrayNumeric<KFTYPE,VEH_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad4f1667845ca7553925160142821cff0">KFArray_VEH</a>;
<a name="l00179"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6454c68e3c45da92e2bbb3e8aa8f6f70">00179</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">CArrayNumeric<KFTYPE,ACT_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6454c68e3c45da92e2bbb3e8aa8f6f70">KFArray_ACT</a>;
<a name="l00180"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6c1b24a2c7b35e77411888a06de4e59d">00180</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">CArrayNumeric<KFTYPE,OBS_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6c1b24a2c7b35e77411888a06de4e59d">KFArray_OBS</a>;
<a name="l00181"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aa0e71d2a45e5d935eba18d817bb31cf9">00181</a> <span class="keyword">typedef</span> <span class="keyword">typename</span> mrpt<a class="code" href="classstd_1_1vector.html" title="STL class.">::aligned_containers<KFArray_OBS>::vector_t</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aa0e71d2a45e5d935eba18d817bb31cf9">vector_KFArray_OBS</a>;
<a name="l00182"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a0b5b72d68126a91918ce085789da06bd">00182</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">CArrayNumeric<KFTYPE,FEAT_SIZE></a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a0b5b72d68126a91918ce085789da06bd">KFArray_FEAT</a>;
<a name="l00183"></a>00183
<a name="l00184"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aabe244779fd4694bc5e6d336874396de">00184</a> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aabe244779fd4694bc5e6d336874396de">getStateVectorLength</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_xkk.size(); }
<a name="l00185"></a>00185 <span class="comment"></span>
<a name="l00186"></a>00186 <span class="comment"> /** Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).</span>
<a name="l00187"></a>00187 <span class="comment"> * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()</span>
<a name="l00188"></a>00188 <span class="comment"> */</span>
<a name="l00189"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a4cc933fc0dacdd126b08b08591c617f4">00189</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a4cc933fc0dacdd126b08b08591c617f4" title="Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems)...">getLandmarkMean</a>(<span class="keywordtype">size_t</span> idx, <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_FEAT</a> &feat )<span class="keyword"> const </span>{
<a name="l00190"></a>00190 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(idx<getNumberOfLandmarksInTheMap())
<a name="l00191"></a>00191 ::<a class="code" href="group__mrpt__system__os.html#gae1184cfb1f617787dc4c9da98becbe3a" title="An OS and compiler independent version of "memcpy".">memcpy</a>(&feat[0], &m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*<span class="keyword">sizeof</span>(m_xkk[0]));
<a name="l00192"></a>00192 }<span class="comment"></span>
<a name="l00193"></a>00193 <span class="comment"> /** Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).</span>
<a name="l00194"></a>00194 <span class="comment"> * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()</span>
<a name="l00195"></a>00195 <span class="comment"> */</span>
<a name="l00196"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ae32eeb6c71cb7ea1acf21ae09ac19827">00196</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ae32eeb6c71cb7ea1acf21ae09ac19827" title="Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).">getLandmarkCov</a>(<span class="keywordtype">size_t</span> idx, <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_FxF</a> &feat_cov )<span class="keyword"> const </span>{
<a name="l00197"></a>00197 m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
<a name="l00198"></a>00198 }
<a name="l00199"></a>00199
<a name="l00200"></a>00200 <span class="keyword">protected</span>:<span class="comment"></span>
<a name="l00201"></a>00201 <span class="comment"> /** @name Kalman filter state</span>
<a name="l00202"></a>00202 <span class="comment"> @{ */</span>
<a name="l00203"></a>00203
<a name="l00204"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a7e8948c9d209c23517c45c6494b46147">00204</a> <a class="code" href="structmrpt_1_1dynamicsize__vector.html">KFVector</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a7e8948c9d209c23517c45c6494b46147" title="The system state vector.">m_xkk</a>; <span class="comment">//!< The system state vector.</span>
<a name="l00205"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a15c291ca9eb40996381dc0e60f01d533">00205</a> <span class="comment"></span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a15c291ca9eb40996381dc0e60f01d533" title="The system full covariance matrix.">m_pkk</a>; <span class="comment">//!< The system full covariance matrix.</span>
<a name="l00206"></a>00206 <span class="comment"></span><span class="comment"></span>
<a name="l00207"></a>00207 <span class="comment"> /** @} */</span>
<a name="l00208"></a>00208
<a name="l00209"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a3eff55c428065c52bd2f9418e3944e48">00209</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1_c_time_logger.html" title="A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.">::utils::CTimeLogger</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a3eff55c428065c52bd2f9418e3944e48">m_timLogger</a>;
<a name="l00210"></a>00210 <span class="comment"></span>
<a name="l00211"></a>00211 <span class="comment"> /** @name Virtual methods for Kalman Filter implementation</span>
<a name="l00212"></a>00212 <span class="comment"> @{</span>
<a name="l00213"></a>00213 <span class="comment"> */</span>
<a name="l00214"></a>00214 <span class="comment"></span>
<a name="l00215"></a>00215 <span class="comment"> /** Must return the action vector u.</span>
<a name="l00216"></a>00216 <span class="comment"> * \param out_u The action vector which will be passed to OnTransitionModel</span>
<a name="l00217"></a>00217 <span class="comment"> */</span>
<a name="l00218"></a>00218 <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnGetAction( <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_ACT</a> &out_u ) <span class="keyword">const</span> = 0;
<a name="l00219"></a>00219 <span class="comment"></span>
<a name="l00220"></a>00220 <span class="comment"> /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$</span>
<a name="l00221"></a>00221 <span class="comment"> * \param in_u The vector returned by OnGetAction.</span>
<a name="l00222"></a>00222 <span class="comment"> * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must have \f$ \hat{x}_{k|k-1} \f$ .</span>
<a name="l00223"></a>00223 <span class="comment"> * \param out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false</span>
<a name="l00224"></a>00224 <span class="comment"> * \note Even if you return "out_skip=true", the value of "inout_x" MUST be updated as usual (this is to allow numeric approximation of Jacobians).</span>
<a name="l00225"></a>00225 <span class="comment"> */</span>
<a name="l00226"></a>00226 <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnTransitionModel(
<a name="l00227"></a>00227 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_ACT</a> &in_u,
<a name="l00228"></a>00228 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> &inout_x,
<a name="l00229"></a>00229 <span class="keywordtype">bool</span> &out_skipPrediction
<a name="l00230"></a>00230 ) <span class="keyword">const</span> = 0;
<a name="l00231"></a>00231 <span class="comment"></span>
<a name="l00232"></a>00232 <span class="comment"> /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$</span>
<a name="l00233"></a>00233 <span class="comment"> * \param out_F Must return the Jacobian.</span>
<a name="l00234"></a>00234 <span class="comment"> * The returned matrix must be \f$V \times V\f$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).</span>
<a name="l00235"></a>00235 <span class="comment"> */</span>
<a name="l00236"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a9102af2605891c4b76ef1216013da46d">00236</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnTransitionJacobian( <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_VxV</a> &out_F )<span class="keyword"> const</span>
<a name="l00237"></a>00237 <span class="keyword"> </span>{
<a name="l00238"></a>00238 m_user_didnt_implement_jacobian=<span class="keyword">true</span>;
<a name="l00239"></a>00239 }
<a name="l00240"></a>00240 <span class="comment"></span>
<a name="l00241"></a>00241 <span class="comment"> /** Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.</span>
<a name="l00242"></a>00242 <span class="comment"> */</span>
<a name="l00243"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6546398d63e8298f0b0a59a86e2395a7">00243</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnTransitionJacobianNumericGetIncrements(<a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> &out_increments)<span class="keyword"> const</span>
<a name="l00244"></a>00244 <span class="keyword"> </span>{
<a name="l00245"></a>00245 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<VEH_SIZE;i++) out_increments[i] = 1e-6;
<a name="l00246"></a>00246 }
<a name="l00247"></a>00247 <span class="comment"></span>
<a name="l00248"></a>00248 <span class="comment"> /** Implements the transition noise covariance \f$ Q_k \f$</span>
<a name="l00249"></a>00249 <span class="comment"> * \param out_Q Must return the covariance matrix.</span>
<a name="l00250"></a>00250 <span class="comment"> * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian</span>
<a name="l00251"></a>00251 <span class="comment"> */</span>
<a name="l00252"></a>00252 <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnTransitionNoise( KFMatrix_VxV &out_Q ) <span class="keyword">const</span> = 0;
<a name="l00253"></a>00253 <span class="comment"></span>
<a name="l00254"></a>00254 <span class="comment"> /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.</span>
<a name="l00255"></a>00255 <span class="comment"> * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.</span>
<a name="l00256"></a>00256 <span class="comment"> * \param in_all_prediction_means The mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.</span>
<a name="l00257"></a>00257 <span class="comment"> * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.</span>
<a name="l00258"></a>00258 <span class="comment"> * \note This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.</span>
<a name="l00259"></a>00259 <span class="comment"> * \sa OnGetObservations, OnDataAssociation</span>
<a name="l00260"></a>00260 <span class="comment"> */</span>
<a name="l00261"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a505fa1e6eb8a300e0e1171a26b4f5b15">00261</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnPreComputingPredictions(
<a name="l00262"></a>00262 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_KFArray_OBS</a> &in_all_prediction_means,
<a name="l00263"></a>00263 <a class="code" href="classstd_1_1vector.html">mrpt::vector_size_t</a> &out_LM_indices_to_predict )<span class="keyword"> const</span>
<a name="l00264"></a>00264 <span class="keyword"> </span>{
<a name="l00265"></a>00265 <span class="comment">// Default: all of them:</span>
<a name="l00266"></a>00266 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = this->getNumberOfLandmarksInTheMap();
<a name="l00267"></a>00267 out_LM_indices_to_predict.resize(N);
<a name="l00268"></a>00268 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
<a name="l00269"></a>00269 }
<a name="l00270"></a>00270 <span class="comment"></span>
<a name="l00271"></a>00271 <span class="comment"> /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.</span>
<a name="l00272"></a>00272 <span class="comment"> * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.</span>
<a name="l00273"></a>00273 <span class="comment"> * \note Upon call, it can be assumed that the previous contents of out_R are all zeros.</span>
<a name="l00274"></a>00274 <span class="comment"> */</span>
<a name="l00275"></a>00275 <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnGetObservationNoise(KFMatrix_OxO &out_R) <span class="keyword">const</span> = 0;
<a name="l00276"></a>00276 <span class="comment"></span>
<a name="l00277"></a>00277 <span class="comment"> /** This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.</span>
<a name="l00278"></a>00278 <span class="comment"> *</span>
<a name="l00279"></a>00279 <span class="comment"> * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.</span>
<a name="l00280"></a>00280 <span class="comment"> * \param out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.</span>
<a name="l00281"></a>00281 <span class="comment"> * \param in_all_predictions A vector with the prediction of ALL the landmarks in the map. Note that, in contrast, in_S only comprises a subset of all the landmarks.</span>
<a name="l00282"></a>00282 <span class="comment"> * \param in_S The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length of "in_lm_indices_in_S".</span>
<a name="l00283"></a>00283 <span class="comment"> * \param in_lm_indices_in_S The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.</span>
<a name="l00284"></a>00284 <span class="comment"> *</span>
<a name="l00285"></a>00285 <span class="comment"> * This method will be called just once for each complete KF iteration.</span>
<a name="l00286"></a>00286 <span class="comment"> * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.</span>
<a name="l00287"></a>00287 <span class="comment"> */</span>
<a name="l00288"></a>00288 <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnGetObservationsAndDataAssociation(
<a name="l00289"></a>00289 vector_KFArray_OBS &out_z,
<a name="l00290"></a>00290 <a class="code" href="classstd_1_1vector.html" title="STL class.">mrpt::vector_int</a> &out_data_association,
<a name="l00291"></a>00291 <span class="keyword">const</span> vector_KFArray_OBS &in_all_predictions,
<a name="l00292"></a>00292 <span class="keyword">const</span> KFMatrix &in_S,
<a name="l00293"></a>00293 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html">vector_size_t</a> &in_lm_indices_in_S,
<a name="l00294"></a>00294 <span class="keyword">const</span> KFMatrix_OxO &in_R
<a name="l00295"></a>00295 ) = 0;
<a name="l00296"></a>00296 <span class="comment"></span>
<a name="l00297"></a>00297 <span class="comment"> /** Implements the observation prediction \f$ h_i(x) \f$.</span>
<a name="l00298"></a>00298 <span class="comment"> * \param idx_landmark_to_predict The indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem.</span>
<a name="l00299"></a>00299 <span class="comment"> * \param out_predictions The predicted observations.</span>
<a name="l00300"></a>00300 <span class="comment"> */</span>
<a name="l00301"></a>00301 <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnObservationModel(
<a name="l00302"></a>00302 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html">mrpt::vector_size_t</a> &idx_landmarks_to_predict,
<a name="l00303"></a>00303 vector_KFArray_OBS &out_predictions
<a name="l00304"></a>00304 ) <span class="keyword">const</span> = 0;
<a name="l00305"></a>00305 <span class="comment"></span>
<a name="l00306"></a>00306 <span class="comment"> /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.</span>
<a name="l00307"></a>00307 <span class="comment"> * \param idx_landmark_to_predict The index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.</span>
<a name="l00308"></a>00308 <span class="comment"> * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.</span>
<a name="l00309"></a>00309 <span class="comment"> * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.</span>
<a name="l00310"></a>00310 <span class="comment"> */</span>
<a name="l00311"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a45e3d11533732ae6fdc6f0d8a0c6a16e">00311</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnObservationJacobians(
<a name="l00312"></a>00312 <span class="keyword">const</span> <span class="keywordtype">size_t</span> &idx_landmark_to_predict,
<a name="l00313"></a>00313 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxV</a> &Hx,
<a name="l00314"></a>00314 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxF</a> &Hy
<a name="l00315"></a>00315 )<span class="keyword"> const</span>
<a name="l00316"></a>00316 <span class="keyword"> </span>{
<a name="l00317"></a>00317 m_user_didnt_implement_jacobian=<span class="keyword">true</span>;
<a name="l00318"></a>00318 }
<a name="l00319"></a>00319 <span class="comment"></span>
<a name="l00320"></a>00320 <span class="comment"> /** Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.</span>
<a name="l00321"></a>00321 <span class="comment"> */</span>
<a name="l00322"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#acbba7768aa0513f67e30628787e429c2">00322</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnObservationJacobiansNumericGetIncrements(
<a name="l00323"></a>00323 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> &out_veh_increments,
<a name="l00324"></a>00324 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_FEAT</a> &out_feat_increments )<span class="keyword"> const</span>
<a name="l00325"></a>00325 <span class="keyword"> </span>{
<a name="l00326"></a>00326 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<VEH_SIZE;i++) out_veh_increments[i] = 1e-6;
<a name="l00327"></a>00327 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<FEAT_SIZE;i++) out_feat_increments[i] = 1e-6;
<a name="l00328"></a>00328 }
<a name="l00329"></a>00329 <span class="comment"></span>
<a name="l00330"></a>00330 <span class="comment"> /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).</span>
<a name="l00331"></a>00331 <span class="comment"> */</span>
<a name="l00332"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a2875f05e00d512746e6a39244fb28561">00332</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnSubstractObservationVectors(<a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> &A, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> &B)<span class="keyword"> const</span>
<a name="l00333"></a>00333 <span class="keyword"> </span>{
<a name="l00334"></a>00334 A -= B;
<a name="l00335"></a>00335 }
<a name="l00336"></a>00336 <span class="comment"></span>
<a name="l00337"></a>00337 <span class="comment"> /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".</span>
<a name="l00338"></a>00338 <span class="comment"> * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().</span>
<a name="l00339"></a>00339 <span class="comment"> * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.</span>
<a name="l00340"></a>00340 <span class="comment"> * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.</span>
<a name="l00341"></a>00341 <span class="comment"> * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.</span>
<a name="l00342"></a>00342 <span class="comment"> *</span>
<a name="l00343"></a>00343 <span class="comment"> * - O: OBS_SIZE</span>
<a name="l00344"></a>00344 <span class="comment"> * - V: VEH_SIZE</span>
<a name="l00345"></a>00345 <span class="comment"> * - F: FEAT_SIZE</span>
<a name="l00346"></a>00346 <span class="comment"> *</span>
<a name="l00347"></a>00347 <span class="comment"> * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.</span>
<a name="l00348"></a>00348 <span class="comment"> * \deprecated This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.</span>
<a name="l00349"></a>00349 <span class="comment"> */</span>
<a name="l00350"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a647a12e0844df2ead68949d5ed33967e">00350</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnInverseObservationModel(
<a name="l00351"></a>00351 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> & in_z,
<a name="l00352"></a>00352 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_FEAT</a> & out_yn,
<a name="l00353"></a>00353 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_FxV</a> & out_dyn_dxv,
<a name="l00354"></a>00354 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_FxO</a> & out_dyn_dhn )<span class="keyword"> const</span>
<a name="l00355"></a>00355 <span class="keyword"> </span>{
<a name="l00356"></a>00356 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00357"></a>00357 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Inverse sensor model required but not implemented in derived class."</span>)
<a name="l00358"></a>00358 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00359"></a>00359 }
<a name="l00360"></a>00360 <span class="comment"></span>
<a name="l00361"></a>00361 <span class="comment"> /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".</span>
<a name="l00362"></a>00362 <span class="comment"> * The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian),</span>
<a name="l00363"></a>00363 <span class="comment"> * and another from the uncertainty in the observation itself. By default, out_use_dyn_dhn_jacobian=true on call, and if it's left at "true",</span>
<a name="l00364"></a>00364 <span class="comment"> * the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn.</span>
<a name="l00365"></a>00365 <span class="comment"> * Only in some problems (e.g. MonoSLAM), it'll be needed for the application to directly return the covariance matrix \a out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:</span>
<a name="l00366"></a>00366 <span class="comment"> *</span>
<a name="l00367"></a>00367 <span class="comment"> * \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial y_n}{\partial h_n}^\top \f$.</span>
<a name="l00368"></a>00368 <span class="comment"> *</span>
<a name="l00369"></a>00369 <span class="comment"> * but may be computed from additional terms, or whatever needed by the user.</span>
<a name="l00370"></a>00370 <span class="comment"> *</span>
<a name="l00371"></a>00371 <span class="comment"> * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().</span>
<a name="l00372"></a>00372 <span class="comment"> * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.</span>
<a name="l00373"></a>00373 <span class="comment"> * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.</span>
<a name="l00374"></a>00374 <span class="comment"> * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.</span>
<a name="l00375"></a>00375 <span class="comment"> * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.</span>
<a name="l00376"></a>00376 <span class="comment"> *</span>
<a name="l00377"></a>00377 <span class="comment"> * - O: OBS_SIZE</span>
<a name="l00378"></a>00378 <span class="comment"> * - V: VEH_SIZE</span>
<a name="l00379"></a>00379 <span class="comment"> * - F: FEAT_SIZE</span>
<a name="l00380"></a>00380 <span class="comment"> *</span>
<a name="l00381"></a>00381 <span class="comment"> * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.</span>
<a name="l00382"></a>00382 <span class="comment"> */</span>
<a name="l00383"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#af7c70d63832d8d45d8c5de0ed6041109">00383</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnInverseObservationModel(
<a name="l00384"></a>00384 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> & in_z,
<a name="l00385"></a>00385 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_FEAT</a> & out_yn,
<a name="l00386"></a>00386 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_FxV</a> & out_dyn_dxv,
<a name="l00387"></a>00387 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_FxO</a> & out_dyn_dhn,
<a name="l00388"></a>00388 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_FxF</a> & out_dyn_dhn_R_dyn_dhnT,
<a name="l00389"></a>00389 <span class="keywordtype">bool</span> & out_use_dyn_dhn_jacobian
<a name="l00390"></a>00390 )<span class="keyword"> const</span>
<a name="l00391"></a>00391 <span class="keyword"> </span>{
<a name="l00392"></a>00392 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00393"></a>00393 OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
<a name="l00394"></a>00394 out_use_dyn_dhn_jacobian=<span class="keyword">true</span>;
<a name="l00395"></a>00395 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00396"></a>00396 }
<a name="l00397"></a>00397 <span class="comment"></span>
<a name="l00398"></a>00398 <span class="comment"> /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.</span>
<a name="l00399"></a>00399 <span class="comment"> * \param in_obsIndex The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.</span>
<a name="l00400"></a>00400 <span class="comment"> * \param in_idxNewFeat The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.</span>
<a name="l00401"></a>00401 <span class="comment"> * \sa OnInverseObservationModel</span>
<a name="l00402"></a>00402 <span class="comment"> */</span>
<a name="l00403"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a41be072d4693b55225b484c249db5fc2">00403</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnNewLandmarkAddedToMap(
<a name="l00404"></a>00404 <span class="keyword">const</span> <span class="keywordtype">size_t</span> in_obsIdx,
<a name="l00405"></a>00405 <span class="keyword">const</span> <span class="keywordtype">size_t</span> in_idxNewFeat )
<a name="l00406"></a>00406 {
<a name="l00407"></a>00407 <span class="comment">// Do nothing in this base class.</span>
<a name="l00408"></a>00408 }
<a name="l00409"></a>00409 <span class="comment"></span>
<a name="l00410"></a>00410 <span class="comment"> /** This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.</span>
<a name="l00411"></a>00411 <span class="comment"> */</span>
<a name="l00412"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a8a5246ad413ba2f6121b9f7f261283b8">00412</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnNormalizeStateVector()
<a name="l00413"></a>00413 {
<a name="l00414"></a>00414 <span class="comment">// Do nothing in this base class.</span>
<a name="l00415"></a>00415 }
<a name="l00416"></a>00416 <span class="comment"></span>
<a name="l00417"></a>00417 <span class="comment"> /** This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().</span>
<a name="l00418"></a>00418 <span class="comment"> */</span>
<a name="l00419"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a5a3514fd81d6f81b8ff7b2c72dfae33e">00419</a> <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnPostIteration()
<a name="l00420"></a>00420 {
<a name="l00421"></a>00421 <span class="comment">// Do nothing in this base class.</span>
<a name="l00422"></a>00422 }
<a name="l00423"></a>00423 <span class="comment"></span>
<a name="l00424"></a>00424 <span class="comment"> /** @}</span>
<a name="l00425"></a>00425 <span class="comment"> */</span>
<a name="l00426"></a>00426
<a name="l00427"></a>00427 <span class="keyword">public</span>:
<a name="l00428"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a44fae14dfa9b912420b6465fb7e8f7c5">00428</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a44fae14dfa9b912420b6465fb7e8f7c5">CKalmanFilterCapable</a>() {} <span class="comment">//!< Default constructor</span>
<a name="l00429"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a19b3a038e3ba1d3994eab0a08db733b5">00429</a> <span class="comment"></span> <span class="keyword">virtual</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a19b3a038e3ba1d3994eab0a08db733b5" title="Destructor.">~CKalmanFilterCapable</a>() {} <span class="comment">//!< Destructor</span>
<a name="l00430"></a>00430 <span class="comment"></span>
<a name="l00431"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a4c2d71f07ef0501d157988ebccf9f3c0">00431</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1_c_time_logger.html" title="A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.">::utils::CTimeLogger</a> &<a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a4c2d71f07ef0501d157988ebccf9f3c0">getProfiler</a>() { <span class="keywordflow">return</span> m_timLogger; }
<a name="l00432"></a>00432
<a name="l00433"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a88651951bad5ea93ae84d848aeed6414">00433</a> <a class="code" href="structmrpt_1_1bayes_1_1_t_k_f__options.html" title="Generic options for the Kalman Filter algorithm in itself.">TKF_options</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a88651951bad5ea93ae84d848aeed6414" title="Generic options for the Kalman Filter algorithm itself.">KF_options</a>; <span class="comment">//!< Generic options for the Kalman Filter algorithm itself.</span>
<a name="l00434"></a>00434 <span class="comment"></span>
<a name="l00435"></a>00435
<a name="l00436"></a>00436 <span class="keyword">private</span>:
<a name="l00437"></a>00437 <span class="comment">// "Local" variables to runOneKalmanIteration, declared here to avoid</span>
<a name="l00438"></a>00438 <span class="comment">// allocating them over and over again with each call.</span>
<a name="l00439"></a>00439 <span class="comment">// (The variables that go into the stack remains in the function body)</span>
<a name="l00440"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a90762ef91865e4f300226f31cb187828">00440</a> <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_KFArray_OBS</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a90762ef91865e4f300226f31cb187828">all_predictions</a>;
<a name="l00441"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a87c0cfc640df569e33a97749951a6e06">00441</a> <a class="code" href="classstd_1_1vector.html">vector_size_t</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a87c0cfc640df569e33a97749951a6e06">predictLMidxs</a>;
<a name="l00442"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a581ce072177eef66789bf8024f3c2ed7">00442</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a581ce072177eef66789bf8024f3c2ed7">dh_dx</a>;
<a name="l00443"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6dcad04e08c1483c1ed5a0e5c7e5ef79">00443</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a6dcad04e08c1483c1ed5a0e5c7e5ef79">dh_dx_full</a>;
<a name="l00444"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a099172f5abfc72517b79cf221200b9f2">00444</a> <a class="code" href="classstd_1_1vector.html">vector_size_t</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a099172f5abfc72517b79cf221200b9f2">idxs</a>;
<a name="l00445"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a65d721081bd1d54c1d05d88c4279fe42">00445</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a65d721081bd1d54c1d05d88c4279fe42">S</a>;
<a name="l00446"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a65e1419380dea75082a0aa0a09f04332">00446</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a65e1419380dea75082a0aa0a09f04332">Pkk_subset</a>;
<a name="l00447"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aa43fef4c1e7af3dee7ad10053132f47b">00447</a> <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_KFArray_OBS</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aa43fef4c1e7af3dee7ad10053132f47b">Z</a>; <span class="comment">// Each entry is one observation:</span>
<a name="l00448"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aac0376b299cfa494e755b1ddfb512b7e">00448</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aac0376b299cfa494e755b1ddfb512b7e">K</a>; <span class="comment">// Kalman gain</span>
<a name="l00449"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a19e1025c17ff21c2714fd5bcb9d58a9b">00449</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a19e1025c17ff21c2714fd5bcb9d58a9b">S_1</a>; <span class="comment">// Inverse of S</span>
<a name="l00450"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a8eca88f65c4032f3179b8e2e5d13250b">00450</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a8eca88f65c4032f3179b8e2e5d13250b">dh_dx_full_obs</a>;
<a name="l00451"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad9c15457835641e93026e505b3c44ac7">00451</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad9c15457835641e93026e505b3c44ac7">aux_K_dh_dx</a>;
<a name="l00452"></a>00452
<a name="l00453"></a>00453 <span class="keyword">protected</span>:
<a name="l00454"></a>00454 <span class="comment"></span>
<a name="l00455"></a>00455 <span class="comment"> /** The main entry point, executes one complete step: prediction + update.</span>
<a name="l00456"></a>00456 <span class="comment"> * It is protected since derived classes must provide a problem-specific entry point for users.</span>
<a name="l00457"></a>00457 <span class="comment"> * The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters</span>
<a name="l00458"></a>00458 <span class="comment"> */</span>
<a name="l00459"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#aeebd325f91acbf7d27134c8c7388649e">00459</a> <span class="keywordtype">void</span> runOneKalmanIteration()
<a name="l00460"></a>00460 {
<a name="l00461"></a>00461 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00462"></a>00462
<a name="l00463"></a>00463 m_timLogger.enable(KF_options.enable_profiler || KF_options.verbose);
<a name="l00464"></a>00464 m_timLogger.enter(<span class="stringliteral">"KF:complete_step"</span>);
<a name="l00465"></a>00465
<a name="l00466"></a>00466 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(<span class="keywordtype">size_t</span>(m_xkk.size())==m_pkk.getColCount())
<a name="l00467"></a>00467 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(<span class="keywordtype">size_t</span>(m_xkk.size())>=VEH_SIZE)
<a name="l00468"></a>00468
<a name="l00469"></a>00469 <span class="comment">// =============================================================</span>
<a name="l00470"></a>00470 <span class="comment">// 1. CREATE ACTION MATRIX u FROM ODOMETRY</span>
<a name="l00471"></a>00471 <span class="comment">// =============================================================</span>
<a name="l00472"></a>00472 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_ACT</a> u;
<a name="l00473"></a>00473
<a name="l00474"></a>00474 m_timLogger.enter(<span class="stringliteral">"KF:1.OnGetAction"</span>);
<a name="l00475"></a>00475 OnGetAction(u);
<a name="l00476"></a>00476 m_timLogger.leave(<span class="stringliteral">"KF:1.OnGetAction"</span>);
<a name="l00477"></a>00477
<a name="l00478"></a>00478 <span class="comment">// Sanity check:</span>
<a name="l00479"></a>00479 <span class="keywordflow">if</span> (FEAT_SIZE) { <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>( (((m_xkk.size()-VEH_SIZE)/FEAT_SIZE)*FEAT_SIZE)== (m_xkk.size()-VEH_SIZE) ) }
<a name="l00480"></a>00480
<a name="l00481"></a>00481 <span class="comment">// =============================================================</span>
<a name="l00482"></a>00482 <span class="comment">// 2. PREDICTION OF NEW POSE xv_{k+1|k}</span>
<a name="l00483"></a>00483 <span class="comment">// =============================================================</span>
<a name="l00484"></a>00484 m_timLogger.enter(<span class="stringliteral">"KF:2.prediction stage"</span>);
<a name="l00485"></a>00485
<a name="l00486"></a>00486 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N_map = getNumberOfLandmarksInTheMap();
<a name="l00487"></a>00487
<a name="l00488"></a>00488 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> xv( &m_xkk[0] ); <span class="comment">// Vehicle pose</span>
<a name="l00489"></a>00489
<a name="l00490"></a>00490 <span class="keywordtype">bool</span> skipPrediction=<span class="keyword">false</span>; <span class="comment">// Wether to skip the prediction step (in SLAM this is desired for the first iteration...)</span>
<a name="l00491"></a>00491
<a name="l00492"></a>00492 <span class="comment">// Update mean: xv will have the updated pose until we update it in the filterState later.</span>
<a name="l00493"></a>00493 <span class="comment">// This is to maintain a copy of the last robot pose in the state vector, required for the Jacobian computation.</span>
<a name="l00494"></a>00494 OnTransitionModel(u, xv, skipPrediction);
<a name="l00495"></a>00495
<a name="l00496"></a>00496 <span class="keywordflow">if</span> ( !skipPrediction )
<a name="l00497"></a>00497 {
<a name="l00498"></a>00498 <span class="comment">// =============================================================</span>
<a name="l00499"></a>00499 <span class="comment">// 3. PREDICTION OF COVARIANCE P_{k+1|k}</span>
<a name="l00500"></a>00500 <span class="comment">// =============================================================</span>
<a name="l00501"></a>00501 <span class="comment">// First, we compute de Jacobian fv_by_xv (derivative of f_vehicle wrt x_vehicle):</span>
<a name="l00502"></a>00502 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_VxV</a> dfv_dxv;
<a name="l00503"></a>00503
<a name="l00504"></a>00504 <span class="comment">// Try closed-form Jacobian first:</span>
<a name="l00505"></a>00505 m_user_didnt_implement_jacobian=<span class="keyword">false</span>; <span class="comment">// Set to true by the default method if not reimplemented in base class.</span>
<a name="l00506"></a>00506 <span class="keywordflow">if</span> (KF_options.use_analytic_transition_jacobian)
<a name="l00507"></a>00507 OnTransitionJacobian(dfv_dxv);
<a name="l00508"></a>00508
<a name="l00509"></a>00509 <span class="keywordflow">if</span> (m_user_didnt_implement_jacobian || !KF_options.use_analytic_transition_jacobian || KF_options.debug_verify_analytic_jacobians)
<a name="l00510"></a>00510 { <span class="comment">// Numeric approximation:</span>
<a name="l00511"></a>00511 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> xkk_vehicle( &m_xkk[0] ); <span class="comment">// A copy of the vehicle part of the state vector.</span>
<a name="l00512"></a>00512 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> xkk_veh_increments;
<a name="l00513"></a>00513 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
<a name="l00514"></a>00514
<a name="l00515"></a>00515 <a class="code" href="group__container__ops__grp.html#ga13e2e339d944b37f2386fc4e0bb56935" title="Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...">mrpt::math::estimateJacobian</a>(
<a name="l00516"></a>00516 xkk_vehicle,
<a name="l00517"></a>00517 &KF_aux_estimate_trans_jacobian, <span class="comment">//(const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out),</span>
<a name="l00518"></a>00518 xkk_veh_increments,
<a name="l00519"></a>00519 std::make_pair<KFCLASS*,KFArray_ACT>(<span class="keyword">this</span>,u),
<a name="l00520"></a>00520 dfv_dxv);
<a name="l00521"></a>00521
<a name="l00522"></a>00522 <span class="keywordflow">if</span> (KF_options.debug_verify_analytic_jacobians)
<a name="l00523"></a>00523 {
<a name="l00524"></a>00524 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_VxV</a> dfv_dxv_gt(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00525"></a>00525 OnTransitionJacobian(dfv_dxv_gt);
<a name="l00526"></a>00526 <span class="keywordflow">if</span> ((dfv_dxv-dfv_dxv_gt).<a class="code" href="eigen__plugins_8h.html#a77d94451cb53e337e050fd77ba12f8ef">Abs</a>().<a class="code" href="eigen__plugins_8h.html#a8e317bb5dedefdd9f59f2894982981ea">sumAll</a>()>KF_options.debug_verify_analytic_jacobians_threshold)
<a name="l00527"></a>00527 {
<a name="l00528"></a>00528 std::cerr << <span class="stringliteral">"[KalmanFilter] ERROR: User analytical transition Jacobians are wrong: \n"</span>
<a name="l00529"></a>00529 << <span class="stringliteral">" Real dfv_dxv: \n"</span> << dfv_dxv << <span class="stringliteral">"\n Analytical dfv_dxv:\n"</span> << dfv_dxv_gt << <span class="stringliteral">"Diff:\n"</span> << (dfv_dxv-dfv_dxv_gt) << <span class="stringliteral">"\n"</span>;
<a name="l00530"></a>00530 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"ERROR: User analytical transition Jacobians are wrong (More details dumped to cerr)"</span>)
<a name="l00531"></a>00531 }
<a name="l00532"></a>00532 }
<a name="l00533"></a>00533
<a name="l00534"></a>00534 }
<a name="l00535"></a>00535
<a name="l00536"></a>00536 <span class="comment">// Q is the process noise covariance matrix, is associated to the robot movement and is necesary to calculate the prediction P(k+1|k)</span>
<a name="l00537"></a>00537 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_VxV</a> Q;
<a name="l00538"></a>00538 OnTransitionNoise(Q);
<a name="l00539"></a>00539
<a name="l00540"></a>00540 <span class="comment">// ====================================</span>
<a name="l00541"></a>00541 <span class="comment">// 3.1: Pxx submatrix</span>
<a name="l00542"></a>00542 <span class="comment">// ====================================</span>
<a name="l00543"></a>00543 <span class="comment">// Replace old covariance:</span>
<a name="l00544"></a>00544 m_pkk.block(0,0,VEH_SIZE,VEH_SIZE) =
<a name="l00545"></a>00545 Q +
<a name="l00546"></a>00546 dfv_dxv * m_pkk.block(0,0,VEH_SIZE,VEH_SIZE) * dfv_dxv.transpose();
<a name="l00547"></a>00547
<a name="l00548"></a>00548 <span class="comment">// ====================================</span>
<a name="l00549"></a>00549 <span class="comment">// 3.2: All Pxy_i</span>
<a name="l00550"></a>00550 <span class="comment">// ====================================</span>
<a name="l00551"></a>00551 <span class="comment">// Now, update the cov. of landmarks, if any:</span>
<a name="l00552"></a>00552 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_VxF</a> aux;
<a name="l00553"></a>00553 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0 ; i<N_map ; i++)
<a name="l00554"></a>00554 {
<a name="l00555"></a>00555 <span class="comment">// aux = dfv_dxv(...) * m_pkk(...)</span>
<a name="l00556"></a>00556 dfv_dxv.multiply_subMatrix(
<a name="l00557"></a>00557 m_pkk,
<a name="l00558"></a>00558 aux, <span class="comment">// Output</span>
<a name="l00559"></a>00559 VEH_SIZE+i*FEAT_SIZE, <span class="comment">// Offset col</span>
<a name="l00560"></a>00560 0, <span class="comment">// Offset row</span>
<a name="l00561"></a>00561 FEAT_SIZE <span class="comment">// Number of columns desired in output</span>
<a name="l00562"></a>00562 );
<a name="l00563"></a>00563
<a name="l00564"></a>00564 m_pkk.insertMatrix (0, VEH_SIZE+i*FEAT_SIZE, aux );
<a name="l00565"></a>00565 m_pkk.insertMatrixTranspose(VEH_SIZE+i*FEAT_SIZE, 0 , aux );
<a name="l00566"></a>00566 }
<a name="l00567"></a>00567
<a name="l00568"></a>00568 <span class="comment">// =============================================================</span>
<a name="l00569"></a>00569 <span class="comment">// 4. NOW WE CAN OVERWRITE THE NEW STATE VECTOR</span>
<a name="l00570"></a>00570 <span class="comment">// =============================================================</span>
<a name="l00571"></a>00571 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<VEH_SIZE;i++)
<a name="l00572"></a>00572 m_xkk[i]=xv[i];
<a name="l00573"></a>00573
<a name="l00574"></a>00574 <span class="comment">// Normalize, if neccesary.</span>
<a name="l00575"></a>00575 OnNormalizeStateVector();
<a name="l00576"></a>00576
<a name="l00577"></a>00577 } <span class="comment">// end if (!skipPrediction)</span>
<a name="l00578"></a>00578
<a name="l00579"></a>00579
<a name="l00580"></a>00580 <span class="keyword">const</span> <span class="keywordtype">double</span> tim_pred = m_timLogger.leave(<span class="stringliteral">"KF:2.prediction stage"</span>);
<a name="l00581"></a>00581
<a name="l00582"></a>00582
<a name="l00583"></a>00583 <span class="comment">// =============================================================</span>
<a name="l00584"></a>00584 <span class="comment">// 5. PREDICTION OF OBSERVATIONS AND COMPUTE JACOBIANS</span>
<a name="l00585"></a>00585 <span class="comment">// =============================================================</span>
<a name="l00586"></a>00586 m_timLogger.enter(<span class="stringliteral">"KF:3.predict all obs"</span>);
<a name="l00587"></a>00587
<a name="l00588"></a>00588 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxO</a> R; <span class="comment">// Sensor uncertainty (covariance matrix): R</span>
<a name="l00589"></a>00589 OnGetObservationNoise(R);
<a name="l00590"></a>00590
<a name="l00591"></a>00591 <span class="comment">// Predict the observations for all the map LMs, so the user</span>
<a name="l00592"></a>00592 <span class="comment">// can decide if their covariances (more costly) must be computed as well:</span>
<a name="l00593"></a>00593 all_predictions.resize(N_map);
<a name="l00594"></a>00594 OnObservationModel(
<a name="l00595"></a>00595 mrpt::math::sequenceStdVec<size_t,1>(0,N_map),
<a name="l00596"></a>00596 all_predictions);
<a name="l00597"></a>00597
<a name="l00598"></a>00598 <span class="keyword">const</span> <span class="keywordtype">double</span> tim_pred_obs = m_timLogger.leave(<span class="stringliteral">"KF:3.predict all obs"</span>);
<a name="l00599"></a>00599
<a name="l00600"></a>00600 m_timLogger.enter(<span class="stringliteral">"KF:4.decide pred obs"</span>);
<a name="l00601"></a>00601
<a name="l00602"></a>00602 <span class="comment">// Decide if some of the covariances shouldn't be predicted.</span>
<a name="l00603"></a>00603 predictLMidxs.clear();
<a name="l00604"></a>00604 <span class="keywordflow">if</span> (FEAT_SIZE==0)
<a name="l00605"></a>00605 <span class="comment">// In non-SLAM problems, just do one prediction, for the entire system state:</span>
<a name="l00606"></a>00606 predictLMidxs.assign(1,0);
<a name="l00607"></a>00607 <span class="keywordflow">else</span>
<a name="l00608"></a>00608 <span class="comment">// On normal SLAM problems:</span>
<a name="l00609"></a>00609 OnPreComputingPredictions(all_predictions, predictLMidxs);
<a name="l00610"></a>00610
<a name="l00611"></a>00611
<a name="l00612"></a>00612 m_timLogger.leave(<span class="stringliteral">"KF:4.decide pred obs"</span>);
<a name="l00613"></a>00613
<a name="l00614"></a>00614 <span class="comment">// =============================================================</span>
<a name="l00615"></a>00615 <span class="comment">// 6. COMPUTE INNOVATION MATRIX "S"</span>
<a name="l00616"></a>00616 <span class="comment">// =============================================================</span>
<a name="l00617"></a>00617 <span class="comment">// Do the prediction of the observation covariances:</span>
<a name="l00618"></a>00618 <span class="comment">// Compute S: S = H P ~H + R</span>
<a name="l00619"></a>00619 <span class="comment">//</span>
<a name="l00620"></a>00620 <span class="comment">// Build a big dh_dx Jacobian composed of the small block Jacobians.</span>
<a name="l00621"></a>00621 <span class="comment">// , but: it's actually a subset of the full Jacobian, since the non-predicted</span>
<a name="l00622"></a>00622 <span class="comment">// features do not appear.</span>
<a name="l00623"></a>00623 <span class="comment">//</span>
<a name="l00624"></a>00624 <span class="comment">// dh_dx: O·M x V+M·F</span>
<a name="l00625"></a>00625 <span class="comment">// S: O·M x O·M</span>
<a name="l00626"></a>00626 <span class="comment">// M = |predictLMidxs|</span>
<a name="l00627"></a>00627 <span class="comment">// O=size of each observation.</span>
<a name="l00628"></a>00628 <span class="comment">// F=size of features in the map</span>
<a name="l00629"></a>00629 <span class="comment">//</span>
<a name="l00630"></a>00630
<a name="l00631"></a>00631 <span class="comment">// This is the beginning of a loop if we are doing sequential</span>
<a name="l00632"></a>00632 <span class="comment">// data association, where after each incorporation of an observation</span>
<a name="l00633"></a>00633 <span class="comment">// into the filter we recompute the vehicle state, then reconsider the</span>
<a name="l00634"></a>00634 <span class="comment">// data associations:</span>
<a name="l00635"></a>00635 <span class="comment">// TKFFusionMethod fusion_strategy</span>
<a name="l00636"></a>00636
<a name="l00637"></a>00637
<a name="l00638"></a>00638 m_timLogger.enter(<span class="stringliteral">"KF:5.build Jacobians"</span>);
<a name="l00639"></a>00639
<a name="l00640"></a>00640 <span class="keywordtype">size_t</span> N_pred = FEAT_SIZE==0 ?
<a name="l00641"></a>00641 1 <span class="comment">/* In non-SLAM problems, there'll be only 1 fixed observation */</span> :
<a name="l00642"></a>00642 predictLMidxs.size();
<a name="l00643"></a>00643
<a name="l00644"></a>00644 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_int</a> data_association; <span class="comment">// -1: New map feature.>=0: Indexes in the state vector</span>
<a name="l00645"></a>00645
<a name="l00646"></a>00646 <span class="comment">// The next loop will only do more than one iteration if the heuristic in OnPreComputingPredictions() fails, </span>
<a name="l00647"></a>00647 <span class="comment">// which will be detected by the addition of extra landmarks to predict into "missing_predictions_to_add"</span>
<a name="l00648"></a>00648 std<a class="code" href="classstd_1_1vector.html">::vector<size_t></a> missing_predictions_to_add;
<a name="l00649"></a>00649
<a name="l00650"></a>00650 <span class="comment">// Indices in xkk (& Pkk) that are involved in this observation (used below).</span>
<a name="l00651"></a>00651 idxs.clear();
<a name="l00652"></a>00652 idxs.reserve(VEH_SIZE+N_pred*FEAT_SIZE);
<a name="l00653"></a>00653 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<VEH_SIZE;i++) idxs.push_back(i);
<a name="l00654"></a>00654
<a name="l00655"></a>00655 dh_dx.zeros(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_pred ); <span class="comment">// Init to zeros.</span>
<a name="l00656"></a>00656 dh_dx_full.zeros(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map ); <span class="comment">// Init to zeros.</span>
<a name="l00657"></a>00657
<a name="l00658"></a>00658 <span class="keywordtype">size_t</span> first_new_pred = 0; <span class="comment">// This will be >0 only if we perform multiple loops due to failures in the prediction heuristic.</span>
<a name="l00659"></a>00659
<a name="l00660"></a>00660 <span class="keywordflow">do</span>
<a name="l00661"></a>00661 {
<a name="l00662"></a>00662 <span class="keywordflow">if</span> (!missing_predictions_to_add.empty())
<a name="l00663"></a>00663 {
<a name="l00664"></a>00664 <span class="keyword">const</span> <span class="keywordtype">size_t</span> nNew = missing_predictions_to_add.size();
<a name="l00665"></a>00665 printf_debug(<span class="stringliteral">"[KF] *Performance Warning*: %u LMs were not correctly predicted by OnPreComputingPredictions()\n"</span>,static_cast<unsigned int>(nNew));
<a name="l00666"></a>00666
<a name="l00667"></a>00667 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(FEAT_SIZE!=0)
<a name="l00668"></a>00668 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<nNew;j++)
<a name="l00669"></a>00669 predictLMidxs.push_back( missing_predictions_to_add[j] );
<a name="l00670"></a>00670
<a name="l00671"></a>00671 N_pred = predictLMidxs.size();
<a name="l00672"></a>00672 missing_predictions_to_add.clear();
<a name="l00673"></a>00673 }
<a name="l00674"></a>00674
<a name="l00675"></a>00675 dh_dx.setSize(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_pred ); <span class="comment">// Pad with zeros.</span>
<a name="l00676"></a>00676 dh_dx_full.setSize(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map ); <span class="comment">// Pad with zeros.</span>
<a name="l00677"></a>00677
<a name="l00678"></a>00678 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=first_new_pred;i<N_pred;++i)
<a name="l00679"></a>00679 {
<a name="l00680"></a>00680 <span class="keyword">const</span> <span class="keywordtype">size_t</span> lm_idx = FEAT_SIZE==0 ? 0 : predictLMidxs[i];
<a name="l00681"></a>00681 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxV</a> Hx(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00682"></a>00682 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxF</a> Hy(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00683"></a>00683
<a name="l00684"></a>00684 <span class="comment">// Try the analitic Jacobian first:</span>
<a name="l00685"></a>00685 m_user_didnt_implement_jacobian=<span class="keyword">false</span>; <span class="comment">// Set to true by the default method if not reimplemented in base class.</span>
<a name="l00686"></a>00686 <span class="keywordflow">if</span> (KF_options.use_analytic_observation_jacobian)
<a name="l00687"></a>00687 OnObservationJacobians(lm_idx,Hx,Hy);
<a name="l00688"></a>00688
<a name="l00689"></a>00689 <span class="keywordflow">if</span> (m_user_didnt_implement_jacobian || !KF_options.use_analytic_observation_jacobian || KF_options.debug_verify_analytic_jacobians)
<a name="l00690"></a>00690 { <span class="comment">// Numeric approximation:</span>
<a name="l00691"></a>00691 <span class="keyword">const</span> <span class="keywordtype">size_t</span> lm_idx_in_statevector = VEH_SIZE+lm_idx*FEAT_SIZE;
<a name="l00692"></a>00692
<a name="l00693"></a>00693 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> x_vehicle( &m_xkk[0] );
<a name="l00694"></a>00694 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_FEAT</a> x_feat( &m_xkk[lm_idx_in_statevector] );
<a name="l00695"></a>00695
<a name="l00696"></a>00696 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> xkk_veh_increments;
<a name="l00697"></a>00697 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_FEAT</a> feat_increments;
<a name="l00698"></a>00698 OnObservationJacobiansNumericGetIncrements(xkk_veh_increments, feat_increments);
<a name="l00699"></a>00699
<a name="l00700"></a>00700 <a class="code" href="group__container__ops__grp.html#ga13e2e339d944b37f2386fc4e0bb56935" title="Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...">mrpt::math::estimateJacobian</a>(
<a name="l00701"></a>00701 x_vehicle,
<a name="l00702"></a>00702 &KF_aux_estimate_obs_Hx_jacobian,
<a name="l00703"></a>00703 xkk_veh_increments,
<a name="l00704"></a>00704 std::make_pair<KFCLASS*,size_t>(<span class="keyword">this</span>,lm_idx),
<a name="l00705"></a>00705 Hx);
<a name="l00706"></a>00706 <span class="comment">// The state vector was temporarily modified by KF_aux_estimate_*, restore it:</span>
<a name="l00707"></a>00707 <a class="code" href="group__mrpt__system__os.html#gae1184cfb1f617787dc4c9da98becbe3a" title="An OS and compiler independent version of "memcpy".">::memcpy</a>(&m_xkk[0],&x_vehicle[0],<span class="keyword">sizeof</span>(m_xkk[0])*VEH_SIZE);
<a name="l00708"></a>00708
<a name="l00709"></a>00709 <a class="code" href="group__container__ops__grp.html#ga13e2e339d944b37f2386fc4e0bb56935" title="Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...">mrpt::math::estimateJacobian</a>(
<a name="l00710"></a>00710 x_feat,
<a name="l00711"></a>00711 &KF_aux_estimate_obs_Hy_jacobian,
<a name="l00712"></a>00712 feat_increments,
<a name="l00713"></a>00713 std::make_pair<KFCLASS*,size_t>(<span class="keyword">this</span>,lm_idx),
<a name="l00714"></a>00714 Hy);
<a name="l00715"></a>00715 <span class="comment">// The state vector was temporarily modified by KF_aux_estimate_*, restore it:</span>
<a name="l00716"></a>00716 <a class="code" href="group__mrpt__system__os.html#gae1184cfb1f617787dc4c9da98becbe3a" title="An OS and compiler independent version of "memcpy".">::memcpy</a>(&m_xkk[lm_idx_in_statevector],&x_feat[0],<span class="keyword">sizeof</span>(m_xkk[0])*FEAT_SIZE);
<a name="l00717"></a>00717
<a name="l00718"></a>00718 <span class="keywordflow">if</span> (KF_options.debug_verify_analytic_jacobians)
<a name="l00719"></a>00719 {
<a name="l00720"></a>00720 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxV</a> Hx_gt(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00721"></a>00721 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxF</a> Hy_gt(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00722"></a>00722 OnObservationJacobians(lm_idx,Hx_gt,Hy_gt);
<a name="l00723"></a>00723 <span class="keywordflow">if</span> ((Hx-Hx_gt).<a class="code" href="eigen__plugins_8h.html#a77d94451cb53e337e050fd77ba12f8ef">Abs</a>().<a class="code" href="eigen__plugins_8h.html#a8e317bb5dedefdd9f59f2894982981ea">sumAll</a>()>KF_options.debug_verify_analytic_jacobians_threshold) {
<a name="l00724"></a>00724 std::cerr << <span class="stringliteral">"[KalmanFilter] ERROR: User analytical observation Hx Jacobians are wrong: \n"</span>
<a name="l00725"></a>00725 << <span class="stringliteral">" Real Hx: \n"</span> << Hx << <span class="stringliteral">"\n Analytical Hx:\n"</span> << Hx_gt << <span class="stringliteral">"Diff:\n"</span> << Hx-Hx_gt << <span class="stringliteral">"\n"</span>;
<a name="l00726"></a>00726 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"ERROR: User analytical observation Hx Jacobians are wrong (More details dumped to cerr)"</span>)
<a name="l00727"></a>00727 }
<a name="l00728"></a>00728 <span class="keywordflow">if</span> ((Hy-Hy_gt).<a class="code" href="eigen__plugins_8h.html#a77d94451cb53e337e050fd77ba12f8ef">Abs</a>().<a class="code" href="eigen__plugins_8h.html#a8e317bb5dedefdd9f59f2894982981ea">sumAll</a>()>KF_options.debug_verify_analytic_jacobians_threshold) {
<a name="l00729"></a>00729 std::cerr << <span class="stringliteral">"[KalmanFilter] ERROR: User analytical observation Hy Jacobians are wrong: \n"</span>
<a name="l00730"></a>00730 << <span class="stringliteral">" Real Hy: \n"</span> << Hy << <span class="stringliteral">"\n Analytical Hx:\n"</span> << Hy_gt << <span class="stringliteral">"Diff:\n"</span> << Hy-Hy_gt << <span class="stringliteral">"\n"</span>;
<a name="l00731"></a>00731 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"ERROR: User analytical observation Hy Jacobians are wrong (More details dumped to cerr)"</span>)
<a name="l00732"></a>00732 }
<a name="l00733"></a>00733 }
<a name="l00734"></a>00734 }
<a name="l00735"></a>00735
<a name="l00736"></a>00736 dh_dx.insertMatrix(i*OBS_SIZE,0, Hx);
<a name="l00737"></a>00737 <span class="keywordflow">if</span> (FEAT_SIZE!=0)
<a name="l00738"></a>00738 dh_dx.insertMatrix(i*OBS_SIZE,VEH_SIZE+i*FEAT_SIZE, Hy);
<a name="l00739"></a>00739
<a name="l00740"></a>00740 dh_dx_full.insertMatrix(i*OBS_SIZE,0, Hx);
<a name="l00741"></a>00741 <span class="keywordflow">if</span> (FEAT_SIZE!=0)
<a name="l00742"></a>00742 {
<a name="l00743"></a>00743 dh_dx_full.insertMatrix(i*OBS_SIZE,VEH_SIZE+lm_idx*FEAT_SIZE, Hy);
<a name="l00744"></a>00744
<a name="l00745"></a>00745 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<FEAT_SIZE;k++)
<a name="l00746"></a>00746 idxs.push_back(k+VEH_SIZE+FEAT_SIZE*lm_idx);
<a name="l00747"></a>00747 }
<a name="l00748"></a>00748 }
<a name="l00749"></a>00749 m_timLogger.leave(<span class="stringliteral">"KF:5.build Jacobians"</span>);
<a name="l00750"></a>00750
<a name="l00751"></a>00751 <span class="comment">// Compute S: S = H P ~H + R</span>
<a name="l00752"></a>00752 <span class="comment">// *TODO*: This can be accelerated by exploiting the sparsity of dh_dx!!!</span>
<a name="l00753"></a>00753 <span class="comment">// ------------------------------------</span>
<a name="l00754"></a>00754 m_timLogger.enter(<span class="stringliteral">"KF:6.build S"</span>);
<a name="l00755"></a>00755
<a name="l00756"></a>00756 S.setSize(N_pred*OBS_SIZE,N_pred*OBS_SIZE);
<a name="l00757"></a>00757
<a name="l00758"></a>00758 <span class="comment">// (TODO: Implement multiply_HCHt for a subset of COV directly.)</span>
<a name="l00759"></a>00759 <span class="comment">// Extract the subset of m_Pkk that is involved in this observation:</span>
<a name="l00760"></a>00760 m_pkk.extractSubmatrixSymmetrical(idxs,Pkk_subset);
<a name="l00761"></a>00761
<a name="l00762"></a>00762 <span class="comment">// S = dh_dx * m_pkk(subset) * (~dh_dx);</span>
<a name="l00763"></a>00763 dh_dx.multiply_HCHt(Pkk_subset,S);
<a name="l00764"></a>00764
<a name="l00765"></a>00765 <span class="comment">// Sum the "R" term:</span>
<a name="l00766"></a>00766 <span class="keywordflow">if</span> ( FEAT_SIZE>0 )
<a name="l00767"></a>00767 {
<a name="l00768"></a>00768 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N_pred;++i)
<a name="l00769"></a>00769 {
<a name="l00770"></a>00770 <span class="keyword">const</span> <span class="keywordtype">size_t</span> obs_idx_off = i*OBS_SIZE;
<a name="l00771"></a>00771 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<OBS_SIZE;j++)
<a name="l00772"></a>00772 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<OBS_SIZE;k++)
<a name="l00773"></a>00773 S.get_unsafe(obs_idx_off+j,obs_idx_off+k) += R.get_unsafe(j,k);
<a name="l00774"></a>00774 }
<a name="l00775"></a>00775 }
<a name="l00776"></a>00776 <span class="keywordflow">else</span>
<a name="l00777"></a>00777 { <span class="comment">// Not a SLAM-like EKF problem:</span>
<a name="l00778"></a>00778 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(S.getColCount() == OBS_SIZE );
<a name="l00779"></a>00779 S+=R;
<a name="l00780"></a>00780 }
<a name="l00781"></a>00781
<a name="l00782"></a>00782 m_timLogger.leave(<span class="stringliteral">"KF:6.build S"</span>);
<a name="l00783"></a>00783
<a name="l00784"></a>00784
<a name="l00785"></a>00785 Z.clear(); <span class="comment">// Each entry is one observation:</span>
<a name="l00786"></a>00786
<a name="l00787"></a>00787 m_timLogger.enter(<span class="stringliteral">"KF:7.get obs & DA"</span>);
<a name="l00788"></a>00788
<a name="l00789"></a>00789 <span class="comment">// Get observations and do data-association:</span>
<a name="l00790"></a>00790 OnGetObservationsAndDataAssociation(
<a name="l00791"></a>00791 Z, data_association, <span class="comment">// Out</span>
<a name="l00792"></a>00792 all_predictions, S, predictLMidxs, R <span class="comment">// In</span>
<a name="l00793"></a>00793 );
<a name="l00794"></a>00794 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(data_association.size()==Z.size() || (data_association.empty() && FEAT_SIZE==0));
<a name="l00795"></a>00795
<a name="l00796"></a>00796 <span class="comment">// Check if an observation hasn't been predicted in OnPreComputingPredictions() but has been actually </span>
<a name="l00797"></a>00797 <span class="comment">// observed. This may imply an error in the heuristic of OnPreComputingPredictions(), and forces us</span>
<a name="l00798"></a>00798 <span class="comment">// to rebuild the matrices </span>
<a name="l00799"></a>00799 missing_predictions_to_add.clear();
<a name="l00800"></a>00800 <span class="keywordflow">if</span> (FEAT_SIZE!=0)
<a name="l00801"></a>00801 {
<a name="l00802"></a>00802 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<data_association.size();++i)
<a name="l00803"></a>00803 {
<a name="l00804"></a>00804 <span class="keywordflow">if</span> (data_association[i]<0) <span class="keywordflow">continue</span>;
<a name="l00805"></a>00805 <span class="keyword">const</span> <span class="keywordtype">size_t</span> assoc_idx_in_map = <span class="keyword">static_cast<</span><span class="keywordtype">size_t</span><span class="keyword">></span>(data_association[i]);
<a name="l00806"></a>00806 <span class="keyword">const</span> <span class="keywordtype">size_t</span> assoc_idx_in_pred = <a class="code" href="group__stlext__grp.html#ga0683c85522dd9009f2c2801c18e12eae" title="Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.">mrpt::utils::find_in_vector</a>(assoc_idx_in_map, predictLMidxs);
<a name="l00807"></a>00807 <span class="keywordflow">if</span> (assoc_idx_in_pred==std::string::npos)
<a name="l00808"></a>00808 missing_predictions_to_add.push_back(assoc_idx_in_map);
<a name="l00809"></a>00809 }
<a name="l00810"></a>00810 }
<a name="l00811"></a>00811
<a name="l00812"></a>00812 first_new_pred = N_pred; <span class="comment">// If we do another loop, start at the begin of new predictions</span>
<a name="l00813"></a>00813
<a name="l00814"></a>00814 } <span class="keywordflow">while</span> (!missing_predictions_to_add.empty());
<a name="l00815"></a>00815
<a name="l00816"></a>00816
<a name="l00817"></a>00817 <span class="keyword">const</span> <span class="keywordtype">double</span> tim_obs_DA = m_timLogger.leave(<span class="stringliteral">"KF:7.get obs & DA"</span>);
<a name="l00818"></a>00818
<a name="l00819"></a>00819 <span class="comment">// =============================================================</span>
<a name="l00820"></a>00820 <span class="comment">// 7. UPDATE USING THE KALMAN GAIN</span>
<a name="l00821"></a>00821 <span class="comment">// =============================================================</span>
<a name="l00822"></a>00822 <span class="comment">// Update, only if there are observations!</span>
<a name="l00823"></a>00823 <span class="keywordflow">if</span> ( !Z.empty() )
<a name="l00824"></a>00824 {
<a name="l00825"></a>00825 m_timLogger.enter(<span class="stringliteral">"KF:8.update stage"</span>);
<a name="l00826"></a>00826
<a name="l00827"></a>00827 <span class="keywordflow">switch</span> (KF_options.method)
<a name="l00828"></a>00828 {
<a name="l00829"></a>00829 <span class="comment">// -----------------------</span>
<a name="l00830"></a>00830 <span class="comment">// FULL KF- METHOD</span>
<a name="l00831"></a>00831 <span class="comment">// -----------------------</span>
<a name="l00832"></a>00832 <span class="keywordflow">case</span> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a5ad91d60187b5cca61d25994f7827dc0">kfEKFNaive</a>:
<a name="l00833"></a>00833 <span class="keywordflow">case</span> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14ac5aee4885e69b59edf793342e96acab3">kfIKFFull</a>:
<a name="l00834"></a>00834 {
<a name="l00835"></a>00835 <span class="comment">// Build the whole Jacobian dh_dx matrix</span>
<a name="l00836"></a>00836 <span class="comment">// ---------------------------------------------</span>
<a name="l00837"></a>00837 <span class="comment">// Keep only those whose DA is not -1</span>
<a name="l00838"></a>00838 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_int</a> mapIndicesForKFUpdate(data_association.size());
<a name="l00839"></a>00839 mapIndicesForKFUpdate.resize( <a class="code" href="group__geometry__grp.html#ga8c0a76e906f12560cfa49fcd269c8398" title="Gets the distance between two points in a 2D space.">std::distance</a>(mapIndicesForKFUpdate.begin(),
<a name="l00840"></a>00840 std::remove_copy_if(
<a name="l00841"></a>00841 data_association.begin(),
<a name="l00842"></a>00842 data_association.end(),
<a name="l00843"></a>00843 mapIndicesForKFUpdate.begin(),
<a name="l00844"></a>00844 binder1st<equal_to<int> >(equal_to<int>(),-1) ) ) );
<a name="l00845"></a>00845
<a name="l00846"></a>00846 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N_upd = (FEAT_SIZE==0) ?
<a name="l00847"></a>00847 1 : <span class="comment">// Non-SLAM problems: Just one observation for the entire system.</span>
<a name="l00848"></a>00848 mapIndicesForKFUpdate.size(); <span class="comment">// SLAM: # of observed known landmarks</span>
<a name="l00849"></a>00849
<a name="l00850"></a>00850 <span class="comment">// Just one, or several update iterations??</span>
<a name="l00851"></a>00851 <span class="keyword">const</span> <span class="keywordtype">size_t</span> nKF_iterations = (KF_options.method==<a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a5ad91d60187b5cca61d25994f7827dc0">kfEKFNaive</a>) ? 1 : KF_options.IKF_iterations;
<a name="l00852"></a>00852
<a name="l00853"></a>00853 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1dynamicsize__vector.html">KFVector</a> xkk_0 = m_xkk;
<a name="l00854"></a>00854
<a name="l00855"></a>00855 <span class="comment">// For each IKF iteration (or 1 for EKF)</span>
<a name="l00856"></a>00856 if (N_upd>0) <span class="comment">// Do not update if we have no observations!</span>
<a name="l00857"></a>00857 {
<a name="l00858"></a>00858 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
<a name="l00859"></a>00859 {
<a name="l00860"></a>00860 <span class="comment">// Compute ytilde = OBS - PREDICTION</span>
<a name="l00861"></a>00861 <a class="code" href="structmrpt_1_1dynamicsize__vector.html">KFVector</a> ytilde(OBS_SIZE*N_upd);
<a name="l00862"></a>00862 <span class="keywordtype">size_t</span> ytilde_idx = 0;
<a name="l00863"></a>00863
<a name="l00864"></a>00864 <span class="comment">// TODO: Use a Matrix view of "dh_dx_full" instead of creating a copy into "dh_dx_full_obs"</span>
<a name="l00865"></a>00865 dh_dx_full_obs.zeros(N_upd*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map ); <span class="comment">// Init to zeros.</span>
<a name="l00866"></a>00866 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> S_observed; <span class="comment">// The KF "S" matrix: A re-ordered, subset, version of the prediction S:</span>
<a name="l00867"></a>00867
<a name="l00868"></a>00868 <span class="keywordflow">if</span> (FEAT_SIZE!=0)
<a name="l00869"></a>00869 { <span class="comment">// SLAM problems:</span>
<a name="l00870"></a>00870 <a class="code" href="classstd_1_1vector.html">vector_size_t</a> S_idxs;
<a name="l00871"></a>00871 S_idxs.reserve(OBS_SIZE*N_upd);
<a name="l00872"></a>00872
<a name="l00873"></a>00873 <span class="keyword">const</span> <span class="keywordtype">size_t</span> row_len = VEH_SIZE + FEAT_SIZE * N_map;
<a name="l00874"></a>00874
<a name="l00875"></a>00875 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<data_association.size();++i)
<a name="l00876"></a>00876 {
<a name="l00877"></a>00877 <span class="keywordflow">if</span> (data_association[i]<0) <span class="keywordflow">continue</span>;
<a name="l00878"></a>00878
<a name="l00879"></a>00879 <span class="keyword">const</span> <span class="keywordtype">size_t</span> assoc_idx_in_map = <span class="keyword">static_cast<</span><span class="keywordtype">size_t</span><span class="keyword">></span>(data_association[i]);
<a name="l00880"></a>00880 <span class="keyword">const</span> <span class="keywordtype">size_t</span> assoc_idx_in_pred = <a class="code" href="group__stlext__grp.html#ga0683c85522dd9009f2c2801c18e12eae" title="Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.">mrpt::utils::find_in_vector</a>(assoc_idx_in_map, predictLMidxs);
<a name="l00881"></a>00881 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(assoc_idx_in_pred!=string::npos, <span class="stringliteral">"OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!"</span>)
<a name="l00882"></a>00882 <span class="comment">// TODO: In these cases, extend the prediction right now instead of launching an exception... or is this a bad idea??</span>
<a name="l00883"></a>00883
<a name="l00884"></a>00884 <span class="comment">// Build the subset dh_dx_full_obs:</span>
<a name="l00885"></a>00885 dh_dx_full_obs.block(S_idxs.size() ,0, OBS_SIZE, row_len)
<a name="l00886"></a>00886 =
<a name="l00887"></a>00887 dh_dx_full.block (assoc_idx_in_pred*OBS_SIZE,0, OBS_SIZE, row_len);
<a name="l00888"></a>00888 <span class="comment">// S_idxs.size() is used as counter for "dh_dx_full_obs".</span>
<a name="l00889"></a>00889 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<OBS_SIZE;k++)
<a name="l00890"></a>00890 S_idxs.push_back(assoc_idx_in_pred*OBS_SIZE+k);
<a name="l00891"></a>00891
<a name="l00892"></a>00892 <span class="comment">// ytilde_i = Z[i] - all_predictions[i]</span>
<a name="l00893"></a>00893 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> ytilde_i = Z[i];
<a name="l00894"></a>00894 OnSubstractObservationVectors(ytilde_i,all_predictions[predictLMidxs[assoc_idx_in_pred]]);
<a name="l00895"></a>00895 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<OBS_SIZE;k++)
<a name="l00896"></a>00896 ytilde[ytilde_idx++] = ytilde_i[k];
<a name="l00897"></a>00897 }
<a name="l00898"></a>00898 <span class="comment">// Extract the subset that is involved in this observation:</span>
<a name="l00899"></a>00899 S.extractSubmatrixSymmetrical(S_idxs,S_observed);
<a name="l00900"></a>00900 }
<a name="l00901"></a>00901 <span class="keywordflow">else</span>
<a name="l00902"></a>00902 { <span class="comment">// Non-SLAM problems:</span>
<a name="l00903"></a>00903 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(Z.size()==1 && all_predictions.size()==1)
<a name="l00904"></a>00904
<a name="l00905"></a>00905 dh_dx_full_obs = dh_dx_full;
<a name="l00906"></a>00906 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> ytilde_i = Z[0];
<a name="l00907"></a>00907 OnSubstractObservationVectors(ytilde_i,all_predictions[0]);
<a name="l00908"></a>00908 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<OBS_SIZE;k++)
<a name="l00909"></a>00909 ytilde[ytilde_idx++] = ytilde_i[k];
<a name="l00910"></a>00910 <span class="comment">// Extract the subset that is involved in this observation:</span>
<a name="l00911"></a>00911 S_observed = S;
<a name="l00912"></a>00912 }
<a name="l00913"></a>00913
<a name="l00914"></a>00914 <span class="comment">// Compute the full K matrix:</span>
<a name="l00915"></a>00915 <span class="comment">// ------------------------------</span>
<a name="l00916"></a>00916 m_timLogger.enter(<span class="stringliteral">"KF:8.update stage:1.FULLKF:build K"</span>);
<a name="l00917"></a>00917
<a name="l00918"></a>00918 K.setSize(m_pkk.getRowCount(), S_observed.getColCount() );
<a name="l00919"></a>00919
<a name="l00920"></a>00920 <span class="comment">// K = m_pkk * (~dh_dx) * S.inv() );</span>
<a name="l00921"></a>00921 K.multiply_ABt(m_pkk, dh_dx_full_obs);
<a name="l00922"></a>00922
<a name="l00923"></a>00923 <span class="comment">//KFMatrix S_1( S_observed.getRowCount(), S_observed.getColCount() );</span>
<a name="l00924"></a>00924 S_observed.inv(S_1); <span class="comment">// Do NOT call inv_fast since it destroys S</span>
<a name="l00925"></a>00925 K*=S_1;
<a name="l00926"></a>00926
<a name="l00927"></a>00927 m_timLogger.leave(<span class="stringliteral">"KF:8.update stage:1.FULLKF:build K"</span>);
<a name="l00928"></a>00928
<a name="l00929"></a>00929 <span class="comment">// Use the full K matrix to update the mean:</span>
<a name="l00930"></a>00930 <span class="keywordflow">if</span> (nKF_iterations==1)
<a name="l00931"></a>00931 {
<a name="l00932"></a>00932 m_timLogger.enter(<span class="stringliteral">"KF:8.update stage:2.FULLKF:update xkk"</span>);
<a name="l00933"></a>00933 m_xkk += K * ytilde;
<a name="l00934"></a>00934 m_timLogger.leave(<span class="stringliteral">"KF:8.update stage:2.FULLKF:update xkk"</span>);
<a name="l00935"></a>00935 }
<a name="l00936"></a>00936 <span class="keywordflow">else</span>
<a name="l00937"></a>00937 {
<a name="l00938"></a>00938 m_timLogger.enter(<span class="stringliteral">"KF:8.update stage:2.FULLKF:iter.update xkk"</span>);
<a name="l00939"></a>00939
<a name="l00940"></a>00940 <a class="code" href="structmrpt_1_1dynamicsize__vector.html">KFVector</a> HAx_column;
<a name="l00941"></a>00941 dh_dx_full_obs.multiply_Ab( m_xkk - xkk_0, HAx_column);
<a name="l00942"></a>00942
<a name="l00943"></a>00943 m_xkk = xkk_0;
<a name="l00944"></a>00944 K.multiply_Ab(
<a name="l00945"></a>00945 (ytilde-HAx_column),
<a name="l00946"></a>00946 m_xkk,
<a name="l00947"></a>00947 <span class="keyword">true</span> <span class="comment">/* Accumulate in output */</span>
<a name="l00948"></a>00948 );
<a name="l00949"></a>00949
<a name="l00950"></a>00950 m_timLogger.leave(<span class="stringliteral">"KF:8.update stage:2.FULLKF:iter.update xkk"</span>);
<a name="l00951"></a>00951 }
<a name="l00952"></a>00952
<a name="l00953"></a>00953 <span class="comment">// Update the covariance just at the end</span>
<a name="l00954"></a>00954 <span class="comment">// of iterations if we are in IKF, always in normal EKF.</span>
<a name="l00955"></a>00955 <span class="keywordflow">if</span> (IKF_iteration == (nKF_iterations-1) )
<a name="l00956"></a>00956 {
<a name="l00957"></a>00957 m_timLogger.enter(<span class="stringliteral">"KF:8.update stage:3.FULLKF:update Pkk"</span>);
<a name="l00958"></a>00958
<a name="l00959"></a>00959 <span class="comment">// Use the full K matrix to update the covariance:</span>
<a name="l00960"></a>00960 <span class="comment">//m_pkk = (I - K*dh_dx ) * m_pkk;</span>
<a name="l00961"></a>00961 <span class="comment">// TODO: "Optimize this: sparsity!"</span>
<a name="l00962"></a>00962
<a name="l00963"></a>00963 <span class="comment">// K * dh_dx_full</span>
<a name="l00964"></a>00964 aux_K_dh_dx.multiply(K,dh_dx_full_obs);
<a name="l00965"></a>00965
<a name="l00966"></a>00966 <span class="comment">// aux_K_dh_dx <-- I-aux_K_dh_dx</span>
<a name="l00967"></a>00967 <span class="keyword">const</span> <span class="keywordtype">size_t</span> stat_len = aux_K_dh_dx.getColCount();
<a name="l00968"></a>00968 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> r=0;r<stat_len;r++)
<a name="l00969"></a>00969 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> c=0;c<stat_len;c++)
<a name="l00970"></a>00970 <span class="keywordflow">if</span> (r==c)
<a name="l00971"></a>00971 aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c) + <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ad66228d2cc13705900669ccbab41f3d9" title="The numeric type used in the Kalman Filter (default=double)">kftype</a>(1);
<a name="l00972"></a>00972 <span class="keywordflow">else</span> aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c);
<a name="l00973"></a>00973
<a name="l00974"></a>00974 m_pkk.multiply_result_is_symmetric(aux_K_dh_dx, m_pkk );
<a name="l00975"></a>00975
<a name="l00976"></a>00976 m_timLogger.leave(<span class="stringliteral">"KF:8.update stage:3.FULLKF:update Pkk"</span>);
<a name="l00977"></a>00977 }
<a name="l00978"></a>00978 } <span class="comment">// end for each IKF iteration</span>
<a name="l00979"></a>00979 }
<a name="l00980"></a>00980 }
<a name="l00981"></a>00981 <span class="keywordflow">break</span>;
<a name="l00982"></a>00982
<a name="l00983"></a>00983 <span class="comment">// --------------------------------------------------------------------</span>
<a name="l00984"></a>00984 <span class="comment">// - EKF 'a la' Davison: One observation element at once</span>
<a name="l00985"></a>00985 <span class="comment">// --------------------------------------------------------------------</span>
<a name="l00986"></a>00986 <span class="keywordflow">case</span> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14acb25ab5d45035eb456de1c7250dbcf5a">kfEKFAlaDavison</a>:
<a name="l00987"></a>00987 {
<a name="l00988"></a>00988 <span class="comment">// For each observed landmark/whole system state:</span>
<a name="l00989"></a>00989 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> obsIdx=0;obsIdx<Z.size();obsIdx++)
<a name="l00990"></a>00990 {
<a name="l00991"></a>00991 <span class="comment">// Known & mapped landmark?</span>
<a name="l00992"></a>00992 <span class="keywordtype">bool</span> doit;
<a name="l00993"></a>00993 <span class="keywordtype">size_t</span> idxInTheFilter=0;
<a name="l00994"></a>00994
<a name="l00995"></a>00995 <span class="keywordflow">if</span> (data_association.empty())
<a name="l00996"></a>00996 {
<a name="l00997"></a>00997 doit = <span class="keyword">true</span>;
<a name="l00998"></a>00998 }
<a name="l00999"></a>00999 <span class="keywordflow">else</span>
<a name="l01000"></a>01000 {
<a name="l01001"></a>01001 doit = data_association[obsIdx] >= 0;
<a name="l01002"></a>01002 <span class="keywordflow">if</span> (doit)
<a name="l01003"></a>01003 idxInTheFilter = data_association[obsIdx];
<a name="l01004"></a>01004 }
<a name="l01005"></a>01005
<a name="l01006"></a>01006 <span class="keywordflow">if</span> ( doit )
<a name="l01007"></a>01007 {
<a name="l01008"></a>01008 m_timLogger.enter(<span class="stringliteral">"KF:8.update stage:1.ScalarAtOnce.prepare"</span>);
<a name="l01009"></a>01009
<a name="l01010"></a>01010 <span class="comment">// Already mapped: OK</span>
<a name="l01011"></a>01011 <span class="keyword">const</span> <span class="keywordtype">size_t</span> idx_off = VEH_SIZE + idxInTheFilter*FEAT_SIZE; <span class="comment">// The offset in m_xkk & Pkk.</span>
<a name="l01012"></a>01012
<a name="l01013"></a>01013 <span class="comment">// Compute just the part of the Jacobian that we need using the current updated m_xkk:</span>
<a name="l01014"></a>01014 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_KFArray_OBS</a> pred_obs;
<a name="l01015"></a>01015 OnObservationModel( <a class="code" href="namespacemrpt.html#ad4d2b1efd37ed750302c76ebbcfc310d">vector_size_t</a>(1,idxInTheFilter),pred_obs);
<a name="l01016"></a>01016 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(pred_obs.size()==1);
<a name="l01017"></a>01017
<a name="l01018"></a>01018 <span class="comment">// ytilde = observation - prediction</span>
<a name="l01019"></a>01019 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> ytilde = Z[obsIdx];
<a name="l01020"></a>01020 OnSubstractObservationVectors(ytilde, pred_obs[0]);
<a name="l01021"></a>01021
<a name="l01022"></a>01022 <span class="comment">// Jacobians:</span>
<a name="l01023"></a>01023 <span class="comment">// dh_dx: already is (N_pred*OBS_SIZE) x (VEH_SIZE + FEAT_SIZE * N_pred )</span>
<a name="l01024"></a>01024 <span class="comment">// with N_pred = |predictLMidxs|</span>
<a name="l01025"></a>01025
<a name="l01026"></a>01026 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxV</a> Hx(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l01027"></a>01027 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">KFMatrix_OxF</a> Hy(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l01028"></a>01028 <span class="keyword">const</span> <span class="keywordtype">size_t</span> i_idx_in_preds = <a class="code" href="group__stlext__grp.html#ga0683c85522dd9009f2c2801c18e12eae" title="Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.">mrpt::utils::find_in_vector</a>(idxInTheFilter,predictLMidxs);
<a name="l01029"></a>01029 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(i_idx_in_preds!=string::npos, <span class="stringliteral">"OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!"</span>)
<a name="l01030"></a>01030 dh_dx.extractMatrix(i_idx_in_preds*OBS_SIZE,0, Hx);
<a name="l01031"></a>01031 dh_dx.extractMatrix(i_idx_in_preds*OBS_SIZE,VEH_SIZE+i_idx_in_preds*OBS_SIZE, Hy);
<a name="l01032"></a>01032
<a name="l01033"></a>01033
<a name="l01034"></a>01034 m_timLogger.leave(<span class="stringliteral">"KF:8.update stage:1.ScalarAtOnce.prepare"</span>);
<a name="l01035"></a>01035
<a name="l01036"></a>01036 <span class="comment">// For each component of the observation:</span>
<a name="l01037"></a>01037 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<OBS_SIZE;j++)
<a name="l01038"></a>01038 {
<a name="l01039"></a>01039 m_timLogger.enter(<span class="stringliteral">"KF:8.update stage:2.ScalarAtOnce.update"</span>);
<a name="l01040"></a>01040
<a name="l01041"></a>01041 <span class="comment">// Compute the scalar S_i for each component j of the observation:</span>
<a name="l01042"></a>01042 <span class="comment">// Sij = dhij_dxv Pxx dhij_dxv^t + 2 * dhij_dyi Pyix dhij_dxv + dhij_dyi Pyiyi dhij_dyi^t + R</span>
<a name="l01043"></a>01043 <span class="comment">// ^^</span>
<a name="l01044"></a>01044 <span class="comment">// Hx:(O*LxSv)</span>
<a name="l01045"></a>01045 <span class="comment">// \----------------------/ \--------------------------/ \------------------------/ \-/</span>
<a name="l01046"></a>01046 <span class="comment">// TERM 1 TERM 2 TERM 3 R</span>
<a name="l01047"></a>01047 <span class="comment">//</span>
<a name="l01048"></a>01048 <span class="comment">// O: Observation size (3)</span>
<a name="l01049"></a>01049 <span class="comment">// L: # landmarks</span>
<a name="l01050"></a>01050 <span class="comment">// Sv: Vehicle state size (6)</span>
<a name="l01051"></a>01051 <span class="comment">//</span>
<a name="l01052"></a>01052
<a name="l01053"></a>01053 <span class="preprocessor"> #if defined(_DEBUG)</span>
<a name="l01054"></a>01054 <span class="preprocessor"></span> {
<a name="l01055"></a>01055 <span class="comment">// This algorithm is applicable only if the scalar component of the sensor noise are INDEPENDENT:</span>
<a name="l01056"></a>01056 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> a=0;a<OBS_SIZE;a++)
<a name="l01057"></a>01057 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> b=0;b<OBS_SIZE;b++)
<a name="l01058"></a>01058 <span class="keywordflow">if</span> ( a!=b )
<a name="l01059"></a>01059 <span class="keywordflow">if</span> (R(a,b)!=0)
<a name="l01060"></a>01060 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"This KF algorithm assumes independent noise components in the observation (matrix R). Select another KF algorithm."</span>)
<a name="l01061"></a>01061 }
<a name="l01062"></a>01062 <span class="preprocessor"> #endif</span>
<a name="l01063"></a>01063 <span class="preprocessor"></span> <span class="comment">// R:</span>
<a name="l01064"></a>01064 KFTYPE Sij = R.get_unsafe(j,j);
<a name="l01065"></a>01065
<a name="l01066"></a>01066 <span class="comment">// TERM 1:</span>
<a name="l01067"></a>01067 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<VEH_SIZE;k++)
<a name="l01068"></a>01068 {
<a name="l01069"></a>01069 KFTYPE accum = 0;
<a name="l01070"></a>01070 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> q=0;q<VEH_SIZE;q++)
<a name="l01071"></a>01071 accum += Hx.get_unsafe(j,q) * m_pkk.get_unsafe(q,k);
<a name="l01072"></a>01072 Sij+= Hx.get_unsafe(j,k) * accum;
<a name="l01073"></a>01073 }
<a name="l01074"></a>01074
<a name="l01075"></a>01075 <span class="comment">// TERM 2:</span>
<a name="l01076"></a>01076 KFTYPE term2=0;
<a name="l01077"></a>01077 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<VEH_SIZE;k++)
<a name="l01078"></a>01078 {
<a name="l01079"></a>01079 KFTYPE accum = 0;
<a name="l01080"></a>01080 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> q=0;q<FEAT_SIZE;q++)
<a name="l01081"></a>01081 accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,k);
<a name="l01082"></a>01082 term2+= Hx.get_unsafe(j,k) * accum;
<a name="l01083"></a>01083 }
<a name="l01084"></a>01084 Sij += 2 * term2;
<a name="l01085"></a>01085
<a name="l01086"></a>01086 <span class="comment">// TERM 3:</span>
<a name="l01087"></a>01087 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<FEAT_SIZE;k++)
<a name="l01088"></a>01088 {
<a name="l01089"></a>01089 KFTYPE accum = 0;
<a name="l01090"></a>01090 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> q=0;q<FEAT_SIZE;q++)
<a name="l01091"></a>01091 accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,idx_off+k);
<a name="l01092"></a>01092 Sij+= Hy.get_unsafe(j,k) * accum;
<a name="l01093"></a>01093 }
<a name="l01094"></a>01094
<a name="l01095"></a>01095 <span class="comment">// Compute the Kalman gain "Kij" for this observation element:</span>
<a name="l01096"></a>01096 <span class="comment">// --> K = m_pkk * (~dh_dx) * S.inv() );</span>
<a name="l01097"></a>01097 <span class="keywordtype">size_t</span> N = m_pkk.getColCount();
<a name="l01098"></a>01098 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector<KFTYPE></a> Kij( N );
<a name="l01099"></a>01099
<a name="l01100"></a>01100 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<N;k++)
<a name="l01101"></a>01101 {
<a name="l01102"></a>01102 KFTYPE K_tmp = 0;
<a name="l01103"></a>01103
<a name="l01104"></a>01104 <span class="comment">// dhi_dxv term</span>
<a name="l01105"></a>01105 <span class="keywordtype">size_t</span> q;
<a name="l01106"></a>01106 <span class="keywordflow">for</span> (q=0;q<VEH_SIZE;q++)
<a name="l01107"></a>01107 K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(j,q);
<a name="l01108"></a>01108
<a name="l01109"></a>01109 <span class="comment">// dhi_dyi term</span>
<a name="l01110"></a>01110 <span class="keywordflow">for</span> (q=0;q<FEAT_SIZE;q++)
<a name="l01111"></a>01111 K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(j,q);
<a name="l01112"></a>01112
<a name="l01113"></a>01113 Kij[k] = K_tmp / Sij;
<a name="l01114"></a>01114 } <span class="comment">// end for k</span>
<a name="l01115"></a>01115
<a name="l01116"></a>01116
<a name="l01117"></a>01117 <span class="comment">// Update the state vector m_xkk:</span>
<a name="l01118"></a>01118 <span class="comment">// x' = x + Kij * ytilde(ij)</span>
<a name="l01119"></a>01119 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<N;k++)
<a name="l01120"></a>01120 m_xkk[k] += Kij[k] * ytilde[j];
<a name="l01121"></a>01121
<a name="l01122"></a>01122 <span class="comment">// Update the covariance Pkk:</span>
<a name="l01123"></a>01123 <span class="comment">// P' = P - Kij * Sij * Kij^t</span>
<a name="l01124"></a>01124 {
<a name="l01125"></a>01125 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<N;k++)
<a name="l01126"></a>01126 {
<a name="l01127"></a>01127 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> q=k;q<N;q++) <span class="comment">// Half matrix</span>
<a name="l01128"></a>01128 {
<a name="l01129"></a>01129 m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
<a name="l01130"></a>01130 <span class="comment">// It is symmetric</span>
<a name="l01131"></a>01131 m_pkk(q,k) = m_pkk(k,q);
<a name="l01132"></a>01132 }
<a name="l01133"></a>01133
<a name="l01134"></a>01134 <span class="preprocessor"> #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)</span>
<a name="l01135"></a>01135 <span class="preprocessor"></span> <span class="keywordflow">if</span> (m_pkk(k,k)<0)
<a name="l01136"></a>01136 {
<a name="l01137"></a>01137 m_pkk.saveToTextFile(<span class="stringliteral">"Pkk_err.txt"</span>);
<a name="l01138"></a>01138 <a class="code" href="group__mrpt__system__os.html#ga2454cccef332c556b3efffc4d0ac24ef" title="A useful function for debuging, which saves a std::vector into a text file (compat.">mrpt::system::vectorToTextFile</a>(Kij,<span class="stringliteral">"Kij.txt"</span>);
<a name="l01139"></a>01139 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(m_pkk(k,k)>0)
<a name="l01140"></a>01140 }
<a name="l01141"></a>01141 <span class="preprocessor"> #endif</span>
<a name="l01142"></a>01142 <span class="preprocessor"></span> }
<a name="l01143"></a>01143 }
<a name="l01144"></a>01144
<a name="l01145"></a>01145
<a name="l01146"></a>01146 m_timLogger.leave(<span class="stringliteral">"KF:8.update stage:2.ScalarAtOnce.update"</span>);
<a name="l01147"></a>01147 } <span class="comment">// end for j</span>
<a name="l01148"></a>01148 } <span class="comment">// end if mapped</span>
<a name="l01149"></a>01149 } <span class="comment">// end for each observed LM.</span>
<a name="l01150"></a>01150 }
<a name="l01151"></a>01151 <span class="keywordflow">break</span>;
<a name="l01152"></a>01152
<a name="l01153"></a>01153 <span class="comment">// --------------------------------------------------------------------</span>
<a name="l01154"></a>01154 <span class="comment">// - IKF method, processing each observation scalar secuentially:</span>
<a name="l01155"></a>01155 <span class="comment">// --------------------------------------------------------------------</span>
<a name="l01156"></a>01156 <span class="keywordflow">case</span> <a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a0c3af1381c1d9b4b4ebb04370dc59ba9">kfIKF</a>: <span class="comment">// TODO !!</span>
<a name="l01157"></a>01157 {
<a name="l01158"></a>01158 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"IKF scalar by scalar not implemented yet."</span>);
<a name="l01159"></a>01159 <span class="preprocessor">#if 0</span>
<a name="l01160"></a>01160 <span class="preprocessor"></span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> h,Hx,Hy;
<a name="l01161"></a>01161
<a name="l01162"></a>01162 <span class="comment">// Just one, or several update iterations??</span>
<a name="l01163"></a>01163 <span class="keywordtype">size_t</span> nKF_iterations = KF_options.IKF_iterations;
<a name="l01164"></a>01164
<a name="l01165"></a>01165 <span class="comment">// To avoid wasting time, if we are doing 1 iteration only, do not reserve memory for this matrix:</span>
<a name="l01166"></a>01166 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> *saved_Pkk=NULL;
<a name="l01167"></a>01167 <span class="keywordflow">if</span> (nKF_iterations>1)
<a name="l01168"></a>01168 {
<a name="l01169"></a>01169 <span class="comment">// Create a copy of Pkk for later restoring it at the beginning of each iteration:</span>
<a name="l01170"></a>01170 saved_Pkk = <span class="keyword">new</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a>( m_pkk );
<a name="l01171"></a>01171 }
<a name="l01172"></a>01172
<a name="l01173"></a>01173 <a class="code" href="structmrpt_1_1dynamicsize__vector.html">KFVector</a> xkk_0 = m_xkk; <span class="comment">// First state vector at the beginning of IKF</span>
<a name="l01174"></a>01174 <a class="code" href="structmrpt_1_1dynamicsize__vector.html">KFVector</a> xkk_next_iter = m_xkk; <span class="comment">// for IKF only</span>
<a name="l01175"></a>01175
<a name="l01176"></a>01176 <span class="comment">// For each IKF iteration (or 1 for EKF)</span>
<a name="l01177"></a>01177 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
<a name="l01178"></a>01178 {
<a name="l01179"></a>01179 <span class="comment">// Restore initial covariance matrix.</span>
<a name="l01180"></a>01180 <span class="comment">// The updated mean is stored in m_xkk and is improved with each estimation,</span>
<a name="l01181"></a>01181 <span class="comment">// and so do the Jacobians which use that improving mean.</span>
<a name="l01182"></a>01182 <span class="keywordflow">if</span> (IKF_iteration>0)
<a name="l01183"></a>01183 {
<a name="l01184"></a>01184 m_pkk = *saved_Pkk;
<a name="l01185"></a>01185 xkk_next_iter = xkk_0;
<a name="l01186"></a>01186 }
<a name="l01187"></a>01187
<a name="l01188"></a>01188 <span class="comment">// For each observed landmark/whole system state:</span>
<a name="l01189"></a>01189 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> obsIdx=0;obsIdx<Z.getRowCount();obsIdx++)
<a name="l01190"></a>01190 {
<a name="l01191"></a>01191 <span class="comment">// Known & mapped landmark?</span>
<a name="l01192"></a>01192 <span class="keywordtype">bool</span> doit;
<a name="l01193"></a>01193 <span class="keywordtype">size_t</span> idxInTheFilter=0;
<a name="l01194"></a>01194
<a name="l01195"></a>01195 <span class="keywordflow">if</span> (data_association.empty())
<a name="l01196"></a>01196 {
<a name="l01197"></a>01197 doit = <span class="keyword">true</span>;
<a name="l01198"></a>01198 }
<a name="l01199"></a>01199 <span class="keywordflow">else</span>
<a name="l01200"></a>01200 {
<a name="l01201"></a>01201 doit = data_association[obsIdx] >= 0;
<a name="l01202"></a>01202 <span class="keywordflow">if</span> (doit)
<a name="l01203"></a>01203 idxInTheFilter = data_association[obsIdx];
<a name="l01204"></a>01204 }
<a name="l01205"></a>01205
<a name="l01206"></a>01206 <span class="keywordflow">if</span> ( doit )
<a name="l01207"></a>01207 {
<a name="l01208"></a>01208 <span class="comment">// Already mapped: OK</span>
<a name="l01209"></a>01209 <span class="keyword">const</span> <span class="keywordtype">size_t</span> idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE; <span class="comment">// The offset in m_xkk & Pkk.</span>
<a name="l01210"></a>01210 <span class="keyword">const</span> <span class="keywordtype">size_t</span> R_row_offset = obsIdx*OBS_SIZE; <span class="comment">// Offset row in R</span>
<a name="l01211"></a>01211
<a name="l01212"></a>01212 <span class="comment">// Compute just the part of the Jacobian that we need using the current updated m_xkk:</span>
<a name="l01213"></a>01213 <a class="code" href="structmrpt_1_1dynamicsize__vector.html">KFVector</a> ytilde; <span class="comment">// Diff. observation - prediction</span>
<a name="l01214"></a>01214 OnObservationModelAndJacobians(
<a name="l01215"></a>01215 Z,
<a name="l01216"></a>01216 data_association,
<a name="l01217"></a>01217 <span class="keyword">false</span>, <span class="comment">// Only a partial Jacobian</span>
<a name="l01218"></a>01218 (<span class="keywordtype">int</span>)obsIdx, <span class="comment">// Which row from Z</span>
<a name="l01219"></a>01219 ytilde,
<a name="l01220"></a>01220 Hx,
<a name="l01221"></a>01221 Hy );
<a name="l01222"></a>01222
<a name="l01223"></a>01223 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(ytilde.size() == OBS_SIZE )
<a name="l01224"></a>01224 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(Hx.getRowCount() == OBS_SIZE )
<a name="l01225"></a>01225 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(Hx.getColCount() == VEH_SIZE )
<a name="l01226"></a>01226
<a name="l01227"></a>01227 <span class="keywordflow">if</span> (FEAT_SIZE>0)
<a name="l01228"></a>01228 {
<a name="l01229"></a>01229 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(Hy.getRowCount() == OBS_SIZE )
<a name="l01230"></a>01230 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(Hy.getColCount() == FEAT_SIZE )
<a name="l01231"></a>01231 }
<a name="l01232"></a>01232
<a name="l01233"></a>01233 <span class="comment">// Compute the OxO matrix S_i for each observation:</span>
<a name="l01234"></a>01234 <span class="comment">// Si = TERM1 + TERM2 + TERM2^t + TERM3 + R</span>
<a name="l01235"></a>01235 <span class="comment">//</span>
<a name="l01236"></a>01236 <span class="comment">// TERM1: dhi_dxv Pxx dhi_dxv^t</span>
<a name="l01237"></a>01237 <span class="comment">// ^^</span>
<a name="l01238"></a>01238 <span class="comment">// Hx:(OxV)</span>
<a name="l01239"></a>01239 <span class="comment">//</span>
<a name="l01240"></a>01240 <span class="comment">// TERM2: dhi_dyi Pyix dhi_dxv</span>
<a name="l01241"></a>01241 <span class="comment">// ^^</span>
<a name="l01242"></a>01242 <span class="comment">// Hy:(OxF)</span>
<a name="l01243"></a>01243 <span class="comment">//</span>
<a name="l01244"></a>01244 <span class="comment">// TERM3: dhi_dyi Pyiyi dhi_dyi^t</span>
<a name="l01245"></a>01245 <span class="comment">//</span>
<a name="l01246"></a>01246 <span class="comment">// i: obsIdx</span>
<a name="l01247"></a>01247 <span class="comment">// O: Observation size</span>
<a name="l01248"></a>01248 <span class="comment">// F: Feature size</span>
<a name="l01249"></a>01249 <span class="comment">// V: Vehicle state size</span>
<a name="l01250"></a>01250 <span class="comment">//</span>
<a name="l01251"></a>01251
<a name="l01252"></a>01252 <span class="comment">// R:</span>
<a name="l01253"></a>01253 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> Si(OBS_SIZE,OBS_SIZE);
<a name="l01254"></a>01254 R.extractMatrix(R_row_offset,0, Si);
<a name="l01255"></a>01255
<a name="l01256"></a>01256 <span class="keywordtype">size_t</span> k;
<a name="l01257"></a>01257 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> term(OBS_SIZE,OBS_SIZE);
<a name="l01258"></a>01258
<a name="l01259"></a>01259 <span class="comment">// TERM1: dhi_dxv Pxx dhi_dxv^t</span>
<a name="l01260"></a>01260 Hx.multiply_HCHt(
<a name="l01261"></a>01261 m_pkk, <span class="comment">// The cov. matrix</span>
<a name="l01262"></a>01262 Si, <span class="comment">// The output</span>
<a name="l01263"></a>01263 <span class="keyword">true</span>, <span class="comment">// Yes, submatrix of m_pkk only</span>
<a name="l01264"></a>01264 0, <span class="comment">// Offset in m_pkk</span>
<a name="l01265"></a>01265 <span class="keyword">true</span> <span class="comment">// Yes, accum results in output.</span>
<a name="l01266"></a>01266 );
<a name="l01267"></a>01267
<a name="l01268"></a>01268 <span class="comment">// TERM2: dhi_dyi Pyix dhi_dxv</span>
<a name="l01269"></a>01269 <span class="comment">// + its transpose:</span>
<a name="l01270"></a>01270 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> Pyix( FEAT_SIZE, VEH_SIZE );
<a name="l01271"></a>01271 m_pkk.extractMatrix(idx_off,0, Pyix); <span class="comment">// Extract cross cov. to Pyix</span>
<a name="l01272"></a>01272
<a name="l01273"></a>01273 term.multiply_ABCt( Hy, Pyix, Hx ); <span class="comment">//term = Hy * Pyix * ~Hx;</span>
<a name="l01274"></a>01274 Si.add_AAt( term ); <span class="comment">// Si += A + ~A</span>
<a name="l01275"></a>01275
<a name="l01276"></a>01276 <span class="comment">// TERM3: dhi_dyi Pyiyi dhi_dyi^t</span>
<a name="l01277"></a>01277 Hy.multiply_HCHt(
<a name="l01278"></a>01278 m_pkk, <span class="comment">// The cov. matrix</span>
<a name="l01279"></a>01279 Si, <span class="comment">// The output</span>
<a name="l01280"></a>01280 <span class="keyword">true</span>, <span class="comment">// Yes, submatrix of m_pkk only</span>
<a name="l01281"></a>01281 idx_off, <span class="comment">// Offset in m_pkk</span>
<a name="l01282"></a>01282 <span class="keyword">true</span> <span class="comment">// Yes, accum results in output.</span>
<a name="l01283"></a>01283 );
<a name="l01284"></a>01284
<a name="l01285"></a>01285 <span class="comment">// Compute the inverse of Si:</span>
<a name="l01286"></a>01286 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> Si_1(OBS_SIZE,OBS_SIZE);
<a name="l01287"></a>01287
<a name="l01288"></a>01288 <span class="comment">// Compute the Kalman gain "Ki" for this i'th observation:</span>
<a name="l01289"></a>01289 <span class="comment">// --> Ki = m_pkk * (~dh_dx) * S.inv();</span>
<a name="l01290"></a>01290 <span class="keywordtype">size_t</span> N = m_pkk.getColCount();
<a name="l01291"></a>01291
<a name="l01292"></a>01292 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">KFMatrix</a> Ki( N, OBS_SIZE );
<a name="l01293"></a>01293
<a name="l01294"></a>01294 <span class="keywordflow">for</span> (k=0;k<N;k++) <span class="comment">// for each row of K</span>
<a name="l01295"></a>01295 {
<a name="l01296"></a>01296 <span class="keywordtype">size_t</span> q;
<a name="l01297"></a>01297
<a name="l01298"></a>01298 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> c=0;c<OBS_SIZE;c++) <span class="comment">// for each column of K</span>
<a name="l01299"></a>01299 {
<a name="l01300"></a>01300 KFTYPE K_tmp = 0;
<a name="l01301"></a>01301
<a name="l01302"></a>01302 <span class="comment">// dhi_dxv term</span>
<a name="l01303"></a>01303 <span class="keywordflow">for</span> (q=0;q<VEH_SIZE;q++)
<a name="l01304"></a>01304 K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(c,q);
<a name="l01305"></a>01305
<a name="l01306"></a>01306 <span class="comment">// dhi_dyi term</span>
<a name="l01307"></a>01307 <span class="keywordflow">for</span> (q=0;q<FEAT_SIZE;q++)
<a name="l01308"></a>01308 K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(c,q);
<a name="l01309"></a>01309
<a name="l01310"></a>01310 Ki.set_unsafe(k,c, K_tmp);
<a name="l01311"></a>01311 } <span class="comment">// end for c</span>
<a name="l01312"></a>01312 } <span class="comment">// end for k</span>
<a name="l01313"></a>01313
<a name="l01314"></a>01314 Ki.multiply(Ki, Si.inv() ); <span class="comment">// K = (...) * S^-1</span>
<a name="l01315"></a>01315
<a name="l01316"></a>01316
<a name="l01317"></a>01317 <span class="comment">// Update the state vector m_xkk:</span>
<a name="l01318"></a>01318 <span class="keywordflow">if</span> (nKF_iterations==1)
<a name="l01319"></a>01319 {
<a name="l01320"></a>01320 <span class="comment">// EKF:</span>
<a name="l01321"></a>01321 <span class="comment">// x' = x + Kij * ytilde(ij)</span>
<a name="l01322"></a>01322 <span class="keywordflow">for</span> (k=0;k<N;k++)
<a name="l01323"></a>01323 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> q=0;q<OBS_SIZE;q++)
<a name="l01324"></a>01324 m_xkk[k] += Ki.get_unsafe(k,q) * ytilde[q];
<a name="l01325"></a>01325 }
<a name="l01326"></a>01326 <span class="keywordflow">else</span>
<a name="l01327"></a>01327 {
<a name="l01328"></a>01328 <span class="comment">// IKF:</span>
<a name="l01329"></a>01329 mrpt<a class="code" href="structmrpt_1_1dynamicsize__vector.html">::dynamicsize_vector<KFTYPE></a> HAx(OBS_SIZE);
<a name="l01330"></a>01330 <span class="keywordtype">size_t</span> o,q;
<a name="l01331"></a>01331 <span class="comment">// HAx = H*(x0-xi)</span>
<a name="l01332"></a>01332 <span class="keywordflow">for</span> (o=0;o<OBS_SIZE;o++)
<a name="l01333"></a>01333 {
<a name="l01334"></a>01334 KFTYPE tmp = 0;
<a name="l01335"></a>01335 <span class="keywordflow">for</span> (q=0;q<VEH_SIZE;q++)
<a name="l01336"></a>01336 tmp += Hx.get_unsafe(o,q) * (xkk_0[q] - m_xkk[q]);
<a name="l01337"></a>01337
<a name="l01338"></a>01338 <span class="keywordflow">for</span> (q=0;q<FEAT_SIZE;q++)
<a name="l01339"></a>01339 tmp += Hy.get_unsafe(o,q) * (xkk_0[idx_off+q] - m_xkk[idx_off+q]);
<a name="l01340"></a>01340
<a name="l01341"></a>01341 HAx[o] = tmp;
<a name="l01342"></a>01342 }
<a name="l01343"></a>01343
<a name="l01344"></a>01344 <span class="comment">// Compute only once (ytilde[j] - HAx)</span>
<a name="l01345"></a>01345 <span class="keywordflow">for</span> (o=0;o<OBS_SIZE;o++)
<a name="l01346"></a>01346 HAx[o] = ytilde[o] - HAx[o];
<a name="l01347"></a>01347
<a name="l01348"></a>01348 <span class="comment">// x' = x_0 + Kij * ( ytilde(ij) - H*(x0-xi)) -> xi: i=this iteration</span>
<a name="l01349"></a>01349 <span class="comment">// xkk_next_iter is initialized to xkk_0 at each IKF iteration.</span>
<a name="l01350"></a>01350 <span class="keywordflow">for</span> (k=0;k<N;k++)
<a name="l01351"></a>01351 {
<a name="l01352"></a>01352 KFTYPE tmp = xkk_next_iter[k];
<a name="l01353"></a>01353 <span class="comment">//m_xkk[k] = xkk_0[k] + Kij[k] * (ytilde[j] - HAx );</span>
<a name="l01354"></a>01354 <span class="keywordflow">for</span> (o=0;o<OBS_SIZE;o++)
<a name="l01355"></a>01355 tmp += Ki.get_unsafe(k,o) * HAx[o];
<a name="l01356"></a>01356
<a name="l01357"></a>01357 xkk_next_iter[k] = tmp;
<a name="l01358"></a>01358 }
<a name="l01359"></a>01359 }
<a name="l01360"></a>01360
<a name="l01361"></a>01361 <span class="comment">// Update the covariance Pkk:</span>
<a name="l01362"></a>01362 <span class="comment">// P' = P - Kij * Sij * Kij^t</span>
<a name="l01363"></a>01363 <span class="comment">//if (IKF_iteration==(nKF_iterations-1)) // Just for the last IKF iteration</span>
<a name="l01364"></a>01364 {
<a name="l01365"></a>01365 <span class="comment">// m_pkk -= Ki * Si * ~Ki;</span>
<a name="l01366"></a>01366 Ki.multiplyByMatrixAndByTransposeNonSymmetric(
<a name="l01367"></a>01367 Si,
<a name="l01368"></a>01368 m_pkk, <span class="comment">// Output</span>
<a name="l01369"></a>01369 <span class="keyword">true</span>, <span class="comment">// Accumulate</span>
<a name="l01370"></a>01370 <span class="keyword">true</span>); <span class="comment">// Substract instead of sum.</span>
<a name="l01371"></a>01371
<a name="l01372"></a>01372 m_pkk.force_symmetry();
<a name="l01373"></a>01373
<a name="l01374"></a>01374 <span class="comment">/* for (k=0;k<N;k++)</span>
<a name="l01375"></a>01375 <span class="comment"> {</span>
<a name="l01376"></a>01376 <span class="comment"> for (size_t q=k;q<N;q++) // Half matrix</span>
<a name="l01377"></a>01377 <span class="comment"> {</span>
<a name="l01378"></a>01378 <span class="comment"> m_pkk(k,q) -= Sij * Kij[k] * Kij[q];</span>
<a name="l01379"></a>01379 <span class="comment"> // It is symmetric</span>
<a name="l01380"></a>01380 <span class="comment"> m_pkk(q,k) = m_pkk(k,q);</span>
<a name="l01381"></a>01381 <span class="comment"> }</span>
<a name="l01382"></a>01382 <span class="comment"></span>
<a name="l01383"></a>01383 <span class="comment"> #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)</span>
<a name="l01384"></a>01384 <span class="comment"> if (m_pkk(k,k)<0)</span>
<a name="l01385"></a>01385 <span class="comment"> {</span>
<a name="l01386"></a>01386 <span class="comment"> m_pkk.saveToTextFile("Pkk_err.txt");</span>
<a name="l01387"></a>01387 <span class="comment"> mrpt::system::vectorToTextFile(Kij,"Kij.txt");</span>
<a name="l01388"></a>01388 <span class="comment"> ASSERT_(m_pkk(k,k)>0)</span>
<a name="l01389"></a>01389 <span class="comment"> }</span>
<a name="l01390"></a>01390 <span class="comment"> #endif</span>
<a name="l01391"></a>01391 <span class="comment"> }</span>
<a name="l01392"></a>01392 <span class="comment"> */</span>
<a name="l01393"></a>01393 }
<a name="l01394"></a>01394
<a name="l01395"></a>01395 } <span class="comment">// end if doit</span>
<a name="l01396"></a>01396
<a name="l01397"></a>01397 } <span class="comment">// end for each observed LM.</span>
<a name="l01398"></a>01398
<a name="l01399"></a>01399 <span class="comment">// Now, save the new iterated mean:</span>
<a name="l01400"></a>01400 <span class="keywordflow">if</span> (nKF_iterations>1)
<a name="l01401"></a>01401 {
<a name="l01402"></a>01402 <span class="preprocessor"> #if 0</span>
<a name="l01403"></a>01403 <span class="preprocessor"></span> cout << <span class="stringliteral">"IKF iter: "</span> << IKF_iteration << <span class="stringliteral">" -> "</span> << xkk_next_iter << endl;
<a name="l01404"></a>01404 <span class="preprocessor"> #endif</span>
<a name="l01405"></a>01405 <span class="preprocessor"></span> m_xkk = xkk_next_iter;
<a name="l01406"></a>01406 }
<a name="l01407"></a>01407
<a name="l01408"></a>01408 } <span class="comment">// end for each IKF_iteration</span>
<a name="l01409"></a>01409
<a name="l01410"></a>01410 <span class="comment">// Copy of pkk not required anymore:</span>
<a name="l01411"></a>01411 <span class="keywordflow">if</span> (saved_Pkk) <span class="keyword">delete</span> saved_Pkk;
<a name="l01412"></a>01412
<a name="l01413"></a>01413 <span class="preprocessor">#endif</span>
<a name="l01414"></a>01414 <span class="preprocessor"></span> }
<a name="l01415"></a>01415 <span class="keywordflow">break</span>;
<a name="l01416"></a>01416
<a name="l01417"></a>01417 <span class="keywordflow">default</span>:
<a name="l01418"></a>01418 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Invalid value of options.KF_method"</span>);
<a name="l01419"></a>01419 } <span class="comment">// end switch method</span>
<a name="l01420"></a>01420
<a name="l01421"></a>01421 }
<a name="l01422"></a>01422
<a name="l01423"></a>01423 <span class="keyword">const</span> <span class="keywordtype">double</span> tim_update = m_timLogger.leave(<span class="stringliteral">"KF:8.update stage"</span>);
<a name="l01424"></a>01424
<a name="l01425"></a>01425 OnNormalizeStateVector();
<a name="l01426"></a>01426
<a name="l01427"></a>01427 <span class="comment">// =============================================================</span>
<a name="l01428"></a>01428 <span class="comment">// 8. INTRODUCE NEW LANDMARKS</span>
<a name="l01429"></a>01429 <span class="comment">// =============================================================</span>
<a name="l01430"></a>01430 <span class="keywordflow">if</span> (!data_association.empty())
<a name="l01431"></a>01431 {
<a name="l01432"></a>01432 <a class="code" href="structmrpt_1_1bayes_1_1detail_1_1_c_run_one_kalman_iteration__add_new_landmarks.html">detail::CRunOneKalmanIteration_addNewLandmarks</a>()(*<span class="keyword">this</span>, Z, data_association,R);
<a name="l01433"></a>01433 } <span class="comment">// end if data_association!=empty</span>
<a name="l01434"></a>01434
<a name="l01435"></a>01435 <span class="keywordflow">if</span> (KF_options.verbose)
<a name="l01436"></a>01436 {
<a name="l01437"></a>01437 printf_debug(<span class="stringliteral">"[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: %.2fms\n"</span>,
<a name="l01438"></a>01438 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
<a name="l01439"></a>01439 1e3*tim_pred,
<a name="l01440"></a>01440 1e3*tim_pred_obs,
<a name="l01441"></a>01441 1e3*tim_obs_DA,
<a name="l01442"></a>01442 1e3*tim_update
<a name="l01443"></a>01443 );
<a name="l01444"></a>01444 }
<a name="l01445"></a>01445
<a name="l01446"></a>01446 <span class="comment">// Post iteration user code:</span>
<a name="l01447"></a>01447 OnPostIteration();
<a name="l01448"></a>01448
<a name="l01449"></a>01449 m_timLogger.leave(<span class="stringliteral">"KF:complete_step"</span>);
<a name="l01450"></a>01450
<a name="l01451"></a>01451 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l01452"></a>01452
<a name="l01453"></a>01453 } <span class="comment">// End of "runOneKalmanIteration"</span>
<a name="l01454"></a>01454
<a name="l01455"></a>01455
<a name="l01456"></a>01456
<a name="l01457"></a>01457 <span class="keyword">private</span>:
<a name="l01458"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ada33322b67cea44a95948712be1c131b">01458</a> <span class="keyword">mutable</span> <span class="keywordtype">bool</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#ada33322b67cea44a95948712be1c131b">m_user_didnt_implement_jacobian</a>;
<a name="l01459"></a>01459 <span class="comment"></span>
<a name="l01460"></a>01460 <span class="comment"> /** Auxiliary functions for Jacobian numeric estimation */</span>
<a name="l01461"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a713c4352da9cc10f827a52ed852a9e6a">01461</a> <span class="keyword">static</span> <span class="keywordtype">void</span> KF_aux_estimate_trans_jacobian(
<a name="l01462"></a>01462 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> &x,
<a name="l01463"></a>01463 <span class="keyword">const</span> std::pair<KFCLASS*,KFArray_ACT> &dat,
<a name="l01464"></a>01464 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> &out_x)
<a name="l01465"></a>01465 {
<a name="l01466"></a>01466 <span class="keywordtype">bool</span> dumm;
<a name="l01467"></a>01467 out_x=x;
<a name="l01468"></a>01468 dat.first->OnTransitionModel(dat.second,out_x, dumm);
<a name="l01469"></a>01469 }
<a name="l01470"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a8fb02067b7c9ff1210085bbd8ba25204">01470</a> <span class="keyword">static</span> <span class="keywordtype">void</span> KF_aux_estimate_obs_Hx_jacobian(
<a name="l01471"></a>01471 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_VEH</a> &x,
<a name="l01472"></a>01472 <span class="keyword">const</span> std::pair<KFCLASS*,size_t> &dat,
<a name="l01473"></a>01473 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> &out_x)
<a name="l01474"></a>01474 {
<a name="l01475"></a>01475 <a class="code" href="classstd_1_1vector.html">vector_size_t</a> idxs_to_predict(1,dat.second);
<a name="l01476"></a>01476 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_KFArray_OBS</a> prediction;
<a name="l01477"></a>01477 <span class="comment">// Overwrite (temporarily!) the affected part of the state vector:</span>
<a name="l01478"></a>01478 <a class="code" href="group__mrpt__system__os.html#gae1184cfb1f617787dc4c9da98becbe3a" title="An OS and compiler independent version of "memcpy".">::memcpy</a>(&dat.first->m_xkk[0],&x[0],<span class="keyword">sizeof</span>(x[0])*VEH_SIZE);
<a name="l01479"></a>01479 dat.first->OnObservationModel(idxs_to_predict,prediction);
<a name="l01480"></a>01480 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(prediction.size()==1);
<a name="l01481"></a>01481 out_x=prediction[0];
<a name="l01482"></a>01482 }
<a name="l01483"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a07208df3280ab8f80b4952ab09bfd55a">01483</a> <span class="keyword">static</span> <span class="keywordtype">void</span> KF_aux_estimate_obs_Hy_jacobian(
<a name="l01484"></a>01484 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_FEAT</a> &x,
<a name="l01485"></a>01485 <span class="keyword">const</span> std::pair<KFCLASS*,size_t> &dat,
<a name="l01486"></a>01486 <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html" title="CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...">KFArray_OBS</a> &out_x)
<a name="l01487"></a>01487 {
<a name="l01488"></a>01488 <a class="code" href="classstd_1_1vector.html">vector_size_t</a> idxs_to_predict(1,dat.second);
<a name="l01489"></a>01489 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_KFArray_OBS</a> prediction;
<a name="l01490"></a>01490 <span class="keyword">const</span> <span class="keywordtype">size_t</span> lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
<a name="l01491"></a>01491 <span class="comment">// Overwrite (temporarily!) the affected part of the state vector:</span>
<a name="l01492"></a>01492 <a class="code" href="group__mrpt__system__os.html#gae1184cfb1f617787dc4c9da98becbe3a" title="An OS and compiler independent version of "memcpy".">::memcpy</a>(&dat.first->m_xkk[lm_idx_in_statevector],&x[0],<span class="keyword">sizeof</span>(x[0])*FEAT_SIZE);
<a name="l01493"></a>01493 dat.first->OnObservationModel(idxs_to_predict,prediction);
<a name="l01494"></a>01494 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(prediction.size()==1);
<a name="l01495"></a>01495 out_x=prediction[0];
<a name="l01496"></a>01496 }
<a name="l01497"></a>01497
<a name="l01498"></a>01498
<a name="l01499"></a><a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html#a1f7c0688afe03ffd6f14c4e9f4e5a050">01499</a> <span class="keyword">friend</span> <span class="keyword">struct </span><a class="code" href="structmrpt_1_1bayes_1_1detail_1_1_c_run_one_kalman_iteration__add_new_landmarks.html">detail::CRunOneKalmanIteration_addNewLandmarks</a>;
<a name="l01500"></a>01500
<a name="l01501"></a>01501 }; <span class="comment">// end class</span>
<a name="l01502"></a>01502
<a name="l01503"></a>01503 <span class="keyword">namespace </span>detail
<a name="l01504"></a>01504 {
<a name="l01505"></a>01505 <span class="comment">// generic version for SLAM. There is a speciation below for NON-SLAM problems.</span>
<a name="l01506"></a>01506 <span class="keyword">struct </span>CRunOneKalmanIteration_addNewLandmarks {
<a name="l01507"></a>01507 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> FEAT_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l01508"></a><a class="code" href="structmrpt_1_1bayes_1_1detail_1_1_c_run_one_kalman_iteration__add_new_landmarks.html#a33a1d2428276d43021c282e611d4f9a2">01508</a> <span class="keywordtype">void</span> operator()(
<a name="l01509"></a>01509 <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE></a> &obj,
<a name="l01510"></a>01510 <span class="keyword">const</span> <span class="keyword">typename</span> <a class="code" href="classstd_1_1vector.html" title="STL class.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::vector_KFArray_OBS</a> & Z,
<a name="l01511"></a>01511 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_int</a> &data_association,
<a name="l01512"></a>01512 <span class="keyword">const</span> <span class="keyword">typename</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::KFMatrix_OxO</a> &R
<a name="l01513"></a>01513 )
<a name="l01514"></a>01514 {
<a name="l01515"></a>01515 <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE></a> KF;
<a name="l01516"></a>01516
<a name="l01517"></a>01517 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> idxObs=0;idxObs<Z.size();idxObs++)
<a name="l01518"></a>01518 {
<a name="l01519"></a>01519 <span class="comment">// Is already in the map?</span>
<a name="l01520"></a>01520 <span class="keywordflow">if</span> ( data_association[idxObs] < 0 ) <span class="comment">// Not in the map yet!</span>
<a name="l01521"></a>01521 {
<a name="l01522"></a>01522 obj.m_timLogger.enter(<span class="stringliteral">"KF:9.create new LMs"</span>);
<a name="l01523"></a>01523 <span class="comment">// Add it:</span>
<a name="l01524"></a>01524
<a name="l01525"></a>01525 <span class="comment">// Append to map of IDs <-> position in the state vector:</span>
<a name="l01526"></a>01526 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(FEAT_SIZE>0)
<a name="l01527"></a>01527 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>( 0 == ((obj.m_xkk.size() - VEH_SIZE) % FEAT_SIZE) ) <span class="comment">// Sanity test</span>
<a name="l01528"></a>01528
<a name="l01529"></a>01529 <span class="keyword">const</span> <span class="keywordtype">size_t</span> newIndexInMap = (obj.m_xkk.size() - VEH_SIZE) / FEAT_SIZE;
<a name="l01530"></a>01530
<a name="l01531"></a>01531 <span class="comment">// Inverse sensor model:</span>
<a name="l01532"></a>01532 <span class="keyword">typename</span> KF::KFArray_FEAT yn;
<a name="l01533"></a>01533 <span class="keyword">typename</span> KF::KFMatrix_FxV dyn_dxv;
<a name="l01534"></a>01534 <span class="keyword">typename</span> KF::KFMatrix_FxO dyn_dhn;
<a name="l01535"></a>01535 <span class="keyword">typename</span> KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
<a name="l01536"></a>01536 <span class="keywordtype">bool</span> use_dyn_dhn_jacobian=<span class="keyword">true</span>;
<a name="l01537"></a>01537
<a name="l01538"></a>01538 <span class="comment">// Compute the inv. sensor model and its Jacobians:</span>
<a name="l01539"></a>01539 obj.OnInverseObservationModel(
<a name="l01540"></a>01540 Z[idxObs],
<a name="l01541"></a>01541 yn,
<a name="l01542"></a>01542 dyn_dxv,
<a name="l01543"></a>01543 dyn_dhn,
<a name="l01544"></a>01544 dyn_dhn_R_dyn_dhnT,
<a name="l01545"></a>01545 use_dyn_dhn_jacobian );
<a name="l01546"></a>01546
<a name="l01547"></a>01547 <span class="comment">// And let the application do any special handling of adding a new feature to the map:</span>
<a name="l01548"></a>01548 obj.OnNewLandmarkAddedToMap(
<a name="l01549"></a>01549 idxObs,
<a name="l01550"></a>01550 newIndexInMap
<a name="l01551"></a>01551 );
<a name="l01552"></a>01552
<a name="l01553"></a>01553 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>( yn.size() == FEAT_SIZE )
<a name="l01554"></a>01554
<a name="l01555"></a>01555 <span class="comment">// Append to m_xkk:</span>
<a name="l01556"></a>01556 <span class="keywordtype">size_t</span> q;
<a name="l01557"></a>01557 <span class="keywordtype">size_t</span> idx = obj.m_xkk.size();
<a name="l01558"></a>01558 obj.m_xkk.resize( obj.m_xkk.size() + FEAT_SIZE );
<a name="l01559"></a>01559
<a name="l01560"></a>01560 <span class="keywordflow">for</span> (q=0;q<FEAT_SIZE;q++)
<a name="l01561"></a>01561 obj.m_xkk[idx+q] = yn[q];
<a name="l01562"></a>01562
<a name="l01563"></a>01563 <span class="comment">// --------------------</span>
<a name="l01564"></a>01564 <span class="comment">// Append to Pkk:</span>
<a name="l01565"></a>01565 <span class="comment">// --------------------</span>
<a name="l01566"></a>01566 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>( obj.m_pkk.getColCount()==idx && obj.m_pkk.getRowCount()==idx );
<a name="l01567"></a>01567
<a name="l01568"></a>01568 obj.m_pkk.setSize( idx+FEAT_SIZE,idx+FEAT_SIZE );
<a name="l01569"></a>01569
<a name="l01570"></a>01570 <span class="comment">// Fill the Pxyn term:</span>
<a name="l01571"></a>01571 <span class="comment">// --------------------</span>
<a name="l01572"></a>01572 <span class="keyword">typename</span> KF::KFMatrix_VxV Pxx;
<a name="l01573"></a>01573 obj.m_pkk.extractMatrix(0,0,Pxx);
<a name="l01574"></a>01574 <span class="keyword">typename</span> KF::KFMatrix_FxV Pxyn; <span class="comment">// Pxyn = dyn_dxv * Pxx</span>
<a name="l01575"></a>01575 Pxyn.multiply( dyn_dxv, Pxx );
<a name="l01576"></a>01576
<a name="l01577"></a>01577 obj.m_pkk.insertMatrix( idx,0, Pxyn );
<a name="l01578"></a>01578 obj.m_pkk.insertMatrixTranspose( 0,idx, Pxyn );
<a name="l01579"></a>01579
<a name="l01580"></a>01580 <span class="comment">// Fill the Pyiyn terms:</span>
<a name="l01581"></a>01581 <span class="comment">// --------------------</span>
<a name="l01582"></a>01582 <span class="keyword">const</span> <span class="keywordtype">size_t</span> nLMs = (idx-VEH_SIZE)/FEAT_SIZE; <span class="comment">// Number of previous landmarks:</span>
<a name="l01583"></a>01583 <span class="keywordflow">for</span> (q=0;q<nLMs;q++)
<a name="l01584"></a>01584 {
<a name="l01585"></a>01585 <span class="keyword">typename</span> KF::KFMatrix_VxF P_x_yq(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l01586"></a>01586 obj.m_pkk.extractMatrix(0,VEH_SIZE+q*FEAT_SIZE,P_x_yq) ;
<a name="l01587"></a>01587
<a name="l01588"></a>01588 <span class="keyword">typename</span> KF::KFMatrix_FxF P_cross(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l01589"></a>01589 P_cross.multiply(dyn_dxv, P_x_yq );
<a name="l01590"></a>01590
<a name="l01591"></a>01591 obj.m_pkk.insertMatrix(idx,VEH_SIZE+q*FEAT_SIZE, P_cross );
<a name="l01592"></a>01592 obj.m_pkk.insertMatrixTranspose(VEH_SIZE+q*FEAT_SIZE,idx, P_cross );
<a name="l01593"></a>01593 } <span class="comment">// end each previous LM(q)</span>
<a name="l01594"></a>01594
<a name="l01595"></a>01595 <span class="comment">// Fill the Pynyn term:</span>
<a name="l01596"></a>01596 <span class="comment">// P_yn_yn = (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R * ~dyn_dhn);</span>
<a name="l01597"></a>01597 <span class="comment">// --------------------</span>
<a name="l01598"></a>01598 <span class="keyword">typename</span> KF::KFMatrix_FxF P_yn_yn(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l01599"></a>01599 dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
<a name="l01600"></a>01600 <span class="keywordflow">if</span> (use_dyn_dhn_jacobian)
<a name="l01601"></a>01601 dyn_dhn.multiply_HCHt(R, P_yn_yn, <span class="keyword">true</span>); <span class="comment">// Accumulate in P_yn_yn</span>
<a name="l01602"></a>01602 <span class="keywordflow">else</span> P_yn_yn+=dyn_dhn_R_dyn_dhnT;
<a name="l01603"></a>01603
<a name="l01604"></a>01604 obj.m_pkk.insertMatrix(idx,idx, P_yn_yn );
<a name="l01605"></a>01605
<a name="l01606"></a>01606 obj.m_timLogger.leave(<span class="stringliteral">"KF:9.create new LMs"</span>);
<a name="l01607"></a>01607 }
<a name="l01608"></a>01608 }
<a name="l01609"></a>01609 }
<a name="l01610"></a>01610
<a name="l01611"></a>01611 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l01612"></a><a class="code" href="structmrpt_1_1bayes_1_1detail_1_1_c_run_one_kalman_iteration__add_new_landmarks.html#a7fb7cdc74f9d74130b0e4983e6c11543">01612</a> <span class="keywordtype">void</span> operator()(
<a name="l01613"></a>01613 <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE></a> &obj,
<a name="l01614"></a>01614 <span class="keyword">const</span> <span class="keyword">typename</span> <a class="code" href="classstd_1_1vector.html" title="STL class.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::vector_KFArray_OBS</a> & Z,
<a name="l01615"></a>01615 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_int</a> &data_association,
<a name="l01616"></a>01616 <span class="keyword">const</span> <span class="keyword">typename</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::KFMatrix_OxO</a> &R
<a name="l01617"></a>01617 )
<a name="l01618"></a>01618 {
<a name="l01619"></a>01619 <span class="comment">// Do nothing: this is NOT a SLAM problem.</span>
<a name="l01620"></a>01620 }
<a name="l01621"></a>01621
<a name="l01622"></a>01622 }; <span class="comment">// end runOneKalmanIteration_addNewLandmarks</span>
<a name="l01623"></a>01623
<a name="l01624"></a>01624 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> FEAT_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l01625"></a><a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aeec894a458a3e7175e707871cd796f34">01625</a> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aeec894a458a3e7175e707871cd796f34">getNumberOfLandmarksInMap</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE></a> &obj)
<a name="l01626"></a>01626 {
<a name="l01627"></a>01627 <span class="keywordflow">return</span> (obj.getStateVectorLength()-VEH_SIZE)/FEAT_SIZE;
<a name="l01628"></a>01628 }
<a name="l01629"></a>01629 <span class="comment">// Specialization for FEAT_SIZE=0</span>
<a name="l01630"></a>01630 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l01631"></a><a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#abb695f7e7ecf6a5deb79b1c4190536b7">01631</a> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aeec894a458a3e7175e707871cd796f34">getNumberOfLandmarksInMap</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE></a> &obj)
<a name="l01632"></a>01632 {
<a name="l01633"></a>01633 <span class="keywordflow">return</span> 0;
<a name="l01634"></a>01634 }
<a name="l01635"></a>01635
<a name="l01636"></a>01636 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> FEAT_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l01637"></a><a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aafff1b2b519f6fb209de6ebde0830c19">01637</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aafff1b2b519f6fb209de6ebde0830c19">isMapEmpty</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE></a> &obj)
<a name="l01638"></a>01638 {
<a name="l01639"></a>01639 <span class="keywordflow">return</span> (obj.getStateVectorLength()==VEH_SIZE);
<a name="l01640"></a>01640 }
<a name="l01641"></a>01641 <span class="comment">// Specialization for FEAT_SIZE=0</span>
<a name="l01642"></a>01642 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> VEH_SIZE, <span class="keywordtype">size_t</span> OBS_SIZE, <span class="keywordtype">size_t</span> ACT_SIZE, <span class="keyword">typename</span> KFTYPE>
<a name="l01643"></a><a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#af98c0e1cce6e03e69204df0c376392eb">01643</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1bayes_1_1detail.html#aafff1b2b519f6fb209de6ebde0830c19">isMapEmpty</a> (<span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE></a> &obj)
<a name="l01644"></a>01644 {
<a name="l01645"></a>01645 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l01646"></a>01646 }
<a name="l01647"></a>01647
<a name="l01648"></a>01648 } <span class="comment">// end namespace "detail"</span>
<a name="l01649"></a>01649
<a name="l01650"></a>01650 } <span class="comment">// end namespace</span>
<a name="l01651"></a>01651
<a name="l01652"></a>01652 <span class="comment">// Specializations MUST occur at the same namespace:</span>
<a name="l01653"></a>01653 <span class="keyword">namespace </span>utils
<a name="l01654"></a>01654 {
<a name="l01655"></a>01655 <span class="keyword">template</span> <>
<a name="l01656"></a>01656 <span class="keyword">struct </span><a class="code" href="structmrpt_1_1utils_1_1_t_enum_type_filler.html" title="Only specializations of this class are defined for each enum type of interest.">TEnumTypeFiller</a><bayes::<a class="code" href="group__mrpt__bayes__grp.html#ga93ff73c3a01619f01d97fd80dff40d14" title="The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...">TKFMethod</a>>
<a name="l01657"></a>01657 {
<a name="l01658"></a><a class="code" href="structmrpt_1_1utils_1_1_t_enum_type_filler_3_01bayes_1_1_t_k_f_method_01_4.html#acf78d97716e240c8ba8e5a6b8380b075">01658</a> <span class="keyword">typedef</span> bayes<a class="code" href="group__mrpt__bayes__grp.html#ga93ff73c3a01619f01d97fd80dff40d14" title="The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...">::TKFMethod</a> <a class="code" href="structmrpt_1_1utils_1_1_t_enum_type_filler_3_01bayes_1_1_t_k_f_method_01_4.html#acf78d97716e240c8ba8e5a6b8380b075">enum_t</a>;
<a name="l01659"></a><a class="code" href="structmrpt_1_1utils_1_1_t_enum_type_filler_3_01bayes_1_1_t_k_f_method_01_4.html#a995cd62ac02eeefa56cf7026e9a3a25c">01659</a> <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="eigen__plugins_8h.html#a57fce471b07c3c84924883b5e17e2388">fill</a>(<a class="code" href="classmrpt_1_1utils_1_1bimap.html" title="A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std::...">bimap<enum_t,std::string></a> &m_map)
<a name="l01660"></a>01660 {
<a name="l01661"></a>01661 m_map.<a class="code" href="classmrpt_1_1utils_1_1bimap.html#af21e6b0fcd4c2046adf6e6a9a9b3c997" title="Insert a new pair KEY<->VALUE in the bi-map.">insert</a>(<a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a5ad91d60187b5cca61d25994f7827dc0">bayes::kfEKFNaive</a>, <span class="stringliteral">"kfEKFNaive"</span>);
<a name="l01662"></a>01662 m_map.<a class="code" href="classmrpt_1_1utils_1_1bimap.html#af21e6b0fcd4c2046adf6e6a9a9b3c997" title="Insert a new pair KEY<->VALUE in the bi-map.">insert</a>(<a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14acb25ab5d45035eb456de1c7250dbcf5a">bayes::kfEKFAlaDavison</a>, <span class="stringliteral">"kfEKFAlaDavison"</span>);
<a name="l01663"></a>01663 m_map.<a class="code" href="classmrpt_1_1utils_1_1bimap.html#af21e6b0fcd4c2046adf6e6a9a9b3c997" title="Insert a new pair KEY<->VALUE in the bi-map.">insert</a>(<a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14ac5aee4885e69b59edf793342e96acab3">bayes::kfIKFFull</a>, <span class="stringliteral">"kfIKFFull"</span>);
<a name="l01664"></a>01664 m_map.<a class="code" href="classmrpt_1_1utils_1_1bimap.html#af21e6b0fcd4c2046adf6e6a9a9b3c997" title="Insert a new pair KEY<->VALUE in the bi-map.">insert</a>(<a class="code" href="group__mrpt__bayes__grp.html#gga93ff73c3a01619f01d97fd80dff40d14a0c3af1381c1d9b4b4ebb04370dc59ba9">bayes::kfIKF</a>, <span class="stringliteral">"kfIKF"</span>);
<a name="l01665"></a>01665 }
<a name="l01666"></a>01666 };
<a name="l01667"></a>01667 } <span class="comment">// End of namespace</span>
<a name="l01668"></a>01668
<a name="l01669"></a>01669 } <span class="comment">// end namespace</span>
<a name="l01670"></a>01670
<a name="l01671"></a>01671 <span class="preprocessor">#endif</span>
</pre></div></div>
</div>
<br><hr><br> <table border="0" width="100%"> <tr> <td> Page generated by <a href="http://www.doxygen.org" target="_blank">Doxygen 1.7.5</a> for MRPT 0.9.5 SVN: at Sun Sep 25 17:20:18 UTC 2011</td><td></td> <td width="100"> </td> <td width="150"> </td></tr> </table> </body></html>
