ekf.hpp File Reference

6-state Extended Kalman Filter driving the fused pose output. More...

#include "LightLib/drive/mcl_config.hpp"
#include "LightLib/drive/odometry.hpp"

Go to the source code of this file.

Namespaces
namespace	light
	Public LightLib odometry / pose-estimation API.
namespace	light::ekf

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

Detailed Description

6-state Extended Kalman Filter driving the fused pose output.

State

[x, y, theta, vx, vy, omega] (inches, radians, per-second).

Frame

Same as odometry.hpp Pose — origin at field center, angles CCW from +y.

The arc integration in odometry.cpp::update() produces local-frame deltas (dLocalX, dLocalY, dTheta) each tick. predict() integrates those into the state mean and propagates covariance.

Measurements are applied independently via update*() calls; each has its own R (measurement variance). The Kalman gain automatically weights noisy measurements less. There is no manual if/else on which sensor "wins".

When LightCast converges and the EKF is uncertain, odometry.cpp calls reset() to snap the state to the LightCast best estimate.

Definition in file ekf.hpp.