I am trying to perform SLAM for cases where only one sensor measurement is available.

For example, suppose I want to track the position of a robot moving in a room with multiple known landmarks (2D scenario). The robot can currently measure both the distance to the landmark and the angle to the landmark via its sensors.

The state of the robot can be represented by its current heading angle and current $x$ and $y$ coordinates. The control input can be represented by the robot's velocity and rate of change of heading angle : $$ X= \begin{bmatrix} x\\ y\\ \psi\\ \end{bmatrix} \quad U=\begin{bmatrix} V\\ \dot{\psi}\\ \end{bmatrix} $$

The robot's state can be updated as follows: $$ \begin{bmatrix} x(i+1)\\ y(i+1)\\ \psi(i+1)\\ \end{bmatrix} =\begin{bmatrix} x(i)\\ y(i)\\ \psi(i)\end{bmatrix} + \begin{bmatrix} Vcos(\psi(i)+\dot{\psi}\Delta T)\Delta T\\ Vsin(\psi(i)+\dot{\psi}\Delta T)\Delta T\\ \dot{\psi}\Delta T\end{bmatrix} $$

Now, as indicated in SLAM for Dummies, the measurement model can be represented as follows: $$ \begin{bmatrix} range = r\\ bearing = b\\ \end{bmatrix} =\begin{bmatrix} \sqrt{(x_L-x)^2+(y_L-y)^2}\\ tan^{-1}(\frac{y_L-y}{x_L-x})\end{bmatrix} $$ Finally, the Jacobian matrix for the measurement model is as follows:

$$ H= \begin{bmatrix} \frac{\partial r}{\partial x_L}&&\frac{\partial r}{\partial y_L} \\ \frac{\partial b}{\partial x_L}&&\frac{\partial b}{\partial y_L} \\ \end{bmatrix} = \begin{bmatrix} \frac{x_L-x}{r}&&\frac{y_L-y}{r} \\ -\frac{y_L-y}{r^2}&&\frac{x_L-x}{r^2}\\ \end{bmatrix} $$

When only one sensor measurement such as bearing or range is only available, do my Jakobians simply become as follows? $$ H_{bearing}= \begin{bmatrix} \frac{y_L-y}{r^2} \\ \frac{x_L-x}{r^2}\\ \end{bmatrix} \quad H_{range}= \begin{bmatrix} \frac{y_L-y}{r} \\ \frac{x_L-x}{r}\\ \end{bmatrix} $$

If so, how do I calculate the covariance and gain for single-measurement cases?

My innovation covariance is $S =HPH^{T}+ R$, where $P$ is my state covariance and $R$ is my measurement noise covariance. Kalman gain is then $K = PH^{T}S^{-1}$. With the current two measurement setup, $S,H, P, R$ are $2x2$ matrices.

When using one measurement, $H$ is $2x1$, $H^{T}$ is $1x2$, $P$ is $1x1$, $R$ is $1x1$ and $S$ therefore remains as $2x2$. However I am running into the issue where $S$ often becomes an invertible matrix, so I cannot calculate $K$ to predict the new landmark position and update the covariance. I am not sure why this is the case and I do not how to proceed.

Reference: https://dspace.mit.edu/bitstream/handle/1721.1/119149/16-412j-spring-2005/contents/projects/1aslam_blas_repo.pdf

  • 1
    $\begingroup$ I think that you H_bearing and H_range matricies look correct. One thing I am suspicious of is that your heading has no impact on the bearing measurement? Or is this due to your simplification of the problem? $\endgroup$
    – mjcarroll
    Commented Feb 17, 2022 at 1:21
  • $\begingroup$ @mjcarroll yes so it is assumed that irrespective of the robot's heading, it is able to measure bearing and distance to a landmark, with the only limitation being how close it is to the landmark. $\endgroup$ Commented Feb 17, 2022 at 8:41


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.