Finish State Estimation Node & Testing

[Sharwin24]

The State Estimation Node provides a good outline for a state estimation pipeline using a kalman filter implementation with an IMU and wheel odometry. The feature requests outlined below aren't an exhaustive list, and more features/issues can be created later.

Feature Requests

• Simulated Wheel Odometry for testing
• Publish robot twist along with odometry from state estimation
• Use reference trajectory with simulated data for tuning kalman filter
  • Eventually use real robot to tune
• Create option for including a camera for VIO and including it in the filter if offered

Testing

Test the individual components of the classes and functions that compose the state estimator and kalman filter:

• RobotState, Covariance, SystemModel, MeasurementModel Structs
• KalmanFilter and StateEstimator Class