Fusion2D Class Reference

2D sensor fusion EKF for position, heading, and speed estimation. More...

#include <Fusion2D.h>

Inheritance diagram for Fusion2D:

Collaboration diagram for Fusion2D:

Classes
struct	Pose2D
	Simple 2D pose (x, y, heading). More...
struct	State2D
	Holds the estimated 2D state. More...

Public Member Functions
	Fusion2D ()
	Constructor. Resets the EKF state to zeros.
bool	begin (const Coordinate< DistanceM > &initialPosition={0, 0}, float initialHeadingDeg=0)
bool	begin (Transform2D initialTransform)
void	predict (uint32_t timeMs, float ax=0.0f, float ay=0.0f)
	Predict the next state based on elapsed time.
void	updateIMU (uint32_t timeMs, float yawRate, float ax=0.0f, float ay=0.0f)
	Update the EKF with IMU measurements.
void	updateIMU (const IMU2D< float > &imu)
void	updateSpeed (uint32_t timeMs, float speed)
	Update the EKF with wheel speed.
void	updateSpeed (uint32_t timeMs, Speed newSpeed)
void	updateHeading (uint32_t timeMs, float heading)
	Update the EKF with an absolute heading measurement.
void	updateHeading (uint32_t timeMs, Angle newHeading)
void	updateGPS (uint32_t timeMs, float x, float y, float accuracy)
	Update the EKF with GPS position.
void	updateGPS (uint32_t timeMs, const Coordinate< DistanceM > &gpsCoord, float accuracyM)
State2D	getState () const
	Get the full estimated state.
Pose2D	getPose () const
	Get 2D pose (position + heading).
float	getSpeedMPS () const
	Get the current speed estimate.
float	getGyroBias () const
	Get the current estimated gyro bias.
Coordinate< DistanceM >	getPosition () const
Angle	getHeading () const
Speed	getSpeed () const
Transform2D	getTransform () const
void	end ()
virtual Coordinate< DistanceM >	getPosition () const =0
virtual Angle	getHeading () const =0
virtual Speed	getSpeed () const =0
virtual Transform2D	getTransform () const =0
virtual bool	begin (Transform2D transform)=0
virtual void	end ()=0
Public Member Functions inherited from MessageSource
void	subscribe (MessageHandler &handler, MessageOrigin origin=MessageOrigin::Undefined, MessageContent content=MessageContent::Undefined)
	Subscribe a message handler to this source, with optional filtering.
void	unsubscribeAll ()
	Remove all registered message handlers.
void	sendMessage (Message< float > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< Coordinate< float > > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< GPSCoordinate > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< MotionState3D > &msg)
	Overload for MotionState3D messages.

Protected Member Functions
void	applyUpdate (float K[5], float residual)
void	updateCovariance (float K[5], int idx)
void	reset (uint32_t timeMs=millis())
	Reset the EKF state and covariance.

Protected Attributes
State2D	state
float	P [5][5] = {}
uint32_t	lastPredictTime = 0
float	omegaMeasured = 0.0f
bool	hasWheelSpeed = false
bool	hasIMU = false
Protected Attributes inherited from MessageSource
std::vector< MessageHandlerEntry >	messageHandlers_

Detailed Description

2D sensor fusion EKF for position, heading, and speed estimation.

Supports optional sensors:

• IMU (gyro + optional acceleration)
• Wheel speed
• Absolute heading (magnetometer or GPS-derived)
• GPS position

The EKF automatically adapts to the available sensors.

Member Function Documentation

begin()

bool begin ( Transform2D  initialTransform )

inlinevirtual

Implements IMotionState2D.

end()

void end ( )

inlinevirtual

Implements IMotionState2D.

getGyroBias()

float getGyroBias ( ) const

inline

Get the current estimated gyro bias.

Returns

Gyro bias in rad/s.

getHeading()

Angle getHeading ( ) const

inlinevirtual

Implements IMotionState2D.

getPose()

Pose2D getPose ( ) const

inline

Get 2D pose (position + heading).

Returns

Pose2D struct with x, y, heading.

getPosition()

Coordinate< DistanceM > getPosition ( ) const

inlinevirtual

Implements IMotionState2D.

getSpeed()

Speed getSpeed ( ) const

inlinevirtual

Implements IMotionState2D.

getSpeedMPS()

float getSpeedMPS ( ) const

inline

Get the current speed estimate.

Returns

Speed in meters per second.

getState()

State2D getState ( ) const

inline

Get the full estimated state.

Returns

Current State2D struct.

getTransform()

Transform2D getTransform ( ) const

inlinevirtual

Implements IMotionState2D.

predict()

void predict ( uint32_t  timeMs, float  ax = 0.0f, float  ay = 0.0f  )

inline

Predict the next state based on elapsed time.

This method performs dead-reckoning using the last speed and heading. If no wheel speed is available but IMU acceleration is provided, it will integrate ax/ay to estimate speed.

Parameters

timeMs	Current timestamp in milliseconds.
ax	Optional IMU acceleration along body-forward axis (m/s²). Used only if no wheel speed.
ay	Optional IMU acceleration along body-sideways axis (m/s²). Used only if no wheel speed.

reset()

void reset ( uint32_t  timeMs = millis() )

inlineprotected

Reset the EKF state and covariance.

Parameters

timeMs

Optional timestamp to set the initial state time.

updateGPS()

void updateGPS ( uint32_t  timeMs, float  x, float  y, float  accuracy  )

inline

Update the EKF with GPS position.

Parameters

timeMs	Timestamp of the GPS measurement.
x	GPS X position in meters (or local coordinates).
y	GPS Y position in meters (or local coordinates).
accuracy	Standard deviation of the GPS measurement (meters).

Corrects the EKF position estimate. Works independently of IMU or wheel speed.

updateHeading()

void updateHeading ( uint32_t  timeMs, float  heading  )

inline

Update the EKF with an absolute heading measurement.

Parameters

timeMs	Timestamp of the heading measurement.
heading	Absolute heading in radians (e.g., from magnetometer or GPS-derived).

This corrects the heading estimate and helps reduce gyro drift.

updateIMU()

void updateIMU ( uint32_t  timeMs, float  yawRate, float  ax = 0.0f, float  ay = 0.0f  )

inline

Update the EKF with IMU measurements.

Parameters

timeMs	Timestamp of the IMU measurement.
yawRate	Gyro z-axis measurement (yaw rate, rad/s).
ax	Optional IMU acceleration along body x-axis (m/s²).
ay	Optional IMU acceleration along body y-axis (m/s²).

The yawRate is used for heading prediction, and optional ax/ay are used to integrate speed when wheel encoder is missing.

updateSpeed()

void updateSpeed ( uint32_t  timeMs, float  speed  )

inline

Update the EKF with wheel speed.

Parameters

timeMs	Timestamp of the speed measurement.
speed	Wheel speed in meters per second.

Overrides IMU acceleration-based speed if available.

---

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

• src/TinyRobotics/fusion/Fusion2D.h