Geometric modeling method and application Geometric method in nonlinear control Planning algorithm Multi-sensor fusion Geometric modeling method and application Geometric method in nonlinear control Planning algorithm Multi-sensor fusion test