SE2 Models

See the Victoria Park example to see these classes in usage.

Odometry Process Model

class InEKF::OdometryProcess : public InEKF::ProcessModel<SE2<>, SE2<>>

Odometry process model with single column.

Public Functions

inline OdometryProcess()

Construct a new Odometry Process object.

inline OdometryProcess(float theta_cov, float x_cov, float y_cov)

Construct a new Odometry Process object and set corresponding covariances.

Parameters

  • theta_cov – Standard deviation of rotation between timesteps.

  • x_cov – Standard deviation of x between timesteps.

  • y_cov – Standard deviation of y between timesteps.

inline OdometryProcess(Eigen::Vector3d q)

Construct a new Odometry Process object. Set Q from vector.

Parameters

q – Vector that becomes diagonal of Q.

inline OdometryProcess(Eigen::Matrix3d q)

Construct a new Odometry Process object. Set Q from matrix.

Parameters

q – Matrix that is set as Q.

inline ~OdometryProcess()

Destroy the Odometry Process object.

virtual SE2 f(SE2<> u, double dt, SE2<> state) override

Overriden from base class. Propagates the model \(X_{t+1} = XU\).

Parameters

  • u – Control

  • dt – Delta time

  • state – Current state

Returns

Updated state estimate

virtual MatrixCov makePhi(const SE2<> &u, double dt, const SE2<> &state, ERROR error) override

Overriden from base class. If right, this is the identity. If left, it’s the adjoint of U.

Parameters

  • u – Control

  • dt – Delta time

  • state – Current state estimate (shouldn’t be needed unless doing an “Imperfect InEKF”)

  • error – Right or left error. Function should be implemented to handle both.

Returns

Phi

inline void setQ(Eigen::Vector3d q)

Set Q from vector.

Parameters

q – Vector that becomes diagonal of Q.

inline void setQ(Eigen::Matrix3d q)

Set Q from matrix.

Parameters

q – Matrix that is set as Q.

inline void setQ(double q)

Set Q from scalar.

Parameters

q – Scalar that becomes diagonal of Q

Dynamic Odometry Process Model

class InEKF::OdometryProcessDynamic : public InEKF::ProcessModel<SE2<Eigen::Dynamic>, SE2<>>

Odometry process model with variable number of columns, for use in SLAM on SE2.

Public Functions

inline OdometryProcessDynamic()

Construct a new Odometry Process Dynamic object.

inline OdometryProcessDynamic(float theta_cov, float x_cov, float y_cov)

Construct a new Odometry Process Dynamic object and set corresponding covariances.

Parameters

  • theta_cov – Standard deviation of rotation between timesteps.

  • x_cov – Standard deviation of x between timesteps.

  • y_cov – Standard deviation of y between timesteps.

inline OdometryProcessDynamic(Eigen::Vector3d q)

Construct a new Odometry Process Dynamic object. Set Q from vector.

Parameters

q – Vector that becomes diagonal of Q.

inline OdometryProcessDynamic(Eigen::Matrix3d q)

Construct a new Odometry Process Dynamic object. Set Q from matrix.

Parameters

q – Matrix that is set as Q.

inline ~OdometryProcessDynamic()

Destroy the Odometry Process Dynamic object.

virtual SE2<Eigen::Dynamic> f(SE2<> u, double dt, SE2<Eigen::Dynamic> state) override

Overriden from base class. Propagates the model \(X_{t+1} = XU\). Landmarks are left as is.

Parameters

  • u – Control

  • dt – Delta time

  • state – Current state

Returns

Updated state estimate

virtual MatrixCov makePhi(const SE2<> &u, double dt, const SE2<Eigen::Dynamic> &state, ERROR error) override

Overriden from base class. If right, this is the identity. If left, it’s the adjoint of U. Landmark elements are the identity in both versions of Phi.

Parameters

  • u – Control

  • dt – Delta time

  • state – Current state estimate (shouldn’t be needed unless doing an “Imperfect InEKF”)

  • error – Right or left error. Function should be implemented to handle both.

Returns

Phi

inline void setQ(Eigen::Vector3d q)

Set Q from vector.

Parameters

q – Vector that becomes diagonal of Q.

inline void setQ(Eigen::Matrix3d q)

Set Q from matrix.

Parameters

q – Matrix that is set as Q.

inline void setQ(double q)

Set Q from scalar.

Parameters

q – Scalar that becomes diagonal of Q

GPS

class InEKF::GPSSensor : public InEKF::MeasureModel<SE2<Eigen::Dynamic>>

GPS Sensor for use in SE2 SLAM model.

Public Functions

GPSSensor(double std = 1)

Construct a new GPSSensor object.

Parameters

std – The standard deviation of the measurement.

inline ~GPSSensor()

Destroy the GPSSensor object.

virtual VectorB processZ(const Eigen::VectorXd &z, const SE2<Eigen::Dynamic> &state) override

Overriden from the base class. Needed to fill out H/z with correct number of columns based on number of landmarks in state.

Parameters

  • z – Measurement

  • state – Current state estimate.

Returns

Processed measurement.

Landmark Sensor

class InEKF::LandmarkSensor : public InEKF::MeasureModel<SE2<Eigen::Dynamic>>

Landmark sensor used in SLAM on SE2.

Public Functions

LandmarkSensor(double std_r, double std_b)

Construct a new Landmark Sensor object.

Parameters

  • std_r – Range measurement standard deviation

  • std_b – Bearing measurement standard deviation

inline ~LandmarkSensor()

Destroy the Landmark Sensor object.

void sawLandmark(int idx, const SE2<Eigen::Dynamic> &state)

Sets H based on what landmark was recently seen.

Parameters

  • idx – Index of landmark recently seen.

  • state – Current state estimate. Used for # of landmarks.

double calcMahDist(const Eigen::VectorXd &z, const SE2<Eigen::Dynamic> &state)

Calculates Mahalanobis distance of having seen a certain landmark. Used for data association.

Parameters

  • z – Range and bearing measurement

  • state – Current state estimate

Returns

Mahalanobis distance

virtual VectorB processZ(const Eigen::VectorXd &z, const SE2<Eigen::Dynamic> &state) override

Overriden from base class. Converts r,b -> x,y coordinates and shifts measurement covariance. Then fills out z accordingly.

Parameters

  • z – Measurement

  • state – Current state estimate.

Returns

Processed measurement.

virtual MatrixS calcSInverse(const SE2<Eigen::Dynamic> &state) override

Overriden from base class. If using RInEKF, takes advantage of sparsity of H to shrink matrix multiplication. Otherwise, operates identically to base class.

Parameters

state – Current state estimate.

Returns

Inverse of measurement noise.

inline virtual MatrixH makeHError(const SE2<Eigen::Dynamic> &state, ERROR iekfERROR) override

Overriden from base class. Saves filter error for later use, then calls base class.

Parameters

  • state – Current state estimate.

  • iekfERROR – Type of filter error.

Returns

H_error_