Odometry: Added Kalman Filtering option

corelib/include/rtabmap/core/Odometry.h

@@ -70,6 +70,9 @@ public:
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
 
+	void initKalmanFilter();
+	void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);
+
 private:
 	std::string _roiRatios;
 	int _minInliers;
@@ -80,7 +83,7 @@ private:
 	int _resetCountdown;
 	bool _force2D;
 	bool _holonomic;
-	bool _particleFiltering;
+	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
 	float _particleLambdaT;
@@ -91,6 +94,8 @@ private:
 	double _pnpReprojError;
 	int _pnpFlags;
 	bool _varianceFromInliersCount;
+	float _kalmanProcessNoise;
+	float _kalmanMeasurementNoise;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
@@ -98,6 +103,7 @@ private:
 	float distanceTravelled_;
 
 	std::vector<ParticleFilter *> filters_;
+	cv::KalmanFilter kalmanFilter_;
 
 protected:
 	Odometry(const rtabmap::ParametersMap & parameters);

corelib/include/rtabmap/core/Parameters.h

@@ -330,12 +330,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
 	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
 	RTABMAP_PARAM(Odom, ImageBufferSize,        unsigned int, 1, "Data buffer size (0 min inf).");
-	RTABMAP_PARAM(Odom, ParticleFiltering, 		bool, false,     "Particle filtering to smooth the odometry trajectory.");
+	RTABMAP_PARAM(Odom, FilteringStrategy, 		int, 0,         "0=No filtering 1=Kalman filtering 2=Particle filtering");
 	RTABMAP_PARAM(Odom, ParticleSize, 		    unsigned int, 400,  "Number of particles of the filter.");
 	RTABMAP_PARAM(Odom, ParticleNoiseT, 		float, 0.002,     "Noise (m) of translation components (x,y,z).");
 	RTABMAP_PARAM(Odom, ParticleLambdaT, 		float, 100,       "Lambda of translation components (x,y,z).");
 	RTABMAP_PARAM(Odom, ParticleNoiseR, 		float, 0.002,     "Noise (rad) of rotational components (roll,pitch,yaw).");
 	RTABMAP_PARAM(Odom, ParticleLambdaR, 		float, 100,       "Lambda of rotational components (roll,pitch,yaw).");
+	RTABMAP_PARAM(Odom, KalmanProcessNoise, 	float, 0.001,     "Process noise covariance value.");
+	RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,      "Process measurement covariance value.");
 
 	// Odometry Bag-of-words
 	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,   "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");

corelib/src/Odometry.cpp

@@ -44,7 +44,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
 		_force2D(Parameters::defaultVisForce2D()),
 		_holonomic(Parameters::defaultOdomHolonomic()),
-		_particleFiltering(Parameters::defaultOdomParticleFiltering()),
+		_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
 		_particleSize(Parameters::defaultOdomParticleSize()),
 		_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
 		_particleLambdaT(Parameters::defaultOdomParticleLambdaT()),
@@ -55,6 +55,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_pnpReprojError(Parameters::defaultVisPnPReprojError()),
 		_pnpFlags(Parameters::defaultVisPnPFlags()),
 		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
+		_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
+		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
 		previousTransform_(Transform::getIdentity()),
@@ -76,7 +78,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
 	UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
 	Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
-	Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
+	Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
 	Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
 	Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT);
@@ -86,8 +88,11 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	UASSERT(_particleLambdaT>0);
 	UASSERT(_particleNoiseR>0);
 	UASSERT(_particleLambdaR>0);
-	if(_particleFiltering)
+	Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
+	Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
+	if(_filteringStrategy == 2)
 	{
+		// Initialize the Particle filters
 		filters_.resize(6);
 		for(unsigned int i = 0; i<filters_.size(); ++i)
 		{
@@ -101,6 +106,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 			}
 		}
 	}
+	else if(_filteringStrategy == 1)
+	{
+		initKalmanFilter();
+	}
 }
 
 Odometry::~Odometry()
@@ -150,6 +159,25 @@ void Odometry::reset(const Transform & initialPose)
 			filters_[4]->init(pitch);
 			filters_[5]->init(yaw);
 		}
