-
Notifications
You must be signed in to change notification settings - Fork 0
/
params_vehiculo.py
118 lines (106 loc) · 4.34 KB
/
params_vehiculo.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
import numpy as np
# Parametros robot de traccion diferencial
Ts = 0.01 # tiempo de muestreo
L = 0.6 # largo base
W = 0.4 # ancho base
H = 0.15 # alto base
r = 0.15 # radio ruedas
m = 10 # masa del cuerpo de la base
# momento de inercia de la base alrededor del eje normal al terreno
J = 1 # m/12*(L^2 + W^2) + 2 J_w(W/2)^2
c = 0.3 # efectos de friccion dinamica (roce aerodinamico y resistencia viscosa) sentido longitudinal
b = 0.3 # efectos de friccion dinamica (roce aerodinamico y resistencia viscosa) sentido eje de giro
# simulacion
t0 = 0
t1 = 2.5
t2 = 5
t3 = 10
t4 = 15
tf = 8.3 # Tiempo final 9.6, 11, no termina hasta cerrar pestana
#tiempos referencia
# matriz de covarianza del estado estimado
P0 = np.array([[0.01, 0, 0, 0, 0],
[0, 0.01, 0, 0, 0],
[0, 0, 0.03, 0, 0],
[0, 0, 0, 0.01, 0],
[0, 0, 0, 0, 0.03]])
P0_lqi = np.array([[0.01, 0, 0, 0, 0, 0, 0, 0],
[0, 0.01, 0, 0, 0, 0, 0, 0],
[0, 0, 0.03, 0, 0, 0, 0, 0],
[0, 0, 0, 0.01, 0, 0, 0, 0],
[0, 0, 0, 0, 0.03, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1],])
# matrices
# perturbaciones de estado con media 0 y varianza Qx iid
sigma_vx = np.array([10**(-4), 10**(-4), 3*10**(-4), 0.01, 0.03])
Qx = np.array([[10**(-4), 0, 0, 0, 0],
[0, 10**(-4), 0, 0, 0],
[0, 0, 3*10**(-4), 0, 0],
[0, 0, 0, 0.01, 0],
[0, 0, 0, 0, 0.03]])
sigma_vx_lqi = np.array([10**(-4), 10**(-4), 3*10**(-4), 0.01, 0.03, 10**(-4), 10**(-4), 10**(-4)])
Qx_lqi = np.array([[10**(-4), 0, 0, 0, 0, 0, 0, 0],
[0, 10**(-4), 0, 0, 0, 0, 0, 0],
[0, 0, 3*10**(-4), 0, 0, 0, 0, 0],
[0, 0, 0, 0.01, 0, 0, 0, 0],
[0, 0, 0, 0, 0.03, 0, 0, 0],
[0, 0, 0, 0, 0, 10**(-4), 0, 0],
[0, 0, 0, 0, 0, 0, 10**(-4), 0],
[0, 0, 0, 0, 0, 0, 0, 10**(-4)]])
# perturbaciones de entrada con media 0 y varianza Qu iid
sigma_vu = np.array([2*10**(-4)*m*r, 2*10**(-4)*m*r])
Qu = np.array([[2*10**(-4)*m*r, 0],
[0, 2*10**(-4)*m*r]])
# ruido en los sensores (tacometro) iid -> medicion de velocidades
sigma_wt = np.array([10**(-4), 10**(-4)])
Rt = np.array([[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 10**(-4), 0],
[0, 0, 0, 0, 10**(-4)]])
# ruido en los sensores (encoder)
sigma_we = (np.pi*r**2/360)*np.array([[1, 2/W], [2/W, 4/(W**2)]])
Re = (np.pi*r**2/360)*np.array([[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 1, 2/W],
[0, 0, 0, 2/W, 4/W**2]])
# ruido en los sensores (IMU)
sigma_wi = np.array([0.0004, 0.0004*np.pi**2])
Ri = np.array([[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0.0004, 0],
[0, 0, 0, 0, 0.0004*np.pi**2]])
# ruido en los sensores (GPS)
sigma_wg = np.array([0.01, 0.01, 2./360.**2*np.pi])
Rg = (np.pi*r**2/360)*np.array([[0.01, 0, 0, 0, 0],
[0, 0.01, 0, 0, 0],
[0, 0, 0, 2./360.**2*np.pi, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0]])
# costos funcion objetivo PID
# matrices de costo LQI
Q_pid = np.array([[0.005,0.,0.,0.,0.],
[0.,0.005,0,0.,0.],
[0.,0.,0.001,0,0.],
[0.,0.,0.,0.001,0.],
[0.,0.,0.,0.,0.008]])
R_pid = np.array([[0.5, 0.0],
[0.0, 0.5]])
# matrices de costo LQI
Q_lqr = np.array([[0.005,0.,0.,0.,0., 0, 0, 0],
[0.,0.005,0,0.,0., 0, 0, 0],
[0.,0.,0.001,0,0.,0, 0, 0],
[0.,0.,0.,0.001,0.,0, 0, 0],
[0.,0.,0.,0.,0.008, 0, 0, 0],
[0.,0.,0.,0.,0, 1/1000000000**2, 0, 0],
[0.,0.,0.,0.,0, 0, 1/1000000000**2, 0],
[0.,0.,0.,0.,0, 0, 0, 1/(200000*np.pi)**2]])
R_lqr = np.array([[0.5, 0.0, 0, 0, 0],
[0.0, 0.5, 0, 0, 0],
[0.0, 0, 10**15, 0, 0],
[0.0, 0, 0, 10**15, 0],
[0.0, 0, 0, 0, 10**15]])