Calibration EKF class. More...

#include <ekf.hpp>

Collaboration diagram for EKF:

Classes
struct	Parameters
	EKF class parameters. More...

Public Types
typedef struct EKF::Parameters	Parameters
	EKF class parameters.

Public Member Functions
	EKF (Parameters params)
	EKF class constructor. More...

unsigned int	GetStateSize ()
	Getter for state size. More...

ImuState	GetImuState (unsigned int imu_id)
	Get IMU sensor state. More...

GpsState	GetGpsState (unsigned int gps_id)
	Get GPS sensor state. More...

CamState	GetCamState (unsigned int cam_id)
	Get camera sensor state. More...

unsigned int	GetImuCount ()
	IMU count getter method. More...

unsigned int	GetImuStateSize ()
	IMU state size getter method. More...

unsigned int	GetGpsCount ()
	IMU count getter method. More...

unsigned int	GetGpsStateSize ()
	GPS state size getter method. More...

unsigned int	GetCamStateSize ()
	Camera state size getter method. More...

unsigned int	GetCamCount ()
	Camera count getter method. More...

void	LogBodyStateIfNeeded (int execution_count)
	Check if body data should be logged and do so if necessary. More...

void	PredictModel (double local_time)
	Predict state to given time using IMU measurements. More...

Eigen::MatrixXd	GetStateTransition (double dT)
	State transition matrix getter method. More...

void	Initialize (double timeInit, const BodyState &bodyStateInit)
	EKF state initialization method. More...

void	RegisterIMU (unsigned int imu_id, const ImuState &imu_state, const Eigen::MatrixXd &covariance)
	IMU Registration function. More...

void	RegisterGPS (unsigned int gps_id, const GpsState &gps_state, const Eigen::Matrix3d &covariance)
	GPS Registration function. More...

void	RegisterCamera (unsigned int cam_id, const CamState &cam_state, const Eigen::MatrixXd &covariance)
	Camera Registration function. More...

void	RegisterFiducial (const FidState &fid_state, const Eigen::MatrixXd &covariance)
	Fiducial Registration function. More...

Eigen::MatrixXd	AugmentCovariance (const Eigen::MatrixXd &in_cov, unsigned int index)
	Augment the state covariance matrix. More...

void	AugmentStateIfNeeded ()
	Check if state should be augmented using current state.

void	AugmentStateIfNeeded (unsigned int camera_id, int frame_id)
	Check if state should be augmented using camera frame. More...

void	SetMaxTrackLength (unsigned int max_track_length)
	Setter for maximum track length. More...

void	SetBodyProcessNoise (const Eigen::VectorXd &process_noise)
	EKF process noise setter. More...

void	SetGpsReference (const Eigen::VectorXd &pos_e_in_g, double ang_l_to_e)
	GPS reference position setter. More...

void	SetZeroAcceleration (bool is_zero_acceleration)
	Zero acceleration flag setter. More...

Eigen::VectorXd	GetReferenceLLA ()
	Getter for the LLA reference position. More...

double	GetReferenceAngle ()
	Getter for the LLA reference frame heading. More...

bool	IsLlaInitialized ()
	Checks if the LLA reference frame has been initialized. More...

bool	IsGravityInitialized ()
	Checks if the gravity angle has been initialized. More...

void	InitializeGravity ()
	Function to initialize gravity angle.

AugState	GetAugState (unsigned int camera_id, int frame_id, double time)
	Find or interpolate augmented state. More...

unsigned int	GetAugStateSize ()
	Get augmented state size. More...

void	RefreshIndices ()
	Refresh the sub-state indices.

void	AttemptGpsInitialization (double time, Eigen::Vector3d gps_lla)
	GPS LLA to ENU Initialization Routine. More...

std::vector< double >	GetGpsTimeVector ()
	GPS time vector getter. More...

std::vector< Eigen::Vector3d >	GetGpsEcefVector ()
	GPS ECEF vector getter. More...

