The alpha - beta - gamma filter
import numpy as np
import matplotlib.pyplot as plt
import random
mid, sigma = 0, 0.1
totalTimeStep = 300
noise = np.random.normal(mid, sigma, totalTimeStep) * 100
dt = 1
v = 0
x_lf = 0
# 测量值
x = 0
x_lf_list = []
x_list = []
# 估计值
x_hat = 0
v_hat = 0
a_hat = 0
# 预测值
x_bar = 0
v_bar = 0
a_bar = 0
alpha, beta, gamma = 0.1, 0.08, 0.0
x_hat_list = []
for i in range(totalTimeStep):
a = random.uniform(-0.1, 0.1)
v += a * dt
x_lf += v * dt
x = x_lf + noise[i]
x_hat = x_bar + alpha * (x - x_bar)
v_hat = v_bar + beta * (x - x_bar) / dt
a_hat = a_bar + gamma * (x - x_bar) / dt
a_bar = a_hat
v_bar = v_hat + a_hat * dt
x_bar = x_hat + v_hat * dt + 0.5 * a_hat * dt * dt
x_lf_list.append(x_lf)
x_list.append(x)
x_hat_list.append(x_hat)
plt.plot(x_lf_list)
plt.plot(x_list)
plt.plot(x_hat_list)
plt.legend(["True", "measured", "filtered"])
plt.show()