Three-robot version of the VINS-based EKF

[shalabymhd]

This is quite similar to the previous PR, but is a bit more complicated as I now consider the 3-robot experiments, which means I have to add the tag-to-tag measurements. You will notice that there is a lot of overlap between the 1-robot EKF and the 3-robot EKF, but this is on purpose to reduce abstracting things and confusing users.

Also, I moved what previous was a StateHistory class to have two general MatrixStateHistory and VectorStateHistory classes, and each EKF now has its own implementation of the EvaluateEKF class to further reduce abstractions.

[shalabymhd]

if you have any suggestions on how to merge some of the methods of the two EKFs without increasing complexity/abstraction lmk. That would also benefit the IMU EKF