#include <ekf_slam.h>

Inheritance diagram for tuw::EKFSLAM:

Classes
struct	CorrData

Public Member Functions
void	cycle (std::vector< Pose2D > &yt, cv::Mat_< double > &C_Yt, const Command &ut, const MeasurementConstPtr &zt)

	EKFSLAM (const std::vector< double > beta)

void	setConfig (const void *config)

Public Member Functions inherited from tuw::SLAMTechnique
Type	getType () const

const std::string	getTypeName () const

void	reset ()

	SLAMTechnique (Type type)

const boost::posix_time::ptime &	time_last_update () const

Private Types
typedef std::shared_ptr< CorrData const >	CorrDataConstPtr

typedef std::shared_ptr< CorrData >	CorrDataPtr

enum	DataAssociationMode { ID = 0, NNSF_LOCAL = 1, NNSF_GLOBAL = 2 }

enum	UpdateMode { None = 0, Single = 1, Combined = 2 }

Private Member Functions
void	data_association (const MeasurementMarkerConstPtr &zt)

void	init ()

void	integration (const MeasurementMarkerConstPtr &zt)

void	measurement (const MeasurementMarkerConstPtr &zt, const CorrDataPtr &corr)

cv::Matx< double, 3, 3 >	measurement_noise (const MeasurementMarker::Marker zi)

void	NNSF_global (const MeasurementMarkerConstPtr &zt, const double gamma)

void	NNSF_local (const MeasurementMarkerConstPtr &zt, const double gamma)

void	prediction (const Command &ut)

void	update ()

void	update_combined ()

void	update_single ()

Private Attributes
const std::vector< double >	beta_
	parameters More...

std::vector< CorrDataPtr >	c_ij
	correspondences: f[k] = j <-> marker k corressponds to landmark j More...

std::vector< CorrDataPtr >	c_ji
	measurement to landmark correspondences More...

cv::Mat_< double >	C_X
	covariance matrix of y = (x m1 m2 ...) More...

cv::Mat_< double >	C_Y
	mean vector of x More...

tuw_marker_slam::EKFSLAMConfig	config_

std::map< int, size_t >	f_kj
	covariance matrix of x More...

cv::Mat_< double >	x
	mean vector of y = (x m1 m2 ...) More...

cv::Mat_< double >	y
	parameters for the implemented measurement noise model More...

std::vector< size_t >	z_known
	landmark to measurement correspondences (ignore j = 0 since landmark numbering starts with 1) More...

std::vector< size_t >	z_new
	measurements corresponding to known landmarks More...

Additional Inherited Members
Public Types inherited from tuw::SLAMTechnique
enum	Type { EKF = 0 }

Protected Member Functions inherited from tuw::SLAMTechnique
bool	updateTimestamp (const boost::posix_time::ptime &t)

Protected Attributes inherited from tuw::SLAMTechnique
boost::posix_time::time_duration	duration_last_update_
	time of the last processed measurment More...

bool	reset_

boost::posix_time::ptime	timestamp_last_update_
	on true the system should be reseted on the next loop More...

Static Protected Attributes inherited from tuw::SLAMTechnique
static std::map< Type, std::string >	TypeName_

Detailed Description

extended kalman filter slam

Definition at line 17 of file ekf_slam.h.

Member Typedef Documentation

typedef std::shared_ptr< CorrData const > tuw::EKFSLAM::CorrDataConstPtr

private

Definition at line 74 of file ekf_slam.h.

typedef std::shared_ptr< CorrData > tuw::EKFSLAM::CorrDataPtr

private

Definition at line 73 of file ekf_slam.h.

Member Enumeration Documentation

enum tuw::EKFSLAM::DataAssociationMode

private

Different supported data association modes

Enumerator
ID
NNSF_LOCAL
NNSF_GLOBAL

Definition at line 47 of file ekf_slam.h.

enum tuw::EKFSLAM::UpdateMode

private

Different supported update modes

Enumerator
None
Single
Combined

Definition at line 56 of file ekf_slam.h.

Constructor & Destructor Documentation

EKFSLAM::EKFSLAM ( const std::vector< double > beta )

Constructor

Parameters

beta	parameters for the implemented measurement noise model

Definition at line 7 of file ekf_slam.cpp.

Member Function Documentation

void EKFSLAM::cycle	(	std::vector< Pose2D > &	yt,
		cv::Mat_< double > &	C_Yt,
		const Command &	ut,
		const MeasurementConstPtr &	zt
	)

virtual

starts the SLAM cycle and predicts the robot and landmark poses at the timestamp encoded into the measurement

Parameters

yt	implicit return of the combined state yt = (xt, mt_1, ..., mt_n) with xt = (x, y, alpha), mt_i = (x_i, y_i, alpha_i)
C_Yt	implicit return of the combined covariance matrix of combined state
ut	current control command
zt	measurment with a timestamp

Implements tuw::SLAMTechnique.

Definition at line 29 of file ekf_slam.cpp.

