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 () const
	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 () const
	IMU count getter method. More...

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

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

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

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

unsigned int	GetCamCount () const
	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...

void	Initialize (double initial_time, const BodyState &body_state_init)
	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) const
	Augment the state covariance matrix. More...

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

void	AugmentStateIfNeeded (unsigned int camera_id, unsigned 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 () const
	Getter for the LLA reference position. More...

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

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

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

void	InitializeGravity ()
	Function to initialize gravity angle.

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

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

void	RefreshIndices ()
	Refresh the sub-state indices.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Static Public Member Functions
static Eigen::MatrixXd	GetStateTransition (double delta_time)
	State transition matrix getter method. 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,
		const 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
	)		const

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,
		unsigned 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,
		unsigned 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 ( ) const

Get augmented state size.

Returns: Augmented state size

◆ GetAugStateStart()

unsigned int EKF::GetAugStateStart ( ) const

Augmented states start getter.

Returns: Augmented states start

◆ GetCamCount()

unsigned int EKF::GetCamCount ( ) const

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 ( ) const

Camera state size getter method.

Returns: Camera state size

◆ GetCamStateStart()

unsigned int EKF::GetCamStateStart ( ) const

Camera state start getter.

Returns: Camera state start

◆ GetCurrentTime()

double EKF::GetCurrentTime ( ) const

Current time getter.

Returns: Current time

◆ GetFidStateStart()

unsigned int EKF::GetFidStateStart ( ) const

Fiducial state start getter.

Returns: Fiducial state start

◆ GetGpsCount()

unsigned int EKF::GetGpsCount ( ) const

IMU count getter method.

Returns: IMU count

◆ GetGpsEcefVector()

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

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 ( ) const

GPS state size getter method.

Returns: GPS state size

◆ GetGpsStateStart()

unsigned int EKF::GetGpsStateStart ( ) const

GPS state start getter.

Returns: GPS state start

◆ GetGpsTimeVector()

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

GPS time vector getter.

Returns: GPS time vector

◆ GetGpsXyzVector()

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

GPS XYZ vector getter.

Returns: GPS XYZ vector

◆ GetImuCount()

unsigned int EKF::GetImuCount ( ) const

IMU count getter method.

Returns: IMU count

◆ GetImuNoiseScaleFactor()

double EKF::GetImuNoiseScaleFactor ( ) const

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 ( ) const

IMU state size getter method.

Returns: IMU state size

◆ GetImuStateStart()

unsigned int EKF::GetImuStateStart ( ) const

IMU state start getter.

Returns: IMU state start

◆ GetMotionDetectionChiSquared()

double EKF::GetMotionDetectionChiSquared ( ) const

Motion detection Chi squared threshold getter.

Returns: Motion detection Chi squared threshold

◆ GetReferenceAngle()

double EKF::GetReferenceAngle ( ) const

Getter for the LLA reference frame heading.

Returns: Reference frame heading

◆ GetReferenceLLA()

Eigen::VectorXd EKF::GetReferenceLLA ( ) const

Getter for the LLA reference position.

Returns: Reference LLA position

◆ GetStateSize()

unsigned int EKF::GetStateSize ( ) const

Getter for state size.

Returns: State size

◆ GetStateTransition()

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

static

State transition matrix getter method.

Parameters

delta_time State transition time

Returns: State transition matrix

◆ GetUseFirstEstimateJacobian()

bool EKF::GetUseFirstEstimateJacobian ( ) const

Getter for use first estimate Jacobians.

Returns: Use first estimate Jacobians

◆ GetUseRootCovariance()

bool EKF::GetUseRootCovariance ( ) const

Getter for use root covariance flag.

Returns: Use root covariance flag

◆ Initialize()

void EKF::Initialize	(	double	initial_time,
		const BodyState &	body_state_init
	)

EKF state initialization method.

Parameters

initial_time	Initial time
body_state_init	Initial state

◆ IsGravityInitialized()

bool EKF::IsGravityInitialized ( ) const

Checks if the gravity angle has been initialized.

Returns: Gravity initialization boolean

◆ IsLlaInitialized()

bool EKF::IsLlaInitialized ( ) const

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

Static 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()