tvlqr-related speedups

significantly improves efficiency of a couple functions used by tvlqr (for high-dimensional inputs)