light::ekf Namespace Reference

Functions
void	init (const Pose &initial, MCLConfig cfg={})
	Initialize the filter.
void	reset (const Pose &pose)
	Snap the state mean to pose and reset covariance to a small value.
void	predict (float dLocalX, float dLocalY, float dTheta, float dt)
	Propagate mean + covariance one tick forward using local-frame arc deltas.
Pose	mean ()
Pose	velocity ()
float	covTrace ()
bool	diverged (float threshold_in_sq)
Measurement updates
Each update is independent. variance is in (state-units)².
void	updateHeadingIMU (float thetaRad, float variance)
void	updateGPS (float x, float y, float variance)
void	updateWheelPose (const Pose &measured, float variance)
Live tuning
Used by the on-brain tuner to push noise constants without reinitializing the filter.
MCLConfig	config ()
void	setConfig (const MCLConfig &cfg)

Function Documentation

config()

MCLConfig light::ekf::config

(

)

covTrace()

float light::ekf::covTrace

(

)

Returns

trace(P[0:2, 0:2]) — position uncertainty in in².

diverged()

bool light::ekf::diverged

(

float

threshold_in_sq

)

Returns

true if the position covariance trace exceeds threshold_in_sq.

init()

void light::ekf::init	(	const Pose &	initial,
		MCLConfig	cfg = {}
	)

Initialize the filter.

Parameters

initial	starting pose (mean)
cfg	process-noise / convergence thresholds

mean()

Pose light::ekf::mean

(

)

Returns

Current state mean as a Pose.

predict()

void light::ekf::predict	(	float	dLocalX,
		float	dLocalY,
		float	dTheta,
		float	dt
	)

Propagate mean + covariance one tick forward using local-frame arc deltas.

Parameters

dLocalX	forward delta this tick, inches
dLocalY	lateral delta this tick, inches
dTheta	heading delta this tick, radians
dt	seconds since last predict

reset()

void light::ekf::reset

(

const Pose &

pose

)

Snap the state mean to pose and reset covariance to a small value.

Used when LightCast strongly disagrees with the EKF.

setConfig()

void light::ekf::setConfig

(

const MCLConfig &

cfg

)

updateGPS()

void light::ekf::updateGPS	(	float	x,
		float	y,
		float	variance
	)

updateHeadingIMU()

void light::ekf::updateHeadingIMU	(	float	thetaRad,
		float	variance
	)

updateWheelPose()

void light::ekf::updateWheelPose	(	const Pose &	measured,
		float	variance
	)

velocity()

Pose light::ekf::velocity

(

)

Returns

Current velocity as a Pose-shaped triple (vx, vy, omega).