Python code I wrote while converting MatLab code to Python