std::vector< Eigen::Vector3d >	GetGpsXyzVector ()
	GPS XYZ vector getter. More...

double	GetCurrentTime ()
	Current time getter. More...

unsigned int	GetImuStateStart ()
	IMU state start getter. More...

unsigned int	GetGpsStateStart ()
	GPS state start getter. More...

unsigned int	GetCamStateStart ()
	Camera state start getter. More...

unsigned int	GetAugStateStart ()
	Augmented states start getter. More...

unsigned int	GetFidStateStart ()
	Fiducial state start getter. More...

double	GetMotionDetectionChiSquared ()
	Motion detection Chi squared threshold getter. More...

double	GetImuNoiseScaleFactor ()
	IMU noise scale factor getter. More...

bool	GetUseRootCovariance ()
	Getter for use root covariance flag. More...

bool	GetUseFirstEstimateJacobian ()
	Getter for use first estimate Jacobians. More...

double	CalculateLocalTime (double time)
	Calculate UTF time to local EKF time. More...

Public Attributes
State	m_state
	EKF state.

Eigen::MatrixXd	m_cov = Eigen::MatrixXd::Identity(g_body_state_size, g_body_state_size) * 1e-2
	EKF covariance.

Detailed Description

Calibration EKF class.

Todo:

Implement check for correlation coefficients to be between +/- 1

Add gravity initialization/check

Create generic function to update(r,H,R)

Constructor & Destructor Documentation

◆ EKF()

EKF::EKF ( Parameters params )

explicit

EKF class constructor.

Parameters

params EKF class parameters

Member Function Documentation

◆ AttemptGpsInitialization()

void EKF::AttemptGpsInitialization	(	double	time,
		Eigen::Vector3d	gps_lla
	)

GPS LLA to ENU Initialization Routine.

Parameters

time	GPS measured time
gps_lla	GPS measured lat-lon-alt

◆ AugmentCovariance()

Eigen::MatrixXd EKF::AugmentCovariance	(	const Eigen::MatrixXd &	in_cov,
		unsigned int	index
	)

Augment the state covariance matrix.

Parameters

in_cov	Original state covariance matrix
index	Augmented state start index

Returns: Augmented state covariance matrix

◆ AugmentStateIfNeeded()

void EKF::AugmentStateIfNeeded	(	unsigned int	camera_id,
		int	frame_id
	)

Check if state should be augmented using camera frame.

Parameters

camera_id	Current camera ID
frame_id	Current frame ID

◆ CalculateLocalTime()

double EKF::CalculateLocalTime ( double time )

Calculate UTF time to local EKF time.

Returns: EKF time

◆ GetAugState()

AugState EKF::GetAugState	(	unsigned int	camera_id,
		int	frame_id,
		double	time
	)

Find or interpolate augmented state.

Parameters

camera_id	Desired camera ID
frame_id	Desired frame ID
time	Frame time

Returns: Augmented state

Todo:: : Use higher order interpolation between states?

◆ GetAugStateSize()

unsigned int EKF::GetAugStateSize ( )

Get augmented state size.

Returns: Augmented state size

◆ GetAugStateStart()

unsigned int EKF::GetAugStateStart ( )

Augmented states start getter.

Returns: Augmented states start

◆ GetCamCount()

unsigned int EKF::GetCamCount ( )

Camera count getter method.

Returns: Camera count

◆ GetCamState()

CamState EKF::GetCamState ( unsigned int cam_id )

Get camera sensor state.

Parameters

cam_id Sensor ID

Returns: Camera state

◆ GetCamStateSize()

unsigned int EKF::GetCamStateSize ( )

Camera state size getter method.

Returns: Camera state size

◆ GetCamStateStart()

unsigned int EKF::GetCamStateStart ( )

Camera state start getter.

Returns: Camera state start

◆ GetCurrentTime()

double EKF::GetCurrentTime ( )

Current time getter.

Returns: Current time

◆ GetFidStateStart()

