Estimation aware Trajectory optimization

An implementation of Estimation-aware Trajectory planning algorithm where the output uncertianties are state dependent. We show that to improve estimation along a trajectory while performing some taks, alternative paths are needed. We present the framework in our paper A. Deole, M. Mesbahi. The estimation aware setup is applicable in general robotics applications if sensor performances have variability across state space. We present a scenario for Ego-Target Rendezvous problem where output uncertainty depends on the illumination. We present the following example:

A Demo of trajectory optimization showing Ego camera view. Here the Ego spacecraft starts tracking the Target and has to reach within 10m of the Target. The trajectory we design achieves this while getting to a relative position that imporves estimation. Note that we start at a position with bad sun angle and end up with best visibility under the given perception map.

The ego trajectory is shown in blue. The red line shows direction of sun-rays. The trajectory end point is where its relative position wrt Target(red dot) and sun-rays are parallel. The video shows trajectory generated as being simulated our platform.

The Target here is a uncooperative satellite with unknown rotation in Low-Earth orbit. The Ego satellite is parked in a relative elliptical orbit around the Target as a starting point.

obsvidagent.1.mp4

We need the Ego spacecraft to be parked at a fixed distance from the Target while saving fuel. The estimation aware trajectory desined here find a trajectory such that relative pose estimation between Target and Ego is optimized as well. We oserve that the final state achieved is such that the sun is directly behind the Ego spacecraft givng the best veiwing conditions.

The algorithm can be adapted for other robotics applications if there exists a convex enveloping function as defined here. If the output map is more complex then a globally convex function, then only local convexity is required as SCVx algorithm can be used to limit exploration within a trust region.

The implementation of the test cases shown in paper:

• navigate to dubincar for trajectory deviation example
• navigate to sat_rendezvous for augmented observability example

Upcoming updates on Multiagent version

The multiagent scenario can be interpreted as a dynamic sensor placement problem.

Here we show that non-homogenity of sensors is a consensus criterion to improve network observability.

obsmultiagent_videos-3.1.mp4