Optimization-based Robot Control and State Estimation on Matrix Lie Groups