unsigned int EKF::GetFidStateStart ( )

Fiducial state start getter.

Returns: Fiducial state start

◆ GetGpsCount()

unsigned int EKF::GetGpsCount ( )

IMU count getter method.

Returns: IMU count

◆ GetGpsEcefVector()

std::vector< Eigen::Vector3d > EKF::GetGpsEcefVector ( )

GPS ECEF vector getter.

Returns: GPS ECEF vector

◆ GetGpsState()

GpsState EKF::GetGpsState ( unsigned int gps_id )

Get GPS sensor state.

Parameters

gps_id Sensor ID

Returns: GPS state

◆ GetGpsStateSize()

unsigned int EKF::GetGpsStateSize ( )

GPS state size getter method.

Returns: GPS state size

◆ GetGpsStateStart()

unsigned int EKF::GetGpsStateStart ( )

GPS state start getter.

Returns: GPS state start

◆ GetGpsTimeVector()

std::vector< double > EKF::GetGpsTimeVector ( )

GPS time vector getter.

Returns: GPS time vector

◆ GetGpsXyzVector()

std::vector< Eigen::Vector3d > EKF::GetGpsXyzVector ( )

GPS XYZ vector getter.

Returns: GPS XYZ vector

◆ GetImuCount()

unsigned int EKF::GetImuCount ( )

IMU count getter method.

Returns: IMU count

◆ GetImuNoiseScaleFactor()

double EKF::GetImuNoiseScaleFactor ( )

IMU noise scale factor getter.

Returns: IMU noise scale factor

◆ GetImuState()

ImuState EKF::GetImuState ( unsigned int imu_id )

Get IMU sensor state.

Parameters

imu_id Sensor ID

Returns: IMU state

◆ GetImuStateSize()

unsigned int EKF::GetImuStateSize ( )

IMU state size getter method.

Returns: IMU state size

◆ GetImuStateStart()

unsigned int EKF::GetImuStateStart ( )

IMU state start getter.

Returns: IMU state start

◆ GetMotionDetectionChiSquared()

double EKF::GetMotionDetectionChiSquared ( )

Motion detection Chi squared threshold getter.

Returns: Motion detection Chi squared threshold

◆ GetReferenceAngle()

double EKF::GetReferenceAngle ( )

Getter for the LLA reference frame heading.

Returns: Reference frame heading

◆ GetReferenceLLA()

Eigen::VectorXd EKF::GetReferenceLLA ( )

Getter for the LLA reference position.

Returns: Reference LLA position

◆ GetStateSize()

unsigned int EKF::GetStateSize ( )

Getter for state size.

Returns: State size

◆ GetStateTransition()

Eigen::MatrixXd EKF::GetStateTransition ( double dT )

State transition matrix getter method.

Parameters

dT	State transition time

Returns: State transition matrix

◆ GetUseFirstEstimateJacobian()

bool EKF::GetUseFirstEstimateJacobian ( )

Getter for use first estimate Jacobians.

Returns: Use first estimate Jacobians

◆ GetUseRootCovariance()

bool EKF::GetUseRootCovariance ( )

Getter for use root covariance flag.

Returns: Use root covariance flag

◆ Initialize()

void EKF::Initialize	(	double	timeInit,
		const BodyState &	bodyStateInit
	)

EKF state initialization method.

Parameters

timeInit	Initial time
bodyStateInit	Initial state

◆ IsGravityInitialized()

bool EKF::IsGravityInitialized ( )

Checks if the gravity angle has been initialized.

Returns: Gravity initialization boolean

◆ IsLlaInitialized()

bool EKF::IsLlaInitialized ( )

Checks if the LLA reference frame has been initialized.

Returns: LLA initialization boolean

◆ LogBodyStateIfNeeded()

void EKF::LogBodyStateIfNeeded ( int execution_count )

Check if body data should be logged and do so if necessary.

Parameters

execution_count

◆ PredictModel()

void EKF::PredictModel ( double local_time )

