def observer_dyn(P_model_cov_true, X_est, u):
P_model_cov_est=inv(inv(P_model_cov_true)+\
mtimes(mtimes(transpose(C),inv(V_cov)),C))
X_est = integrator_fun_a(X_est, u)
Y_meas = X_true[0:3, :] + vk
Ck_gk = np.dot(C, X_est)
measurement_residual = Y_meas - Ck_gk
Kalman_gain = mtimes(mtimes(P_model_cov_est, transpose(C)), inv(V_cov))
X_est = X_est + mtimes(Kalman_gain, measurement_residual)
print(Kalman_gain)
return (X_est, P_model_cov_est)
def plant_dyn(X_true, u, A, P_model_cov_est):
X_true = integrator_fun_a(X_true, u)
P_model_cov_true=mtimes(A, mtimes(P_model_cov_est, \
transpose(A))) + W_cov
"""
if(i<80):
X_true[4]=X_initial[4]
X_true[5]=X_initial[5]
X_true[6]=X_initial[6]
elif(i>80):
X_true[4]=-X_initial[4]
X_true[5]=-X_initial[5]
X_true[6]=-X_initial[6] """
X_true[4:7] = X_initial[4:7]
print(X_true)
return (X_true, P_model_cov_true)
def observer_dyn_offline(P_model_cov_true, X_est, u):
P_model_cov_est=inv(inv(P_model_cov_true)+\
mtimes(mtimes(transpose(C),inv(V_cov)),C))
X_est = mod_aug.integrator_fun_a(X_est, u)
#reduced the value of vk
Y_meas = X_true[0:3, :] + vk
Ck_gk = np.dot(C, X_est)
measurement_residual = Y_meas - Ck_gk
Kalman_gain = mtimes(mtimes(P_model_cov_est, transpose(C)), inv(V_cov))
X_est = X_est + mtimes(Kalman_gain, measurement_residual)
X_est[4] = d_1[i]
X_est[5] = d_2[i]
X_est[6] = d_3[i]
return (X_est, P_model_cov_est, Y_meas)
X_est = [0.1, 0.1, 0.1, 2, -1, 1, 0]
X_values = X_initial
X_true = X_initial
xbar=np.array([0,\
0,\
0,\
1,
0,
0,
0\
])
ubar = np.array([0.08138923, 0.09206817])
xbar_fun = integrator_fun_a(xbar, ubar)
xbar_fun[0]=xbar_fun[0]-DT*(xbar[3]*np.cos(xbar[2] \
+C1*ubar[1])/(1-kapparef[0]*xbar[1]))-DT*xbar[4]
s_plot = np.append(s_plot, X_true[0])
n_plot = np.append(n_plot, X_true[1])
alpha_plot = np.append(alpha_plot, X_true[2])
velocity_plot = np.append(velocity_plot, X_true[3])
s_plot_est = np.append(s_plot_est, X_est[0])
n_plot_est = np.append(n_plot_est, X_est[1])
alpha_plot_est = np.append(alpha_plot_est, X_est[2])
velocity_plot_est = np.append(velocity_plot_est, X_est[3])
A_ss = integrator_jac_x_cont_a(xbar, ubar)
A_ss = A_ss[0:4, 0:4]