Use UKF for parameter identification

[tliu1023]

Hi ,

I'm learning to use UKF to identifying the parameter in the dynamic system from simulation data. Here is how I define the system. The x[3] is the parameter I want to identify, and x[0] and x[1] is the measurement I have.

def f_cv(x, dt, f):
    # x = [x, v, a, c1,]
    xt = x.T
    F = np.array([[ xt[0] + dt*xt[1] ],
                  [ xt[1] + dt*xt[2] ],
                  [ xt[2] + dt*(f - (x[3]*xt[1] + k1*xt[0] + k3*xt[0]**3)) ],
                  [ xt[3]]]).squeeze()
    return F
def h_cv(x):
    return [x[0], x[1]]
def force(t):
    return A * math.sin(w * t)

if __name__ == "__main__":
    dim_x = 4

    points = MerweScaledSigmaPoints(n=dim_x, alpha=.1, beta=2., kappa=3-dim_x)
    ukf = UKF(dim_x=dim_x, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=points)

    ukf.x = np.array([1., 1., 1., 1, ])
    ukf.P = np.diag([1, 1, 1, 1])
    ukf.R = np.diag([0.0001, 0.0001]) 
    ukf.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=dt, var=0.001)
    ukf.Q[3,3] = 0.01

    t = cubic_data['t'].squeeze()
    x = cubic_data['x'].squeeze()
    v = cubic_data['v'].squeeze()

    uxs = []
    for ti, xi, vi in zip(t, x, v):
        ukf.predict(f = force(ti))
        ukf.update(np.array([xi, vi]))
        uxs.append(ukf.x.copy())
    uxs = np.array(uxs)

After I implemented it, the filter works well for the measurement, but the parameter estimation cannot converge......

I would be very grateful if someone could help me.