I am trying to perform uncertainty aware planning, where my planner tries to connect start and goal in such a way that the resultant path provides the least covariance at the end. This is inspired by techniques such as LQG.
The way I 'estimate' what covariance would result from a certain path is by using the EKF equations while assuming maximum likelihood observations, and I am trying to test what's called the 'light-dark' scenario that was used in many papers: where a 2D robot is traversing the environment, and there's a specific region where it would receive measurements that would reduce the covariance greatly. Hence, the uncertainty aware planner tries to take the robot to this 'light' area, receive good measurements, and then proceed to the goal. As seen in this picture from [1], the final covariance drops significantly by using this modified path, than the shortest path from start to goal (ignore the red line).
https://i.stack.imgur.com/dPYDC.jpg
I am trying to replicate similar behavior with my planner, which does result in covariance reduction compared to some other path that doesn't visit the good area, but the reduction isn't significant at all. In my sample environment, which is a 20x20 grid, the X coordinate of 17 represents the 'light' area, so I express the environmental noise as a matrix which is written as $\begin{bmatrix} x-17+0.01 && 0 \\ 0 && x-17+0.01 \end{bmatrix}$,
, hence would get a (0.01,0.01) matrix whenever I'm precisely at the x=17 column in the grid. Problem is, my result looks something like this, with the covariance ellipses plotted in red (the span of which I obtain from the Eigen values of the matrix).
Although the robot does visit the good area thanks to the planner, my covariance still increases rapidly once I leave: so I'm guessing I am making a mistake in my EKF equations. This is how I am 'simulating' the covariance at coordinates x2, when stepping from x1 to x2 with P1 being the covariance at x1 (adapted from equations in some open source code).
function P2 = predictCovariance(P1, x1, x2)
H = eye(2);
u = x2-x1;
G = [u(1) 0 ; 0 u(2)];
Q = eye(2);
R = eye(2);
M = [(x2(1)-17 + 0.01) 0 ; 0 (x2(1)-17 + 0.01)];
P = P1 + G*Q*G';
S = H*P*H' + M*R*M';
K = (P*H')/S;
P2 = (eye(2)-K*H)*P;
end
[1] Van Den Berg, Jur, Sachin Patil, and Ron Alterovitz. "Motion planning under uncertainty using iterative local optimization in belief space." The International Journal of Robotics Research 31.11 (2012): 1263-1278.