I see you follow Fredrik's orgiginal text and instead of a 5x7 and 1x7 you use a 5x7 and 7x1 matrix. So Fredrik can chose whatever he needs or transpose his matrix B first.
BTW, you may shorten your routine for two lines by omitting z and replacing it with r.
