When defining M on line 178 (corresponding to matrix in equation (8) of paper), I changed the row index used on the A matrix (C matrix in paper) to match the paper