Include a running accumulation of the unscented filter
This should include the ability to lookup the delta transform and uncertainty between two poses at arbitrary times This is to replace the current naive approach of selecting the most recent odom/imu and assuming constant vel/angular vel between samples
// todo: include a buffer to accumulate the odometry and store so that the correction can be // done by looking up the difference between two points on the buffer //std::deque<std::pair<ros::Time, std::<ColVectorXd, MatrixXd>> accumulated_odometry;