En Parte 1 salimos después de derivar las ecuaciones básicas para un algoritmo de filtro de Kalman. Aquí se vuelven a mencionar para una referencia fácil.
A. Predecir:
a. X = A * X + B * u
b. P = A * P * AT * Q
B. Medida
a. Y = Z – H * X
b. K = (P * HT) / ((H * P * HT) + R)
C. Actualización
a. X = X + K * Y
b. P = (I – K * H) * P
Vamos a codificar la ecuación anterior en esta publicación, usando lecturas de sensor de un archivo de texto que está disponible gratuitamente en el controlador github de Udacity. Este archivo de texto ( obj_pose-laser-radar-synthetic-input.txt ) tiene lecturas de sensor de Láser y Radar junto con la marca de tiempo de lectura y los valores de verdad del terreno. Hasta este punto, solo hemos cubierto el algoritmo de filtro de Kalman básico, por lo tanto, en este ejercicio de codificación solo utilizaremos lecturas de láser del archivo de entrada indicado anteriormente. Una vez que cubramos “Filtro de Kalman ampliado” en publicaciones futuras, también comenzaremos a usar lecturas de Radar. Pero con nuestra comprensión actual de las ecuaciones del Filtro de Kalman, el solo uso de lecturas Láser servirá como un ejemplo perfecto para cimentar nuestro concepto con ayuda de la codificación.
Las lecturas del sensor capturadas en el archivo de texto de entrada están en el siguiente formato.
Para una fila que contiene datos de radar, las columnas son: sensor_type (R), rho_measured, phi_measured, rhodot_measured, timestamp, x_groundtruth, y_groundtruth, vx_groundtruth, vy_groundtruth, yaw_groundtruth, yawrate_groundtruth.
Para una fila que contiene datos lidar, las columnas son: sensor_type (L ), x_measured, y_measured, timestamp, x_groundtruth, y_groundtruth, vx_groundtruth, vy_groundtruth, yaw_groundtruth, yawrate_groundtruth.
Con toda esta información en la punta del dedo, comencemos a codificar sin más demoras. Al igual que con cualquier archivo de Python, importemos todas las bibliotecas requeridas primero
# ************** Importación de bibliotecas requeridas ************* import numpy as np importar pandas como pd de numpy.linalg import inv
A continuación, leer el archivo de texto de entrada que contiene las lecturas del sensor.
# ************* Declarar Variables ************************** #Read Input File mediciones = pd.read_csv (& # 039; obj_pose-laser -radar-synthetic-input.txt & # 039 ;, header = None, delim_whitespace = True, skiprows = 1)
He hecho el parámetro ‘skiprows’ como 1 porque, cuando comenzamos a implementar el algoritmo KF, no tenemos cualquier conocimiento previo sobre el estado del vehículo. En tales casos, usualmente establecemos nuestro estado por defecto a la primera lectura de la salida del sensor. Y esto es exactamente lo que haremos. En el código que se muestra a continuación, inicializaremos nuestro estado X con la lectura de la primera fila del archivo de entrada. Y como esta lectura ya se ha utilizado, simplemente la saltamos mientras leemos el archivo de entrada. Junto con los valores iniciales del vector de estado X también usaremos el primer sello de tiempo del archivo de entrada como nuestro tiempo anterior . Como se explicó en la publicación anterior, necesitamos marcas de tiempo actuales y anteriores para calcular delta_t . Las marcas de tiempo proporcionadas están en unidades microsegundos, que dividiremos por 10⁶. Esto tiene dos razones: primero, un número más pequeño es más fácil de mantener. Segundo, las lecturas terrestres de velocidad (y por lo tanto los valores de velocidad en nuestro código) están en unidades de segundos.
# Copie manualmente las lecturas iniciales de la primera fila del archivo de entrada.
prv_time = 1477010443000000 / 1000000.0
x = np.array ([
[0.312242]
[0.5803398]
[0]
[0]
])
A continuación, inicializamos las variables para almacenar la verdad sobre el terreno y los valores RMSE. RMSE (Root Mean Square Error) se usa para evaluar el rendimiento de nuestro algoritmo frente a los valores de verdad del terreno.
#Inicializar variables para almacenar la verdad del suelo y los valores RMSE ground_truth = np.zeros ([4, 1]) rmse = np.zeros ([4, 1])
Inicializamos la matriz P y A . Para obtener detalles sobre la estructura de P y A matrices, consulte Parte 1 donde se explica con más profundidad. Básicamente, la matriz A se usa para implementar ecuaciones cinemáticas de distancia, velocidad y tiempo, y la matriz P es la matriz de covarianza estatal que tiene varianzas en x, y, vx y vy como sus elementos diagonales. Lo que significan estos valores iniciales P es que tenemos una alta confianza en nuestros valores posicionales (lo cual tiene sentido según lo hemos tomado de las lecturas reales del sensor), indicado por un valor de varianza relativamente bajo y baja confianza en los valores de velocidad (lo que nuevamente tiene sentido ya que no tenemos idea de la velocidad), indicado por un valor de varianza relativamente grande.
#Inicializar las matrices P y A
P = np.array ([
[1, 0, 0, 0]
[0, 1, 0, 0]
[0, 0, 1000, 0]
[0, 0, 0, 1000]
])
A = np.array ([
[1.0, 0, 1.0, 0]
[0, 1.0, 0, 1.0]
[0, 0, 1.0, 0]
[0, 0, 0, 1.0]
])
A continuación definimos H y I matrices, que, como expliqué en la última publicación, serán 4 x 2 y 4 x 4 matrices, respectivamente. Definimos el vector Z que, como nuestras lecturas Lidar consistirán en 2 lecturas posicionales (x e y), será un vector 2 x 1.
H = np.array ([
[1.0, 0, 0, 0]
[0, 1.0, 0, 0]
])
I = np.identity (4)
z_lidar = np.zeros ([2, 1])
Definimos la matriz de covarianza de medición R que de nuevo, según la última publicación será una matriz de 2 x 2. Hablaremos más sobre cómo obtener valores para R matriz y noise_ax y noise_ay en futuros artículos.
R = np.array ([
[0.0225, 0]
[0, 0.0225]
])
A continuación definimos noise_ax noise_ay y matrix Q .
noise_ax = 5 noise_ay = 5 Q = np.zeros ([4, 4])
Tómense unos minutos para comprenderlos. Si revisamos las ecuaciones cinemáticas definidas en la primera parte de esta serie, puede ver que hay un factor de aceleración en los términos de posición y velocidad. Han sido reescritos aquí para mayor facilidad.
- Px (t + 1) = Px + delta_t * vx + 0.5 * ax * delta_t²
- Py (t + 1) = Py + delta_t * vy + 0.5 * ay * delta_t²
- Vx (t + 1) = Vx + ax * delta_t
- Vy (t + 1) = Vy + ay * delta_t
Como la aceleración es desconocida, podemos agregarla al componente de ruido, y este aleatorio el ruido se expresaría analíticamente como los últimos términos en la ecuación derivada anteriormente. Entonces, tenemos un vector de aceleración aleatorio v en esta forma, que se describe con una media cero y una matriz de covarianza P.
El vector v se puede descomponer en dos componentes, una matriz G de 4 por 2 que no contiene variables aleatorias y una matriz 2 por 1 a que contiene los componentes de aceleración aleatoria:
delta_t se calcula en cada iteración de Kalman Filter, y como no tenemos datos de aceleración, definimos acceleration a como vector aleatorio con cero media y desviaciones estándar noise_ax y noise_ay. En base a nuestro vector de ruido, podemos definir ahora la nueva matriz de covarianza Q. La matriz de covarianza se define como el valor esperado del vector de ruido v multiplicado por el vector de ruido v . Así que vamos a escribir esto:
Para saber más sobre ‘Valor esperado’, mira este video de Khan Academy. Como G no contiene variables aleatorias, podemos ponerlo fuera del cálculo de la expectativa
ax y ay se supone que son procesos de ruido no correlacionados. Esto significa que la covarianza sigma_axy en Q es cero.
Entonces, después de combinar todo en una matriz obtenemos nuestra matriz Q 4 por 4:
En cada iteración de Filtro de Kalman, calcularemos la matriz Q según la fórmula anterior . Con todas nuestras variables definidas, comencemos con la iteración a través de los datos del sensor y la aplicación de Kalman Filter en ellos. Ejecutar un bucle for hasta la longitud de las mediciones, leer la línea de medición, verificar si es una lectura Lidar (‘L’).
# ********************* * Iterar a través del ciclo principal ******************** # Comenzar a iterar a través de los datos del sensor para i dentro del rango (len (mediciones)): new_measurement = measurements.iloc [i, :] .values if new_measurement [0] == & # 039; L & # 039;:
Obtenga la marca de tiempo de la lectura actual, calcule el cambio en el tiempo comparándolo con la marca de tiempo anterior y luego reemplace la marca de tiempo actual como marca de tiempo anterior para la siguiente iteración.
#Calculate Timestamp y sus variables de potencia cur_time = new_measurement [3] /1000000.0 dt = cur_time - prv_time prv_time = cur_time
Calcular delta_t’s (‘dt’ en el código) square, cube, and 4th poder de delta_t que se requieren para calcular la matriz Q.
dt_2 = dt * dt dt_3 = dt_2 * dt dt_4 = dt_3 * dt
Actualizando la matriz A con el valor delta_t. Delta_t se multiplicará por velocidad para obtener valores posicionales.
#Updating matriz A con valor dt A [0][2] = dt A [1][3] = dt
Actualización de la matriz Q. Si miras hacia atrás a las ecuaciones derivadas anteriores para la matriz Q, puedes facilitar el corelado debajo de las líneas de código provistas con eso.
#Updating Q matrix Q [0][0] = dt_4 / 4 * noise_ax Q [0][2] = dt_3 / 2 * noise_ax Q [1][1] = dt_4 / 4 * noise_ay Q [1][3] = dt_3 / 2 * noise_ay Q [2][0] = dt_3 / 2 * noise_ax Q [2][2] = dt_2 * noise_ax Q [3][1] = dt_3 / 2 * noise_ay Q [3][3] = dt_2 * noise_ay
# Actualizaciones de las lecturas del sensor
z_lidar [0][0] = new_measurement [1]
z_lidar [1][0] = new_measurement [2]
#Recopilación de verdades sobre el terreno
ground_truth [0] = new_measurement [19659117] ground_truth [1] = new_measurement [5]
ground_truth [2] = new_measurement [6]
ground_truth [3] = new_measurement [7]
Y, por último, llamar a Predict and Update functions.
predicción () actualización (z_lidar)
Ahora echemos un vistazo a nuestra función de predicción () que sería muy similar a las siguientes ecuaciones de predicción que hemos estado usando en esta serie. No hay mucho que explicar en la sección de códigos, es realmente solo una réplica directa de fórmulas derivadas.
A. Predecir
a. X = A * X + B * u
b. P = A * P * AT * Q
# ********************** Definir funciones ************* **************** def predecir (): # Predecir el paso global x, P, Q x = np.matmul (A, x) At = np.transpose (A) P = np.add (np.matmul (A, np.matmul (P, At)), Q)
Avanzando para definir la actualización () función. Implementaremos los pasos de “medición” y “actualización” en esta función.
B. Medida
a. Y = Z – H * X
b. K = (P * HT) / ((H * P * HT) + R)
C. Actualización
a. X = X + K * Y
b. P = (I – K * H) * P
def actualización (z):
global x, P
# Paso de actualización de medición
Y = np.subtract (z_lidar, np.matmul (H , x))
Ht = np.transponer (H)
S = np.add (np.matmul (H, np.matmul (P, Ht)), R)
K = np.matmul (P, Ht)
Si = inv (S)
K = np.matmul (K, Si)
# Estado nuevo
x = np.add (x, np.matmul (K, Y))
P = np.matmul (np.subtract (I, np.matmul (K, H)), P)
… y con eso, ha pasado por el código completo de un algoritmo de filtro de Kalman. Aunque podría parecer un pequeño paso, este es el algoritmo fundamental para muchas de las versiones avanzadas utilizadas para la tecnología de fusión Sensor. Como se dijo anteriormente, todas las variantes de Kalman Filter constan de los mismos estados de Predicción, Medición y Actualización que hemos definido en esta serie hasta el momento. La única diferencia en las versiones más avanzadas son las diferentes ecuaciones cinemáticas y de sensores que utilizan. También los revisaremos paso a paso en esta serie. Pero en este momento, tengamos un máximo de cinco para completar nuestro paso básico de un clásico algoritmo de filtro de Kalman. Puede encontrar el código completo junto con el archivo de entrada en my github repo aquí .
Como de costumbre, si le gustó mi artículo, demuestre su aprecio con Me gusta y comentarios. También puede encontrarme a mí y a mis otros artículos en twitter
Hasta la próxima vez, ¡aplausos!