def fillNode(self, node, data):
"""Fill a node with a subdataset
This method is called recursively :
a node is filled when all its children nodes are filled.
Arguments:
node {Node} -- the node to fill
data {numpy.array} -- the subdataset to fill the node with
"""
# the dataset is reduced to a cell. It can't be bisected
if max(data.shape) == 1:
node.itv = pyibex.Interval(data[0, 0], data[0, 0])
# the dataset can still be bisected
else:
# the bisection is performed along the larger side
node.axis = np.argmax(data.shape)
node.left = Node()
node.right = Node()
# fill both children with a half of the dataset each
self.fillNodeChildren(node, np.array_split(data, 2,
axis=node.axis))
# compute the interval (ex: left=[4,6], right=[5,7] ==> node=[4,7])
node.itv = node.left.itv | node.right.itv
def getDistanceFromLandmark(X):
norm = np.linalg.norm(a - X[:2, :])
noise = 0.5
return pyibex.Interval(norm - noise, norm + noise)
T = np.arange(t0, tmax, dt)
tm = 0.3 # period between two SIVIAs
i = 0 # number of SIVIAs already computed
P = pyibex.IntervalVector(2, [-1000, 1000]) # SIVIA init box
vibes.beginDrawing()
for t in T:
print(t)
"""measurement step"""
# velocity
v_noise = dv * (2 * np.random.rand() - 1)
v_measured = X[3, 0] + v_noise
v = pyibex.Interval(v_measured - dv, v_measured + dv)
# heading
theta_noise = dtheta * (2 * np.random.rand() - 1)
theta_measured = X[2, 0] + theta_noise
theta = pyibex.Interval(theta_measured - dtheta,
theta_measured + dtheta)
u = np.array([[0, 0.3]]).T # command
X += dt * f(X, u) # Euler's scheme
"""localization step"""
if t > tm * i:
h = getDepth(X)
if i == 0: # first SIVIA
# generation of the contractor associated with the current sea depth measurement
ctc = getCtcBathy(bathy, h, dh)