python 卡尔曼滤波

code

def g_h_filter(data,x0,dx,g,h,dt=1.0,pred=None):
    x=x0
    results=[]
    for z in data:
        x_est=x+(dx*dt)
        dx=dx
        if pred is not None:
            pred.append(x_est)
        residual=z-x_est
        dx=dx+h*(residual)/dt
        x=x_est+g*residual
        results.append(x)
    return results
weights = ([158.0, 164.2, 160.3, 159.9, 162.1, 164.6,169.6, 167.4, 166.4, 171.0, 171.2, 172.6])
print(weights)
data = g_h_filter(data=weights, x0=160, dx=1, g=3./10, h=1./3, dt=1.)
print(data)

outputs

macname@MacdeMBP ~ % python -u "/Users/macname/Desktop/py/test.py"
[158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 169.6, 167.4, 166.4, 171.0, 171.2, 172.6]
[160.1, 161.32999999999998, 161.97766666666666, 161.75181111111112, 161.63644185185186, 162.4602027530864, 165.2501011329218, 167.99299628783263, 169.41535746699267, 170.78589113674047, 171.87663432665045, 172.83460978403727]
macname@MacdeMBP ~ %