+
+		if(_filteringStrategy == 1)
+		{
+			if(_force2D)
+			{
+				kalmanFilter_.statePost.at<float>(0) = x;
+				kalmanFilter_.statePost.at<float>(1) = y;
+				kalmanFilter_.statePost.at<float>(6) = yaw;
+			}
+			else
+			{
+				kalmanFilter_.statePost.at<float>(0) = x;
+				kalmanFilter_.statePost.at<float>(1) = y;
+				kalmanFilter_.statePost.at<float>(2) = z;
+				kalmanFilter_.statePost.at<float>(9) = roll;
+				kalmanFilter_.statePost.at<float>(10) = pitch;
+				kalmanFilter_.statePost.at<float>(11) = yaw;
+			}
+		}
 	}
 	else
 	{
@@ -176,12 +204,13 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 	UTimer time;
 	Transform t = this->computeTransform(data, info);
 
+	double dt = data.stamp() - previousStamp_;
 	if(info)
 	{
 		info->timeEstimation = time.ticks();
 		info->lost = t.isNull();
 		info->stamp = data.stamp();
-		info->interval = data.stamp() - previousStamp_;
+		info->interval = dt;
 		info->transform = t;
 	}
 
@@ -192,13 +221,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 	{
 		_resetCurrentCount = _resetCountdown;
 
-		if(_force2D || !_holonomic || filters_.size())
+		if(_force2D || !_holonomic || filters_.size() || _filteringStrategy==1)
 		{
 			float x,y,z, roll,pitch,yaw;
 			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 
-			if(filters_.size())
+			if(_filteringStrategy == 1)
 			{
+				if(_pose.isIdentity())
+				{
+					// reset Kalman
+					initKalmanFilter();
+				}
+				else
+				{
+					// Kalman filtering
+					updateKalmanFilter(dt,x,y,z,roll,pitch,yaw);
+				}
+			}
+			else if(filters_.size())
+			{
+				// Particle filtering
 				UASSERT(filters_.size()==6);
 				if(_pose.isIdentity())
 				{
@@ -261,7 +304,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 							x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
 			t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
 
-			if(info && filters_.size())
+			if(info && _filteringStrategy > 0)
 			{
 				info->transformFiltered = t;
 			}
@@ -296,4 +339,165 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 	return Transform();
 }
 
+void Odometry::initKalmanFilter()
+{
+	UDEBUG("");
+	// See OpenCV tutorial: http://docs.opencv.org/master/dc/d2c/tutorial_real_time_pose.html
+	// See Kalman filter pose/orientation estimation theory: http://campar.in.tum.de/Chair/KalmanFilter
+
+	// initialize the Kalman filter
+	int nStates = 18;            // the number of states (x,y,z,x',y',z',x'',y'',z'',roll,pitch,yaw,roll',pitch',yaw',roll'',pitch'',yaw'')
+	int nMeasurements = 6;       // the number of measured states (x,y,z,roll,pitch,yaw)
+	if(_force2D)
+	{
+		nStates = 9;             // the number of states (x,y,x',y',x'',y'',yaw,yaw',yaw'')
+		nMeasurements = 3;       // the number of measured states (x,y,z,roll,pitch,yaw)
+	}
+	int nInputs = 0;             // the number of action control
+
+	kalmanFilter_.init(nStates, nMeasurements, nInputs);                 // init Kalman Filter
+	cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise));       // set process noise
+	cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise));   // set measurement noise
+	cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1));             // error covariance
+
+	if(_force2D)
+	{
+        /* MEASUREMENT MODEL */
+		//  [1 0 0 0 0 0 0 0 0]
+		//  [0 1 0 0 0 0 0 0 0]
+		//  [0 0 0 0 0 0 1 0 0]
+		kalmanFilter_.measurementMatrix.at<float>(0,0) = 1;  // x
+		kalmanFilter_.measurementMatrix.at<float>(1,1) = 1;  // y
+		kalmanFilter_.measurementMatrix.at<float>(2,6) = 1; // yaw
+	}
+	else
+	{
+	    /* MEASUREMENT MODEL */
+		//  [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+		//  [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+		//  [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+		//  [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
+		//  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
+		//  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
+		kalmanFilter_.measurementMatrix.at<float>(0,0) = 1;  // x
+		kalmanFilter_.measurementMatrix.at<float>(1,1) = 1;  // y
+		kalmanFilter_.measurementMatrix.at<float>(2,2) = 1;  // z
+		kalmanFilter_.measurementMatrix.at<float>(3,9) = 1;  // roll
+		kalmanFilter_.measurementMatrix.at<float>(4,10) = 1; // pitch
+		kalmanFilter_.measurementMatrix.at<float>(5,11) = 1; // yaw
+	}
+}
+
+void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw)
+{
+	// Set transition matrix with current dt
+	if(_force2D)
+	{
+		// 2D:
+		//  [1 0 dt  0 dt2    0   0    0     0] x
+		//  [0 1  0 dt   0  dt2   0    0     0] y
+		//  [0 0  1  0   dt   0   0    0     0] x'
+		//  [0 0  0  1   0   dt   0    0     0] y'
+		//  [0 0  0  0   1    0   0    0     0] x''
+		//  [0 0  0  0   0    0   0    0     0] y''
+		//  [0 0  0  0   0    0   1   dt   dt2] yaw
+		//  [0 0  0  0   0    0   0    1    dt] yaw'
+		//  [0 0  0  0   0    0   0    0     1] yaw''
+		kalmanFilter_.transitionMatrix.at<float>(0,2) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(1,3) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(2,4) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(3,5) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(0,4) = 0.5*pow(dt,2);
+		kalmanFilter_.transitionMatrix.at<float>(1,5) = 0.5*pow(dt,2);
+		// orientation
+		kalmanFilter_.transitionMatrix.at<float>(6,7) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(7,8) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(6,8) = 0.5*pow(dt,2);
+	}
+	else
+	{
+		//  [1 0 0 dt  0  0 dt2   0   0 0 0 0  0  0  0   0   0   0] x
+		//  [0 1 0  0 dt  0   0 dt2   0 0 0 0  0  0  0   0   0   0] y
+		//  [0 0 1  0  0 dt   0   0 dt2 0 0 0  0  0  0   0   0   0] z
+		//  [0 0 0  1  0  0  dt   0   0 0 0 0  0  0  0   0   0   0] x'
+		//  [0 0 0  0  1  0   0  dt   0 0 0 0  0  0  0   0   0   0] y'
+		//  [0 0 0  0  0  1   0   0  dt 0 0 0  0  0  0   0   0   0] z'
+		//  [0 0 0  0  0  0   1   0   0 0 0 0  0  0  0   0   0   0] x''
+		//  [0 0 0  0  0  0   0   1   0 0 0 0  0  0  0   0   0   0] y''
+		//  [0 0 0  0  0  0   0   0   1 0 0 0  0  0  0   0   0   0] z''
+		//  [0 0 0  0  0  0   0   0   0 1 0 0 dt  0  0 dt2   0   0]
+		//  [0 0 0  0  0  0   0   0   0 0 1 0  0 dt  0   0 dt2   0]
+		//  [0 0 0  0  0  0   0   0   0 0 0 1  0  0 dt   0   0 dt2]
+		//  [0 0 0  0  0  0   0   0   0 0 0 0  1  0  0  dt   0   0]
+		//  [0 0 0  0  0  0   0   0   0 0 0 0  0  1  0   0  dt   0]
+		//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  1   0   0  dt]
+		//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   1   0   0]
+		//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   1   0]
+		//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   0   1]
+		// position
+		kalmanFilter_.transitionMatrix.at<float>(0,3) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(1,4) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(2,5) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(3,6) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(4,7) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(5,8) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(0,6) = 0.5*pow(dt,2);
+		kalmanFilter_.transitionMatrix.at<float>(1,7) = 0.5*pow(dt,2);
+		kalmanFilter_.transitionMatrix.at<float>(2,8) = 0.5*pow(dt,2);
+		// orientation
+		kalmanFilter_.transitionMatrix.at<float>(9,12) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(10,13) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(11,14) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(12,15) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(13,16) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(14,17) = dt;
+		kalmanFilter_.transitionMatrix.at<float>(9,15) = 0.5*pow(dt,2);
+		kalmanFilter_.transitionMatrix.at<float>(10,16) = 0.5*pow(dt,2);
+		kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
+	}
+
+	// Set measurement to predict
+	cv::Mat measurements;
+	if(!_force2D)
+	{
+		measurements = cv::Mat(6,1,CV_32FC1);
+		measurements.at<float>(0) = x;     // x
+		measurements.at<float>(1) = y;     // y
+		measurements.at<float>(2) = z;     // z
+		measurements.at<float>(3) = roll;  // roll
+		measurements.at<float>(4) = pitch; // pitch
+		measurements.at<float>(5) = yaw;   // yaw
+	}
+	else
+	{
+		measurements = cv::Mat(3,1,CV_32FC1);
+		measurements.at<float>(0) = x;     // x
+		measurements.at<float>(1) = y;     // y
+		measurements.at<float>(5) = yaw;   // yaw
+	}
+
+	// First predict, to update the internal statePre variable
+	UDEBUG("Predict");
+	cv::Mat prediction = kalmanFilter_.predict();
+	// The "correct" phase that is going to use the predicted value and our measurement
+	UDEBUG("Correct");
+	cv::Mat estimated = kalmanFilter_.correct(measurements);
+
+	if(_force2D)
+	{
+		x = estimated.at<float>(0);
+		y = estimated.at<float>(1);
+		yaw = estimated.at<float>(6);
+	}
+	else
+	{
+		x = estimated.at<float>(0);
+		y = estimated.at<float>(1);
+		z = estimated.at<float>(2);
+		roll = estimated.at<float>(9);
+		pitch = estimated.at<float>(10);
+		yaw = estimated.at<float>(11);
+	}
+}
+
 } /* namespace rtabmap */

