alpha-beta滤波

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()