Here algorithm uses several small matrices having type ad size known at compile time (as well as in several other functions from this PR). Should not they be replaced with `Matx`?