Hybrid LQ-optimization using dynamic programming
applied optimal control : optimization , estimation , and control .