<jupyter_start><jupyter_text>Sketch of UWB pipelineThis notebook contains the original sketch of the uwb implementation which is availible in the uwb package.Code in the package is mostly reworked and devide in modules. For usage of the package please check outthe other notebook in the directory.<jupyter_code>%matplotlib inline

import numpy as np
import matplotlib.pyplot as plt
from sklearn.datasets import make_blobs
from sklearn.cluster import DBSCAN
from itertools import product
from scipy.stats import multivariate_normal
from functools import reduce
def multi_dim_noise(grid_dims, amount, step, std=10, means=(1,5)):
    prod = reduce((lambda x,y: x*y), grid_dims)
    samples = np.zeros(grid_dims + [amount , len(grid_dims)])
    clusters = np.random.randint(
        means[0], means[1] + 1, size=grid_dims
    )
    
    grid = []
    for dim in grid_dims:
        grid.append(((np.arange(dim) + 1) * step))

    mean = np.array(np.meshgrid(*grid, indexing="ij")).reshape(prod, len(grid_dims))
    noise = np.random.randn(means[1], prod, len(grid_dims)) * std
    centers = (noise + mean).reshape([means[1]] + grid_dims + [len(grid_dims)])
    
    # transpose hack for selection
    roll_idx = np.roll(np.arange(centers.ndim),-1).tolist()
    centers = np.transpose(centers, roll_idx)

    for idxs in product(*[range(i) for i in grid_dims]):
        print(idxs)
        samples[idxs] = make_blobs(
            n_samples=amount, centers=(centers[idxs][:, 0:clusters[idxs]]).T
        )[0]
    return samples
def generate_noise(width, length, amount, step, std=10, means=(1,5)):
        samples = np.zeros((width, length, amount, 2))

        clusters = np.random.randint(
            means[0], means[1] + 1, size=(width, length)
        )

        # calculate centers
        grid_width = (np.arange(width) + 1) * step
        grid_length = (np.arange(length) + 1) * step
        mean = np.array(
            [
                np.repeat(grid_width, len(grid_length)),
                np.tile(grid_length, len(grid_width)),
            ]
        ).T
        noise = np.random.randn(means[1], width * length, 2) * std
        centers = (noise + mean).reshape((means[1], width, length, 2))

        for i in range(width):
            for j in range(length):
                samples[i, j, :] = make_blobs(
                    n_samples=amount, centers=centers[0 : clusters[i, j], i, j, :]
                )[0]

        return samples, (grid_width, grid_length)
np.random.seed(0)
data, map_grid = generate_noise(3, 3, 50, 10)
multi_dim_noise([4,2,5], 50, 10)
plt.plot(data[0,0,:,0], data[0,0,:,1], 'o') # example of 5 clusters in position 0,0
plt.show()
def generate_map(noise, eps=2, min_samples=3):
    db = DBSCAN(eps=eps, min_samples=min_samples).fit(noise)
    core_samples_mask = np.zeros_like(db.labels_, dtype=bool)
    core_samples_mask[db.core_sample_indices_] = True
    labels = db.labels_

    # Number of clusters in labels, ignoring noise if present.
    n_clusters_ = len(set(labels)) - (1 if -1 in labels else 0)
    n_noise_ = list(labels).count(-1)
    return labels, core_samples_mask, n_clusters_
def plot_clusters(X, labels, core_sapmles_mask, n_clusters_):
    
    unique_labels = set(labels)
    colors = [plt.cm.Spectral(each)
              for each in np.linspace(0, 1, len(unique_labels))]
    for k, col in zip(unique_labels, colors):
        if k == -1:
            # Black used for noise.
            col = [0, 0, 0, 1]

        class_member_mask = (labels == k)

        xy = X[class_member_mask & core_samples_mask]
        plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col),
                 markeredgecolor='k', markersize=14)

        xy = X[class_member_mask & ~core_samples_mask]
        plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col),
                 markeredgecolor='k', markersize=6)
    plt.title('Estimated number of clusters: %d' % n_clusters_)
labels = np.zeros((3, 3, 50), dtype=int)
for x,y in product(range(3), range(3)):
    labels[x,y,:], core_samples_mask, n_clusters_ = generate_map(data[x,y,:,:])
    plot_clusters(data[x,y,:,:], labels[x,y,:], core_samples_mask, n_clusters_)
    plt.show()
# estimate parameters
# this is quite slow but calculation is perfomed only once per map generation

