Very simple Kalman filter implementation in C++ using Eigen