2 Added Example edited Sep 28 '15 at 20:55 Josh Vander Hook 4,7941414 silver badges3737 bronze badges This is the basic slam problem. You have to find out (model) how the uncertainty of the robot affects the uncertainty of the landmarks, and visa-versa. This is done using the cross-correlation terms of the uncertainty. Or, the covariance. Those four things you referred to as covariances are actually variances. The covariance describes how the undertaintiesuncertainties in one thing bleed over to create uncertainty in the other things. This sounds terrible, but actually, if you reduce uncertainty in one thing, then the fact that two things co-vary allows you to reduce uncertainty in two things. What you have to do for $$N$$ lamps in $$2D$$ space and 1 robot moving in $$2D$$, is construct the giant $$2\times (N+1)$$ by $$2\times (N+1)$$ matrix. How to do this is mechanically straightforward, but an intuitive understanding is difficult to build up. I'm sure someone will come along that has a better mechanical answer for you. But until then, you might check out Probabilistic Robotics, by Thrun.UPDATE: For example. Let's say you have one landmark, and one robot. Both are on the line, and so the state is simply their $$x$$ position. (This greatly simplifies the problem). Then, let's say the robot measures the distance (with direction) to the landmark. The the measurement function (which describes what the robot measures) is $$h=x_l - x_r$$. We'll use the EKF update equations to find the covariance of the robot-landmark state. Note, this will be a $$2\times 2$$ matrix, given the discussion above. We're not really limited to the EKF update equations. Most estimators use the same equation to estimate the covariance of the state. If you really want to know more about this, read up on the Fisher Information Matrix, which is the gold standard for covariance estimation. Back to the topic at hand. The full state covariance matrix after the robot moves with some uncertain actuation is given by:$$P = F^T P F + G$$ and after the measurement, is given by$$P = \left( P^{-1} + H^T R^{-1} H \right)^{-1}$$ Here, $$R$$ is the measurement noise, $$H$$ is the jacobian of the measurement function with respect to the state, $$F$$ is the jacobian of the update function (the kinematics) with respect to the state, and $$G$$ is the noise matrix for the controls. In our case, $$H =\begin{bmatrix} \frac{\partial h}{x_r}, \frac{\partial h}{x_l} \end{bmatrix}\\ =\begin{bmatrix} -1, 1 \end{bmatrix}\\$$ Suppose the robot's position is known up to variance $$\sigma_r^2$$ and the measurement is corrupted by zero-mean noise with variance $$\sigma_s^2$$. Then, when the landmark is measured, (assuming a diagonal starting covariance)$$P^{-1} = \begin{bmatrix} \frac{1}{\sigma_r^2}, 0\\ 0, \frac{1}{\sigma_l^2} \end{bmatrix} + \begin{bmatrix} -1\\ 1 \end{bmatrix} \frac{1}{\sigma_s^2} \begin{bmatrix} -1, 1 \end{bmatrix}$$ Simplifying, $$P^{-1}= \begin{bmatrix} \frac{1}{\sigma_r^2}, 0\\ 0, \frac{1}{\sigma_l^2} \end{bmatrix}+ \frac{1}{\sigma_s^2} \begin{bmatrix} 1, -1\\-1,1 \end{bmatrix}$$ Implying, $$P= \begin{bmatrix} \frac{1}{\sigma_r^2} +\frac{1}{\sigma_s^2}& -\frac{1}{\sigma_s^2}\\ -\frac{1}{\sigma_s^2}& \frac{1}{\sigma_l^2}+\frac{1}{\sigma_s^2} \end{bmatrix}^{-1}$$ The previous defines a computational way to calculate the covariance after a measurement. A similar method using the control function to get the matrix $$F$$ will get you the update step. To get this in closed form, just carry the partial derivatives through. Or, a better way: In general, the matrix on the right is known as the Fisher Information, with the caveat that the matrix $$H$$ is not linearized at the true target location. To construct an arbitrary covariance matrix from scratch, you can do so by noting that the FIM has each i,j element given by (assuming gaussian noise), for state vector $$x$$.$$\begin{bmatrix} \frac{\partial h}{x(i)}\ln \mathcal{L}(x) \frac{\partial h}{x(j)}\ln \mathcal{L}(x) \end {bmatrix}$$ In the previous, $$\mathcal{L}(\cdot)$$ is the liklihood function of the measurements. In this case, a Gaussian. This derivation is covered in this thesis and also this one (with the full disclosure that the second one is mine). This is the basic slam problem. You have to find out (model) how the uncertainty of the robot affects the uncertainty of the landmarks, and visa-versa. This is done using the cross-correlation terms of the uncertainty. Or, the covariance. Those four things you referred to as covariances are actually variances. The covariance describes how the undertainties in one thing bleed over to create uncertainty in the other things. This sounds terrible, but actually, if you reduce uncertainty in one thing, then the fact that two things co-vary allows you to reduce uncertainty in two things. What you have to do for $$N$$ lamps in $$2D$$ space and 1 robot moving in $$2D$$, is construct the giant $$2\times (N+1)$$ by $$2\times (N+1)$$ matrix. How to do this is mechanically straightforward, but an intuitive understanding is difficult to build up. I'm sure someone will come along that has a better mechanical answer for you. But until then, you might check out Probabilistic Robotics, by Thrun. This is the basic slam problem. You have to find out (model) how the uncertainty of the robot affects the uncertainty of the landmarks, and visa-versa. This is done using the cross-correlation terms of the uncertainty. Or, the covariance. Those four things you referred to as covariances are actually variances. The covariance describes how the uncertainties in one thing bleed over to create uncertainty in the other things. This sounds terrible, but actually, if you reduce uncertainty in one thing, then the fact that two things co-vary allows you to reduce uncertainty in two things. What you have to do for $$N$$ lamps in $$2D$$ space and 1 robot moving in $$2D$$, is construct the giant $$2\times (N+1)$$ by $$2\times (N+1)$$ matrix. How to do this is mechanically straightforward, but an intuitive understanding is difficult to build up. I'm sure someone will come along that has a better mechanical answer for you. But until then, you might check out Probabilistic Robotics, by Thrun.UPDATE: For example. Let's say you have one landmark, and one robot. Both are on the line, and so the state is simply their $$x$$ position. (This greatly simplifies the problem). Then, let's say the robot measures the distance (with direction) to the landmark. The the measurement function (which describes what the robot measures) is $$h=x_l - x_r$$. We'll use the EKF update equations to find the covariance of the robot-landmark state. Note, this will be a $$2\times 2$$ matrix, given the discussion above. We're not really limited to the EKF update equations. Most estimators use the same equation to estimate the covariance of the state. If you really want to know more about this, read up on the Fisher Information Matrix, which is the gold standard for covariance estimation. Back to the topic at hand. The full state covariance matrix after the robot moves with some uncertain actuation is given by:$$P = F^T P F + G$$ and after the measurement, is given by$$P = \left( P^{-1} + H^T R^{-1} H \right)^{-1}$$ Here, $$R$$ is the measurement noise, $$H$$ is the jacobian of the measurement function with respect to the state, $$F$$ is the jacobian of the update function (the kinematics) with respect to the state, and $$G$$ is the noise matrix for the controls. In our case, $$H =\begin{bmatrix} \frac{\partial h}{x_r}, \frac{\partial h}{x_l} \end{bmatrix}\\ =\begin{bmatrix} -1, 1 \end{bmatrix}\\$$ Suppose the robot's position is known up to variance $$\sigma_r^2$$ and the measurement is corrupted by zero-mean noise with variance $$\sigma_s^2$$. Then, when the landmark is measured, (assuming a diagonal starting covariance)$$P^{-1} = \begin{bmatrix} \frac{1}{\sigma_r^2}, 0\\ 0, \frac{1}{\sigma_l^2} \end{bmatrix} + \begin{bmatrix} -1\\ 1 \end{bmatrix} \frac{1}{\sigma_s^2} \begin{bmatrix} -1, 1 \end{bmatrix}$$ Simplifying, $$P^{-1}= \begin{bmatrix} \frac{1}{\sigma_r^2}, 0\\ 0, \frac{1}{\sigma_l^2} \end{bmatrix}+ \frac{1}{\sigma_s^2} \begin{bmatrix} 1, -1\\-1,1 \end{bmatrix}$$ Implying, $$P= \begin{bmatrix} \frac{1}{\sigma_r^2} +\frac{1}{\sigma_s^2}& -\frac{1}{\sigma_s^2}\\ -\frac{1}{\sigma_s^2}& \frac{1}{\sigma_l^2}+\frac{1}{\sigma_s^2} \end{bmatrix}^{-1}$$ The previous defines a computational way to calculate the covariance after a measurement. A similar method using the control function to get the matrix $$F$$ will get you the update step. To get this in closed form, just carry the partial derivatives through. Or, a better way: In general, the matrix on the right is known as the Fisher Information, with the caveat that the matrix $$H$$ is not linearized at the true target location. To construct an arbitrary covariance matrix from scratch, you can do so by noting that the FIM has each i,j element given by (assuming gaussian noise), for state vector $$x$$.$$\begin{bmatrix} \frac{\partial h}{x(i)}\ln \mathcal{L}(x) \frac{\partial h}{x(j)}\ln \mathcal{L}(x) \end {bmatrix}$$ In the previous, $$\mathcal{L}(\cdot)$$ is the liklihood function of the measurements. In this case, a Gaussian. This derivation is covered in this thesis and also this one (with the full disclosure that the second one is mine). 1 answered Sep 25 '15 at 14:30 Josh Vander Hook 4,7941414 silver badges3737 bronze badges This is the basic slam problem. You have to find out (model) how the uncertainty of the robot affects the uncertainty of the landmarks, and visa-versa. This is done using the cross-correlation terms of the uncertainty. Or, the covariance. Those four things you referred to as covariances are actually variances. The covariance describes how the undertainties in one thing bleed over to create uncertainty in the other things. This sounds terrible, but actually, if you reduce uncertainty in one thing, then the fact that two things co-vary allows you to reduce uncertainty in two things. See, when you discuss covariance of the robot's position, what you mean is something like a 2x2 matrix. The matrix has multiple terms, two variances in the principal x-y directions, and the covariance of the x uncertainty and the y uncertainty. So you're already doing SLAM. Sorta. What you have to do for $$N$$ lamps in $$2D$$ space and 1 robot moving in $$2D$$, is construct the giant $$2\times (N+1)$$ by $$2\times (N+1)$$ matrix. How to do this is mechanically straightforward, but an intuitive understanding is difficult to build up. I'm sure someone will come along that has a better mechanical answer for you. But until then, you might check out Probabilistic Robotics, by Thrun.