Python code I wrote while converting some MatLab code to Python