Modified Matrix.h, previous optimization was useless. Added a Win32 project to debug more easilly. Started implementing Kalman filter.