g-h filter

Introduced in the Kalman Filter book.

This filter is the basis for a huge number of filters, such as the Kalman Filter.

def g_h_filter(data, x0, dx, g, h, dt=1.):
	x_est = x0
	results = []
	for z in data:
		# prediction step
		x_pred = x_est + (dx*dt)
		dx = dx
		# update step
		residual = z - x_pred
		dx = dx + h * (residual) / dt
		x_est = x_pred + g * residual
		results.append(x_est)
	return np.array(results)