Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dynamic number of landmarks #309

Open
Fizpok1 opened this issue Dec 28, 2024 · 0 comments
Open

Dynamic number of landmarks #309

Fizpok1 opened this issue Dec 28, 2024 · 0 comments

Comments

@Fizpok1
Copy link

Fizpok1 commented Dec 28, 2024

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)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant