pchang718
56 lines
1.6 KiB
Python
56 lines
1.6 KiB
Python
import numpy as np
|
|
from scipy import signal
|
|
|
|
def integer_lfilter(b_int, a_int, x_float, shift_in, shift_out, shift_b, shift_a):
|
|
x_int = np.round(x_float * (2**shift_in)).astype(np.int64)
|
|
y_int = np.zeros(len(x_int), dtype=np.int64)
|
|
|
|
q_bx = shift_in + shift_b
|
|
q_ay = shift_out + shift_a
|
|
q_acc = max(q_bx, q_ay)
|
|
|
|
s_bx = q_acc - q_bx
|
|
s_ay = q_acc - q_ay
|
|
|
|
A0 = a_int[0]
|
|
nb = len(b_int)
|
|
na = len(a_int)
|
|
|
|
for n in range(len(x_int)):
|
|
acc = 0
|
|
for i in range(nb):
|
|
if n - i >= 0:
|
|
acc += (b_int[i] * x_int[n - i]) << s_bx
|
|
for j in range(1, na):
|
|
if n - j >= 0:
|
|
acc -= (a_int[j] * y_int[n - j]) << s_ay
|
|
|
|
# 原本的邏輯：(acc // A0) >> s_ay
|
|
# 修正後的邏輯：直接計算總位移，減少除法誤差
|
|
# y_int[n] = (acc // A0) >> s_ay
|
|
|
|
# 模擬 DSP 的 A0 通常是 2^shift_a
|
|
# 所以 acc / 2^shift_a >> (q_acc - shift_a - shift_out)
|
|
# 等於 acc >> (q_acc - shift_out)
|
|
total_shift = q_acc - shift_out
|
|
y_int[n] = acc >> total_shift
|
|
|
|
return y_int.astype(float) / (2**shift_out)
|
|
|
|
# 測試 1: 低通濾波器
|
|
fs = 1000
|
|
fc = 10
|
|
b, a = signal.butter(1, fc / (fs/2), 'low')
|
|
# b = [0.03046, 0.03046], a = [1.0, -0.939]
|
|
shift = 14
|
|
b_int = [int(round(x * (2**shift))) for x in b]
|
|
a_int = [int(round(x * (2**shift))) for x in a]
|
|
|
|
x = np.ones(100) # Step input
|
|
y_float = signal.lfilter(b, a, x)
|
|
y_fixed = integer_lfilter(b_int, a_int, x, shift, shift, shift, shift)
|
|
|
|
print("Float Final:", y_float[-1])
|
|
print("Fixed Final:", y_fixed[-1])
|
|
print("Diff:", y_float[-1] - y_fixed[-1])
|