Simple outlier removal sampler for the BNO055 sensor (or any distance sensor for that matter)

orientation_sampler.py

import math

'''
Divides the sample into two clusters - left and right, where the left cluster
contains all values > mean(sample) and the right cluster contains all other
values.

The cluster with most number of values wins! Returns the winning cluster's mean.

If the cluster's max deviation from mean (i.e., min(cluster) - mean(sample)) is
greater than max_deviation, then the winning cluster is further clustered.
'''
def sample_result(sample, max_deviation = 1.0):
	deviation = math.inf
	while deviation > max_deviation:
		mean = sum(sample) / len(sample)
		left = []
		right = []

		for value in sample:
			(left if value > mean else right).append(value)

		sample = left if len(left) > len(right) else right
		deviation = abs(min(sample) - mean)

	return sum(sample) / len(sample)


##### TESTING #####

import random
def test(true_value = 45.0, nearly_correct_count = 30, outlier_count = 5):
	def random_sign():
		return 1 if random.choice([True, False]) else -1;
	
	samples = [
		*[true_value + random.uniform(-0.2, 0.2) for x in range(nearly_correct_count)],
		*[true_value + (random_sign() * 1000) + (random_sign() * 1000 * random.uniform(0.0, 1.0)) for x in range(outlier_count)]
	]
	
	result = sample_result(samples)
	print("True value: %.4f" % true_value)
	print("Sampled value: %.4f" % result)
	print("Deviation: %.4f" % abs(result - true_value))

test()
'''
Sample Test Output:
True value: 45.0000
Sampled value: 44.9209
Deviation: 0.0791
'''