
drag = 0.00070115
mass = 6.01093e-04
# drag = 0.00070115/2

A = np.array([[0, 1], [0, -drag/mass]])
B = np.array([[0],[1/(mass*10**4)]])
C = np.array([[-1, 0]])
step_size = 100

process_noise_1 = 50
process_noise_2 = 50
sensor_noise = 20

sigma_u = np.array([[process_noise_1**2,0],[0,process_noise_2**2]]) 
sigma_z = np.array([[sensor_noise**2]]) 

# discretize 
dt = t[1] - t[0]
Ad = np.eye(2) + dt * A
Bd = dt * B


mu = np.array([[-tof_interp[0]],[60]])
sigma = np.array([[0,5**2],[5**2,0]])

def kf(mu,sigma,u,y):
    
    mu_p = Ad.dot(mu) + Bd.dot(u) 
    sigma_p = Ad.dot(sigma.dot(Ad.transpose())) + sigma_u
    
    sigma_m = C.dot(sigma_p.dot(C.transpose())) + sigma_z
    kkf_gain = sigma_p.dot(C.transpose().dot(np.linalg.inv(sigma_m)))

    y_m = y-C.dot(mu_p)
    mu = mu_p + kkf_gain.dot(y_m)    
    sigma=(np.eye(2)-kkf_gain.dot(C)).dot(sigma_p)

    return mu,sigma


kf_mu = np.empty((1,2))

for i in range(len(t_interp)):
    mu, sigma = kf(mu, sigma, [[u_interp[i]/step_size]], [[tof_interp[i]]])
    kf_mu = np.append(kf_mu, [mu[:,0]], axis=0)

Initialize¶

Run KF¶