You forgot to ask a question.
It is not clear how x, p, r and the matrix are connected. And what is w? I suggest to explain the problem with more details and a small example. You can post the code, you have created so far and explain the occurring problems.