Implementation of a Kalman Filter in C++