<jupyter_start><jupyter_text>Data<jupyter_code># Set Random Seed
np.random.seed(42)

# Generate Data
position = 0
velocity = 100
altitude = 1000
dt = 0.05

num = 400
mea_dis = []
mea_pos = []
mea_vel = []
mea_alt = []
for i in range(num):
    v = velocity + 5 * np.random.randn()
    a = altitude + 10 * np.random.randn()
    p = position + velocity * dt
    d = np.sqrt(np.square(a) + np.square(p)) + p * 0.05 * np.random.randn()
    mea_dis.append(d)
    mea_pos.append(p)
    mea_vel.append(v)
    mea_alt.append(a)
    position = p<jupyter_output><empty_output><jupyter_text>Run<jupyter_code># Set Parameters
x = np.matrix([[0], 
               [90],
               [1100]])
P = np.matrix([[100, 0, 0], 
               [0, 100, 0],
               [0, 0, 100]])
Q = np.matrix([[0.01, 0, 0], 
               [0, 0.01, 0],
               [0, 0, 0.01]])
R = np.matrix([[10]])

def predict(x, dt=0.05):
    A = np.eye(3) + dt*np.matrix([[0, 1, 0],
                                  [0, 0, 0], 
                                  [0, 0, 0]])     
    return np.dot(A, x)

def measure(x):
    return np.sqrt(x[0, 0]**2+x[2, 0]**2)

# UKF
ukf = UKF(predict, measure, P, Q, R)
est_pos = []
est_vel = []
est_alt = []
Ps = []
Ks = []
for z in mea_dis:
    z = np.matrix([z])
    x = ukf.step(x, z)
    est_pos.append(x[0, 0])
    est_vel.append(x[1, 0])
    est_alt.append(x[2, 0])
est_dis = np.sqrt(np.square(est_pos) + np.square(est_alt))<jupyter_output><empty_output><jupyter_text>Graph<jupyter_code>plt.figure(figsize=(25, 4))

plt.subplot(1, 4, 1)
plt.plot(mea_pos, 'o', alpha=0.5, color='k', label='Data')
plt.plot(est_pos, lw=3, color='r', label='Estimation (EKF)')
plt.legend()
plt.xlim(0, num)
plt.xlabel('Time')
plt.ylabel('Position')

plt.subplot(1, 4, 2)
plt.plot(mea_vel, 'o', alpha=0.5, color='k', label='Data')
plt.plot(est_vel, lw=3, color='r', label='Estimation (EKF)')
plt.legend()
plt.xlim(0, num)
plt.xlabel('Time')
plt.ylabel('Velocity')

plt.subplot(1, 4, 3)
plt.plot(mea_alt, 'o', alpha=0.5, color='k', label='Data')
plt.plot(est_alt, lw=3, color='r', label='Estimation (EKF)')
plt.legend()
plt.xlim(0, num)
plt.xlabel('Time')
plt.ylabel('Altitude')

plt.subplot(1, 4, 4)
plt.plot(mea_dis, 'o', alpha=0.5, color='k', label='Data')
plt.plot(est_dis, lw=3, color='r', label='Estimation (EKF)')
plt.legend()
plt.xlim(0, num)
plt.xlabel('Time')
plt.ylabel('Distance')

plt.show()<jupyter_output><empty_output>
StarcoderdataJupyterStructuredCleanDedup