params = [[[] for i in range(3)] for i in range(3)]

for x,y in product(range(3), range(3)):
    used_data = 50 - list(labels[x,y]).count(-1)
    
    for i in range(np.max(labels[x,y,:]) + 1):
        mask = labels[x,y] == i
        mean_noise = data[x,y,mask,:].mean(axis=0) - np.array([(x+1) * 10,(y+1) * 10])
        cov_noise = np.cov(data[x,y,mask,:].T)
        weight = mask.sum() / used_data
        params[x][y].append((mean_noise, cov_noise, weight))

print(params)
# dynamics model

walk = []
start_state = np.array([[20, 20, 0, 0]], dtype=float)
walk.append(start_state)

def transition_function(current_state, x_range=(10, 40), y_range=(10, 40), std=1):
    """Performs a one step transition assuming sensing interval of one
    
    Format of current_state = [x,y,x',y'] + first dimension is batch size
    """
    next_state = np.copy(current_state)
    next_state[: ,0:2] += current_state[:, 2:4]
    next_state[: ,2:4] += np.random.randn(2) * std
    
    
    next_state[: ,0] = np.clip(next_state[: ,0], x_range[0], x_range[1])
    next_state[: ,1] = np.clip(next_state[: ,1], y_range[0], y_range[1])
    return next_state

next_state = transition_function(start_state)
walk.append(next_state)
for i in range(100):
    next_state = transition_function(next_state)
    walk.append(next_state)
walk = np.array(walk)
print(walk.shape)
plt.plot(walk[:,0,0], walk[:,0, 1])
plt.show()
# measurement noise map augmented particle filter

def find_nearest_map_position(x,y, map_grid):
    x_pos = np.searchsorted(map_grid[0], x)
    y_pos = np.searchsorted(map_grid[1], y, side="right")

    x_valid = (x_pos != 0) & (x_pos < len(map_grid[0]))
    x_pos = np.clip(x_pos, 0, len(map_grid[0]) - 1)
    x_dist_right = map_grid[0][x_pos] - x
    x_dist_left = x - map_grid[0][x_pos - 1]
    x_pos[x_valid & (x_dist_right > x_dist_left)] -= 1
    
    y_valid = (y_pos != 0) & (y_pos < len(map_grid[1]))
    y_pos = np.clip(y_pos, 0, len(map_grid[1]) - 1)
    y_dist_right = map_grid[1][y_pos] - y
    y_dist_left = y - map_grid[0][y_pos - 1]
    y_pos[y_valid & (y_dist_right > y_dist_left)] -= 1  
  
    return x_pos, y_pos


def reweight_samples(x, z, w, params, map_grip):
    x_pos, y_pos = find_nearest_map_position(x[:,0], x[:,1], map_grid)
    new_weights = np.zeros_like(w)
    
    for i, (x_p, y_p) in enumerate(zip(x_pos, y_pos)):
        for gm in params[x_p][y_p]:
            # calculating p(z|x) for GM
            mean, cov, weight = gm
            new_weights[i] += multivariate_normal.pdf(z[i, 0:2] ,mean=mean, cov=cov) * weight * w[i]
    denorm = np.sum(new_weights)
    return new_weights / denorm
                
    
print(map_grid)
x = np.array([9, 10, 11, 14, 16, 24, 31, 30, 29, 15])
y = np.array([9, 10, 11, 14, 16, 24, 31, 30, 29, 15])
w = np.ones(10) * 0.1
print(find_nearest_map_position(
    x,
    y,
    map_grid
))
x_noise = np.random.randn(10)
y_noise = np.random.randn(10)
particles = np.stack((x, y, x_noise, y_noise)).T
transitioned_particles = transition_function(particles)

n_w = reweight_samples(particles, transitioned_particles, w, params, map_grid)

# compute metrics for resampling

def compute_ESS(x, w):
    M = len(x)
    CV = 1/M * np.sum((w*M-1)**2)
    return M / (1 + CV)

print(compute_ESS(particles, w))
print(compute_ESS(particles, n_w)) # needs to be resampled<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1473670421797223, 'fraction_numerical': 0.026722556981922975, 'mean_word_length': 3.286917462099944, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 7, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '668604', 'license': 'MIT', 'n_tokens_mistral': 2970, 'n_tokens_neox': 2824, 'n_words': 660, 'path': 'notebooks/noise_map_generator_example.py.ipynb', 'repo_name': 'freiberg-roman/uwb-proto', 'size': '7634'}