[Kalman] Rewrote the prediction and most of the correction steps

.vscode/settings.json

CMakeLists.txt

src/shared/algorithms/ExtendedKalman/ExtendedKalman.cpp

src/shared/algorithms/ExtendedKalman/ExtendedKalman.h

src/shared/algorithms/ExtendedKalman/ExtendedKalmanConfig.h

src/shared/algorithms/ExtendedKalman/ExtendedKalmanState.h

src/shared/algorithms/ExtendedKalman/StateInitializer.h

src/tests/algorithms/ExtendedKalman/test-ekf-with-logs.cpp

src/tests/algorithms/ExtendedKalman/test-ekf-with-triad.cpp

src/tests/algorithms/ExtendedKalman/test-ekf.cpp

src/tests/algorithms/ExtendedKalman/test-triad.cpp

src/tests/algorithms/Kalman/test-kalman-data.h

src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp

src/tests/catch/test-kalman-eigen.cpp