-
Notifications
You must be signed in to change notification settings - Fork 663
Description
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)