Pushing after some decent words, NR to be checked again, numerical stability to be testing from matlab implementation