Accela Filter in FreePIE

[Leshchuk]

I am trying to rewrite Stanislaw Halik's famous accela filter for my sketch that is connecting arduino ahrs to freetrack. But, so far, with no success. Probably, because I don't fully understand the algorithm that stands behind aforementioned filter. I would appreciate any help regarding to it.

Code: Select all

def do_deltas(deltas, output):

	norm = [0, 0, 0]
	
	dist = math.sqrt(
				  deltas[0] * deltas[0]
				+ deltas[1] * deltas[1]
				+ deltas[2] * deltas[2])
	
	value = 40 * rot_gains_divided_by_40.getY(dist)
	
	diagnostics.watch(value)
	
	for k in range(3):
		c = clamp(math.fabs(deltas[k]) / dist, 0, 1) if dist > 10**-6 else 0
		norm[k] = c
		
	diagnostics.watch(norm[0])
	diagnostics.watch(norm[1])
	diagnostics.watch(norm[2])
	
	n = sum(norm)
	
	diagnostics.watch(n)
	
	if n > 10**-6:
		ret = 1 / n
		for k in range(3):
			norm[k] *= ret
	else:
		for k in range(3):
			norm[k] = 0
	
	for k in range(3):
		d = norm[k] * value
		output[k] = math.copysign(d, deltas[k])


def accela_filter(input, output):

	global accela_last_output
	global accela_deltas
	global accela_time_stamp
		
	global ROTATION_THRESHOLD
	global ROTATION_DEADZONE
	
	dt = time.time() - accela_time_stamp
	accela_time_stamp = time.time()

	diagnostics.watch(dt)
	
	for i in range(3):
		d = input[i] - accela_last_output[i]
		
		if math.fabs(d) > math.radians(ROTATION_DEADZONE):
			d -= math.copysign(math.radians(ROTATION_DEADZONE), d)
		else:
			d = 0
			
		diagnostics.watch(d)
		
		accela_deltas[i] = d / math.radians(ROTATION_THRESHOLD)
		
		diagnostics.watch(accela_deltas[0])

	do_deltas(accela_deltas, output)
	
	for k in range(3):
		output[k] *= dt
		output[k] += accela_last_output[k]
		
		accela_last_output[k] = output[k]

Those are two main functions that I have managed to rewrite from C++. In a second or so after the sketch start, output vector becomes filled with NaNs.