corelib/src/OdometryOpticalFlow.cpp

@@ -204,7 +204,7 @@ Transform OdometryOpticalFlow::computeTransform(
 		std::vector<unsigned char> status;
 		std::vector<float> err;
 		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-		int winSize = (newCorners.size()||!flowGuessFromMotion_)?flowWinSize_:(flowWinSize_*2);
+		int winSize = flowWinSize_;
 		cv::calcOpticalFlowPyrLK(
 				refFrame_,
 				newLeftFrame,
@@ -213,7 +213,7 @@ Transform OdometryOpticalFlow::computeTransform(
 				status,
 				err,
 				cv::Size(winSize, winSize),
-				(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:flowMaxLevel_*2,
+				(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:3,
 				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
 				cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
 		UDEBUG("cv::calcOpticalFlowPyrLK() end");

corelib/src/Parameters.cpp

@@ -145,6 +145,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 
 		removedParameters_.insert(std::make_pair("RGBD/PoseScanMatching",         std::make_pair(true,  Parameters::kRGBDNeighborLinkRefining())));
 
+		removedParameters_.insert(std::make_pair("Odom/ParticleFiltering",        std::make_pair(false, Parameters::kOdomFilteringStrategy())));
 		removedParameters_.insert(std::make_pair("Odom/FeatureType",              std::make_pair(true,  Parameters::kVisFeatureType())));
 		removedParameters_.insert(std::make_pair("Odom/EstimationType",           std::make_pair(true,  Parameters::kVisEstimationType())));
 		removedParameters_.insert(std::make_pair("Odom/MaxFeatures",              std::make_pair(true,  Parameters::kVisMaxFeatures())));

guilib/src/PreferencesDialog.cpp

@@ -687,13 +687,17 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_maxVariance->setObjectName(Parameters::kOdomMonoMaxVariance().c_str());
 
 	//Odometry particle filter
-	_ui->odom_particleFiltering->setObjectName(Parameters::kOdomParticleFiltering().c_str());
+	_ui->odom_filteringStrategy->setObjectName(Parameters::kOdomFilteringStrategy().c_str());
 	_ui->spinBox_particleSize->setObjectName(Parameters::kOdomParticleSize().c_str());
 	_ui->doubleSpinBox_particleNoiseT->setObjectName(Parameters::kOdomParticleNoiseT().c_str());
 	_ui->doubleSpinBox_particleLambdaT->setObjectName(Parameters::kOdomParticleLambdaT().c_str());
 	_ui->doubleSpinBox_particleNoiseR->setObjectName(Parameters::kOdomParticleNoiseR().c_str());
 	_ui->doubleSpinBox_particleLambdaR->setObjectName(Parameters::kOdomParticleLambdaR().c_str());
 
+	//Odometry Kalman filter
+	_ui->doubleSpinBox_kalmanProcessNoise->setObjectName(Parameters::kOdomKalmanProcessNoise().c_str());
+	_ui->doubleSpinBox_kalmanMeasurementNoise->setObjectName(Parameters::kOdomKalmanMeasurementNoise().c_str());
+
 	//Stereo
 	_ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str());
 	_ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>8</number>
+                <number>17</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -6575,7 +6575,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_233">
                         <property name="text">
-                         <string>Particle filtering to smooth the odometry trajectory. See &quot;Particle Filter&quot; panel for the related parameters.</string>
+                         <string>Pose estimation filtering strategy.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6592,13 +6592,6 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
-                       <widget class="QCheckBox" name="odom_particleFiltering">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="1">
                        <widget class="QLabel" name="label_103">
                         <property name="text">
@@ -6714,6 +6707,28 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
+                      <item row="4" column="0">
+                       <widget class="QComboBox" name="odom_filteringStrategy">
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContents</enum>
+                        </property>
+                        <item>
+                         <property name="text">
+                          <string>No filtering</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>Kalman filtering</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>Particle filtering</string>
+                         </property>
+                        </item>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -7446,6 +7461,119 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                  </item>
                 </layout>
                </widget>
+               <widget class="QWidget" name="page_52">
+                <layout class="QVBoxLayout" name="verticalLayout_84">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_odometryKalmanFilter2">
+                   <property name="title">
+                    <string>Kalman Filter</string>
+                   </property>
+                   <layout class="QVBoxLayout" name="verticalLayout_171">
+                    <item>
+                     <widget class="QLabel" name="label_673">
+                      <property name="text">
+                       <string>Parameters for the Kalman filter when used to smooth the odometry trajectory.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item>
+                     <layout class="QGridLayout" name="gridLayout_168" columnstretch="0,1">
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_kalmanMeasurementNoise">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="decimals">
+                         <number>5</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_kalmanProcessNoise">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="decimals">
+                         <number>5</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.001000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_674">
+                        <property name="text">
+                         <string>Process noise.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_675">
+                        <property name="text">
+                         <string>Measurement noise.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                     </layout>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_43">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>1953</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
                <widget class="QWidget" name="page_46">
                 <layout class="QVBoxLayout" name="verticalLayout_81">
                  <item>