Skip to content

Dynamic number of landmarks #309

@Fizpok1

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