void EKFSLAM::data_association ( const MeasurementMarkerConstPtr & zt )

private

data association to find matching between measurements and landmarks

Parameters

zt	measurements at time t

Definition at line 124 of file ekf_slam.cpp.

void EKFSLAM::init ( )

privatevirtual

initializes the filter

Implements tuw::SLAMTechnique.

Definition at line 12 of file ekf_slam.cpp.

void EKFSLAM::integration ( const MeasurementMarkerConstPtr & zt )

private

integrating new landmarks

Definition at line 530 of file ekf_slam.cpp.

void EKFSLAM::measurement	(	const MeasurementMarkerConstPtr &	zt,
		const CorrDataPtr &	corr
	)

private

compares observed and predicted measurement

Parameters

zt	measurements at time t
corr	contains the indices of the measurement and associated landmark, also used for implicit return

Definition at line 334 of file ekf_slam.cpp.

cv::Matx< double, 3, 3 > EKFSLAM::measurement_noise ( const MeasurementMarker::Marker zi )

private

implements the measurement noise model

Parameters

zi	single measured marker

Returns: covariance matrix describing the measurement noise

Definition at line 399 of file ekf_slam.cpp.

void EKFSLAM::NNSF_global	(	const MeasurementMarkerConstPtr &	zt,
		const double	gamma
	)

private

performs Nearest Neighbor Standard Filter (NNSF) by finding a minimum Mahalanobis distance assignment between measurements and unassociated landmarks

Parameters

zt	measurements at time t
gamma	gamma is a threshold such that 100*(1-alpha)% of true measurements are rejected (with alpha from the config)

Definition at line 259 of file ekf_slam.cpp.

void EKFSLAM::NNSF_local	(	const MeasurementMarkerConstPtr &	zt,
		const double	gamma
	)

private

performs Nearest Neighbor Standard Filter (NNSF) by associating each measurement to an up then unassociated landmark with minimum Mahalanobis distance

Note: does not avoid local optimums, i.e. to a single landmark multiple measurements could be assigned. In this case both associations are deprecated (conservative approach)

Parameters

zt	measurements at time t
gamma	gamma is a threshold such that 100*(1-alpha)% of true measurements are rejected (with alpha from the config)

Definition at line 181 of file ekf_slam.cpp.

void EKFSLAM::prediction ( const Command & ut )

private

predicting robots pose

Parameters

ut	command at time t

Definition at line 61 of file ekf_slam.cpp.

void EKFSLAM::setConfig ( const void * config )

virtual

virtual function to set the config parameters

Parameters

config of type tuw_marker_slam::EKFSLAMConfig*

Implements tuw::SLAMTechnique.

Definition at line 57 of file ekf_slam.cpp.

void EKFSLAM::update ( )

private

correcting robot and landmarks poses

Parameters

zt	measurements at time t

Definition at line 413 of file ekf_slam.cpp.

void EKFSLAM::update_combined ( )

private

corrects the robot and landmarks poses by using all observations combined in one step

Definition at line 482 of file ekf_slam.cpp.

void EKFSLAM::update_single ( )

private

iterates over each single observation and corrects the robot and landmarks poses

Definition at line 429 of file ekf_slam.cpp.

Member Data Documentation

const std::vector<double> tuw::EKFSLAM::beta_

private

parameters

Definition at line 156 of file ekf_slam.h.

std::vector<CorrDataPtr> tuw::EKFSLAM::c_ij

private

correspondences: f[k] = j <-> marker k corressponds to landmark j

Definition at line 165 of file ekf_slam.h.

std::vector<CorrDataPtr> tuw::EKFSLAM::c_ji

private

measurement to landmark correspondences

Definition at line 166 of file ekf_slam.h.

cv::Mat_<double> tuw::EKFSLAM::C_X

private

covariance matrix of y = (x m1 m2 ...)

Definition at line 161 of file ekf_slam.h.

cv::Mat_<double> tuw::EKFSLAM::C_Y

private

mean vector of x

Definition at line 160 of file ekf_slam.h.

tuw_marker_slam::EKFSLAMConfig tuw::EKFSLAM::config_

private

Definition at line 155 of file ekf_slam.h.

std::map<int, size_t> tuw::EKFSLAM::f_kj

private

covariance matrix of x

Definition at line 163 of file ekf_slam.h.

cv::Mat_<double> tuw::EKFSLAM::x

private

mean vector of y = (x m1 m2 ...)

Definition at line 159 of file ekf_slam.h.

cv::Mat_<double> tuw::EKFSLAM::y

private

parameters for the implemented measurement noise model

Definition at line 158 of file ekf_slam.h.

std::vector<size_t> tuw::EKFSLAM::z_known

private

landmark to measurement correspondences (ignore j = 0 since landmark numbering starts with 1)

Definition at line 168 of file ekf_slam.h.

std::vector<size_t> tuw::EKFSLAM::z_new

private

measurements corresponding to known landmarks

Definition at line 169 of file ekf_slam.h.

The documentation for this class was generated from the following files:

Classes

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation