Modify batch multiplication helpers to support non-batched Q, R, x_des, u_des
Modify batch multiplication helpers to support non-batched Q, R, x_des, u_des