Fixed initialisation bug for SMS and added quadratic interpolation to peak estimate