3
$\begingroup$

Assume we have a robot that moves and observes landmarks on his path. The path is composed of $k$ poses $s_1 \: ... s_k$ where $s_i = [x,\ y,\ \theta]$. From the sensors, we read the motion measurements as a sequence of gaussian random variables $\mathcal{N}(\boldsymbol{\mu}^1_2, \boldsymbol{\Sigma}^1_2) \ ... \ \mathcal{N}(\boldsymbol{\mu}^{k-1}_k, \boldsymbol{\Sigma}^{k-1}_k)$, where $\boldsymbol{\mu}^{i-1}_i = [\Delta x, \ \Delta y, \ \Delta\theta]$ is the measurement of the motion from $s_{i-1}$ to $s_i$ and $\Sigma^{i-1}_{i}$ is the covariance of that measurement.

At pose $s_1$, the robot measures a landmark $l$, generating the measurement $\phi = \mathcal{N}(l^{(1)}, \Sigma_{l^{(1)}})$ where $l^{(1)} = [x, y]$ and represents the landmark position in the reference frame of $s_1$.

We now transform the landmark measurement $l^{(1)}$ into the reference frame of the last pose $s_k$ with the homogeneous transformation \begin{equation} l^{(k)} = M^{k}_1 \ \ l^{(1)} \end{equation} where $M^k_1$ is the transformation from $s_k$ to $s_1$ given by \begin{equation} M^k_1 = \prod_{i=1}^{k-1}\Big(M_{i+1}^{i}\Big)^{-1} \end{equation} and $M_{i+1}^i$ is the homogeneous matrix correspondent to the motion measurement $\boldsymbol{\mu}_i$.

I am interested in understanding what will the covariance of $l^{(k)}$ be. My intuition is that since components of $l^{(1)}$ do not appear in $M^1_k$, then $l^{k}$ is just a linear transformation of $l^{(1)}$ and the covariance $\Sigma_{l^{(k)}}$ will be \begin{equation} \Sigma_{l^{(k)}} = \Sigma^1_k \ \Sigma_{l^{(i)}} \ \Sigma^1_k \end{equation} where $\Sigma^1_k$ is the covariance associated with the transformation $M^1_k$, and I cannot directly compute it, but only approximate it with an EKF-like algorithm. Am I correct?

$\endgroup$
0
$\begingroup$

I believe you can directly estimate the covariance if you have a trajectory before and after a loop closure. You can think of the trajectory after the loop closure as ground truth and you will see how much it drifts on each pose on average from the uncorrected trajectory. That's where you can get the hint for how you calculate the uncertainty of your individual pose estimation.

One tricky thing is that the poses are on the manifold. You can calculate the covariance on R3 space but it gives better results when done on SE2.

Also, your last equation is a simplified linear model whereas it should be much more complicated if you have to do it accurately. One old passioned method is Sigma point and the other more recent method is doing it on the manifold.

See State estimation book by Prof. Tim Barfoot. All you want to know is there.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.