Question about EKF covariance equations

Question

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.

Answer 1

There seem to be several problems with the EKF equations in the code snippet: which I've come across before as part of an LQG related code available online. I am by no means an authoritative reference on filtering equations, but this is out of my own experience working in belief space and the 'light-dark' scenario.

1. That's not how the $G$ matrix works in the prediction equation. $G$ is the process noise Jacobian. $G$ could literally just be an identity matrix, with the predicted covariance equation reading

$ \bar{P} = A*P_{prev}*A^T + Q $

where $Q$ is the process noise covariance matrix.

2. Exactly similar to point 1: the $M$ matrix does not represent what you've coded it to: it's the measurement noise Jacobian. The measurements are encoded in the $H$ matrix, which is the measurement Jacobian: hence, how far you are from your 'light' area needs to be represented in the $H$ matrix, not the noise matrices. Hence $M$ can be set to $I$, and $R$ can be any Gaussian noise you choose.

Once you fix these problems and obtain right values for $H$, $\bar{P}$ and $S$, rest of the equations should be fairly straightforward, and are correctly written in the code.