Observability-based local path planning and obstacle avoidance using bearing-only measurements

• We analyze observability of state estimation with bearing-only measurements.
• We use the observability analysis to explicitly design a path planning algorithm.
• The algorithm minimizes uncertainties of TTC estimate while avoiding obstacles.

In this paper we present an observability-based local path planning and obstacle avoidance technique that utilizes an extended Kalman Filter (EKF) to estimate the time-to-collision (TTC) and bearing to obstacles using bearing-only measurements. To ensure that the error covariance matrix computed by an EKF is bounded, the system should be observable. We perform a nonlinear observability analysis to obtain the necessary conditions for complete observability of the system. These conditions are used to explicitly design a path planning algorithm that enhances observability while simultaneously avoiding collisions with obstacles. We analyze the behavior of the path planning algorithm and specially define the environments where the path planning algorithm will guarantee collision-free paths that lead to a goal configuration. Numerical results show the effectiveness of the planning algorithm in solving single and multiple obstacle avoidance problems while improving the estimation accuracy.