I'm really sorry if this is a dumb question, but I don't have a clue on how to do this. I'm trying to write a kalman filter with a State vector of :
To estimate the 2 dimensional position and velocity of a holonomic robot. Currently, I'm planning on fusing encoder odometry(50 hz, negligible latency), accelerometer data(50 hz, negligible latency), vision data (100 hz, 20 ms latency), and 3d vision data through solvepnp (20 hz, 20 ms latency). I'm hoping to get position and velocity data (+/- 1 meter, +/- 2 m/s respectively). How do I set up a filter for this using the apache commons math library?