Predict state to given time using IMU measurements.

Parameters

local_time Measurement in EKF time

Todo:: : Use RK4 or other higher-order prediction step

◆ RegisterCamera()

void EKF::RegisterCamera	(	unsigned int	cam_id,
		const CamState &	cam_state,
		const Eigen::MatrixXd &	covariance
	)

Camera Registration function.

Parameters

cam_id	IMU ID
cam_state	Initial camera state
covariance	Initial camera covariance

◆ RegisterFiducial()

void EKF::RegisterFiducial	(	const FidState &	fid_state,
		const Eigen::MatrixXd &	covariance
	)

Fiducial Registration function.

Parameters

fid_state	Initial fiducial state
covariance	Initial fiducial covariance

◆ RegisterGPS()

void EKF::RegisterGPS	(	unsigned int	gps_id,
		const GpsState &	gps_state,
		const Eigen::Matrix3d &	covariance
	)

GPS Registration function.

Parameters

gps_id	GPS ID
gps_state	Initial GPS state
covariance	Initial GPS covariance

◆ RegisterIMU()

void EKF::RegisterIMU	(	unsigned int	imu_id,
		const ImuState &	imu_state,
		const Eigen::MatrixXd &	covariance
	)

IMU Registration function.

Parameters

imu_id	IMU ID
imu_state	Initial IMU state
covariance	Initial IMU covariance

◆ SetBodyProcessNoise()

void EKF::SetBodyProcessNoise ( const Eigen::VectorXd & process_noise )

EKF process noise setter.

Parameters

process_noise Diagonal terms of process noise

◆ SetGpsReference()

void EKF::SetGpsReference	(	const Eigen::VectorXd &	pos_e_in_g,
		double	ang_l_to_e
	)

GPS reference position setter.

Parameters

pos_e_in_g	GPS reference LLA
ang_l_to_e	GPS reference header

◆ SetMaxTrackLength()

void EKF::SetMaxTrackLength ( unsigned int max_track_length )

Setter for maximum track length.

Parameters

max_track_length maximum track length

◆ SetZeroAcceleration()

void EKF::SetZeroAcceleration ( bool is_zero_acceleration )

Zero acceleration flag setter.

Parameters

is_zero_acceleration Body has zero acceleration flag

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

src/ekf/ekf.hpp
src/ekf/ekf.cpp

Classes

Public Types

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ EKF()

Member Function Documentation

◆ AttemptGpsInitialization()

◆ AugmentCovariance()

◆ AugmentStateIfNeeded()

◆ CalculateLocalTime()

◆ GetAugState()

◆ GetAugStateSize()

◆ GetAugStateStart()

◆ GetCamCount()

◆ GetCamState()

◆ GetCamStateSize()

◆ GetCamStateStart()

◆ GetCurrentTime()

◆ GetFidStateStart()

◆ GetGpsCount()

◆ GetGpsEcefVector()

◆ GetGpsState()

◆ GetGpsStateSize()

◆ GetGpsStateStart()

◆ GetGpsTimeVector()

◆ GetGpsXyzVector()

◆ GetImuCount()

◆ GetImuNoiseScaleFactor()

◆ GetImuState()

◆ GetImuStateSize()

◆ GetImuStateStart()

◆ GetMotionDetectionChiSquared()

◆ GetReferenceAngle()

◆ GetReferenceLLA()

◆ GetStateSize()

◆ GetStateTransition()

◆ GetUseFirstEstimateJacobian()

◆ GetUseRootCovariance()

◆ Initialize()

◆ IsGravityInitialized()

◆ IsLlaInitialized()

◆ LogBodyStateIfNeeded()

◆ PredictModel()

◆ RegisterCamera()

◆ RegisterFiducial()

◆ RegisterGPS()

◆ RegisterIMU()

◆ SetBodyProcessNoise()

◆ SetGpsReference()

◆ SetMaxTrackLength()

◆ SetZeroAcceleration()