Calibration matrix calculation and mapping + unit tests