Add calculation of LMI and a sample test