def comparison_noise(sigma_V, sigma_omega):
"""
makes a comparison of relative error according 3 methods
Method 1 : pseudoinv
Method 2 : 2 AN
Method 3 : single AN
In : sigma_V : noise for V
sigma_omega : noise for omega
Out : an array([[error_Rr_method1, error_Rl_method1, error_L_method1],
[error_Rr_method2, error_Rl_method2, error_L_method2],
[error_Rr_method3, error_Rl_method3, error_L_method3]])
"""
Robot = Vehicle(0.08, 0.081, 0.2, sigma_V, sigma_omega)
Rr = Robot.right_wheel_radius
Rl = Robot.left_wheel_radius
L = Robot.space_wheels
ds = Robot.generate_data(-10, 10, 5000)
Rr_est3, Rl_est3, L_est3 = odo.all_param_AN(ds)
Rr_est2, Rl_est2 = odo.wheels_radius_AN(ds)
L_est2 = odo.space_wheels_AN(ds, (Rr_est2, Rl_est2))
Rr_est1, Rl_est1 = odo.wheels_radius_INV(ds)
L_est1 = odo.space_wheels_INV(ds, (Rr_est1, Rl_est1))
error = np.array([[
abs((Rr_est1 - Rr) / Rr),
abs((Rl_est1 - Rl) / Rl),
abs((L_est1 - L) / L)
],
[
abs((Rr_est2 - Rr) / Rr),
abs((Rl_est2 - Rl) / Rl),
abs((L_est2 - L) / L)
],
[
abs((Rr_est3 - Rr) / Rr),
abs((Rl_est3 - Rl) / Rl),
abs((L_est3 - L) / L)
]])
return error
def comparison_noise(sigma_V_max, sigma_omega_max):
Npoints = 5 #nb of point for the plot
tab_sigma_V = np.linspace(0, sigma_V_max, Npoints)
tab_sigma_omega = np.linspace(0, sigma_omega_max, Npoints)
res_V_sigma1 = np.zeros(Npoints)
res_V_mu1 = np.zeros(Npoints)
res_omega_sigma1 = np.zeros(Npoints)
res_omega_mu1 = np.zeros(Npoints)
res_V_sigma2 = np.zeros(Npoints)
res_V_mu2 = np.zeros(Npoints)
res_omega_sigma2 = np.zeros(Npoints)
res_omega_mu2 = np.zeros(Npoints)
for i in range(Npoints):
Robot = Vehicle(0.08, 0.081, 0.2, tab_sigma_V[i], tab_sigma_omega[i])
ds = Robot.generate_data(-10, 10, 5000)
Rr_est1, Rl_est1, L_est1 = odo.all_param_AN(ds)
residuals1 = odo.residuals(ds, Rr_est1, Rl_est1, L_est1)
res_V_sigma1[i] = residuals1[0][2]
res_V_mu1[i] = residuals1[0][1]
res_omega_sigma1[i] = residuals1[1][2]
res_omega_mu1[i] = residuals1[1][1]
Rr_est2, Rl_est2 = odo.wheels_radius_INV(ds)
L_est2 = odo.space_wheels_INV(ds, (Rr_est2, Rl_est2))
residuals2 = odo.residuals(ds, Rr_est2, Rl_est2, L_est2)
res_V_sigma2[i] = residuals2[0][2]
res_V_mu2[i] = residuals2[0][1]
res_omega_sigma2[i] = residuals2[1][2]
res_omega_mu2[i] = residuals2[1][1]
plt.figure()
plt.subplot(221)
plt.plot(tab_sigma_V, res_V_sigma1, label="AN")
plt.plot(tab_sigma_V, res_V_sigma2, label="INV")
plt.xlabel("$\sigma_V$")
plt.ylabel("ecart type")
plt.title("V residuals")
plt.legend()
plt.subplot(222)
plt.plot(tab_sigma_V, res_V_mu1)
plt.plot(tab_sigma_V, res_V_mu2)
plt.xlabel("$\sigma_V$")
plt.ylabel("moyenne")
plt.title("V residuals")
plt.subplot(223)
plt.plot(tab_sigma_omega, res_omega_sigma1)
plt.plot(tab_sigma_omega, res_omega_sigma2)
plt.xlabel("$\sigma_{\omega}$")
plt.ylabel("ecart type")
plt.title("omega residuals")
plt.subplot(224)
plt.plot(tab_sigma_omega, res_omega_mu1)
plt.plot(tab_sigma_omega, res_omega_mu2)
plt.xlabel("$\sigma_{\omega}$")
plt.ylabel("moyene")
plt.title("omega residuals")
def comparison_ANs(sigma_V_max, sigma_omega_max):
"""
draws a comparison of two-AN algo and all_param_An algo according noise. No outliers.
sigma(noise)_V = [0, ..., sigma_V_max]
sigma(noise)_omega = [0, ..., sigma_omega_max]
Comparison based on mean and std of residuals. (fct odometry.residuals())
Results : no difference !
"""
Npoints = 5 #nb of point for the plot
tab_sigma_V = np.linspace(0, sigma_V_max, Npoints)
tab_sigma_omega = np.linspace(0, sigma_omega_max, Npoints)
res_V_sigma1 = np.zeros(Npoints)
res_V_mu1 = np.zeros(Npoints)
res_omega_sigma1 = np.zeros(Npoints)
res_omega_mu1 = np.zeros(Npoints)
res_V_sigma2 = np.zeros(Npoints)
res_V_mu2 = np.zeros(Npoints)
res_omega_sigma2 = np.zeros(Npoints)
res_omega_mu2 = np.zeros(Npoints)
for i in range(Npoints):
Robot = Vehicle(0.08, 0.081, 0.2, tab_sigma_V[i], tab_sigma_omega[i])
ds = Robot.generate_data(-10, 10, 5000)
Rr_est1, Rl_est1 = odo.wheels_radius_AN(ds)
L_est1 = odo.space_wheels_AN(ds, (Rr_est1, Rl_est1))
residuals1 = odo.residuals(ds, Rr_est1, Rl_est1, L_est1)
res_V_sigma1[i] = residuals1[0][2]
res_V_mu1[i] = residuals1[0][1]
res_omega_sigma1[i] = residuals1[1][2]
res_omega_mu1[i] = residuals1[1][1]
Rr_est2, Rl_est2, L_est2 = odo.all_param_AN(ds)
residuals2 = odo.residuals(ds, Rr_est2, Rl_est2, L_est2)
res_V_sigma2[i] = residuals2[0][2]
res_V_mu2[i] = residuals2[0][1]
res_omega_sigma2[i] = residuals2[1][2]
res_omega_mu2[i] = residuals2[1][1]
plt.figure()
plt.subplot(221)
plt.plot(tab_sigma_V, res_V_sigma1, label="2ANs")
plt.plot(tab_sigma_V, res_V_sigma2, label="1ANs")
plt.xlabel("$\sigma_V$")
plt.ylabel("ecart type")
plt.title("V residuals")
plt.legend()
plt.subplot(222)
plt.plot(tab_sigma_V, res_V_mu1)
plt.plot(tab_sigma_V, res_V_mu2)
plt.xlabel("$\sigma_V$")
plt.ylabel("moyenne")
plt.title("V residuals")
plt.subplot(223)
plt.plot(tab_sigma_omega, res_omega_sigma1)
plt.plot(tab_sigma_omega, res_omega_sigma2)
plt.xlabel("$\sigma_{\omega}$")
plt.ylabel("ecart type")
plt.title("omega residuals")
plt.subplot(224)
plt.plot(tab_sigma_omega, res_omega_mu1)
plt.plot(tab_sigma_omega, res_omega_mu2)
plt.xlabel("$\sigma_{\omega}$")
plt.ylabel("moyene")
plt.title("omega residuals")
