Can potentially speed up purely in R by doing the matrix algebra and manipulations more thoughtfully. Alternately, could implement in c.