Skip to content

Dynamic number of landmarks #309

Description

@Fizpok1

I have UKF based localization that uses landmarks. At every particular moment robot can see
different number of landmarks. I tried to handle it by

self.arrSigmas = []
for nLandmarkIdx, lmark in enumerate(self.landmarks):
if(self.arrShowLandmarks[nLandmarkIdx] == True):
self.arrSigmas[j] = distance_adjusted_sigma_range**2

self.ukf.R = np.diag(self.arrSigmas)

so every time I recreate self.ukf.R.
But it looks like this is not enough. What else should I adjust for UKF to be able to handle dynamic number of landmarks?

I am getting

.../.local/lib/python3.8/site-packages/filterpy/kalman/unscented_transform.py", line 126, in unscented_transform
P += noise_cov
ValueError: operands could not be broadcast together with shapes (20,20) (2,2) (20,20)

in
def unscented_transform(sigmas, Wm, Wc, noise_cov=None,
mean_fn=None, residual_fn=None): if residual_fn is np.subtract or residual_fn is None:
y = sigmas - x[np.newaxis, :]
P = np.dot(y.T, np.dot(np.diag(Wc), y))
else:
P = np.zeros((n, n))
for k in range(kmax):
y = residual_fn(sigmas[k], x)
P += Wc[k] * np.outer(y, y)

if noise_cov is not None:
P += noise_cov

return (x, P)

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

      Milestone

      No milestone

      Relationships

      None yet

      Development

      No branches or pull requests

      Issue actions