fix getP bug

There was a bug in the ```getPivotMatrix``` function. I am sorry. I don't know how I missed it.