Working on Federated Kalman Filter Simulation

Initial odometry pre-filter