Control And Estimation Of Hybrid Systems With Mathematical Optimization