Filtro de partículas - proyecto de vehículo secuestrado

Github: https://github.com/williamhyin/CarND-Kidnapped-Vehicle

1. Definición de filtro de partículas

filtro Bayesiano es un filtro de partículas o un filtro localización Markov implementado. Filtro de partículas basado en el "principio de la supervivencia del más apto" se utiliza principalmente para resolver el problema de localización. Filtro de partículas tiene la ventaja de la flexibilidad y la facilidad de programación.

Comparación del rendimiento de tres filtros:

Comparar	Espacio de Estados	Creencia	Eficiencia	en robótica
filtro de histograma	Discreto	Multimodal	Exponencial	aproximado
filtro de Kalman	Continuo	unimodal	Cuadrático	aproximado
Filtro de partículas	Continuo	Multimodal	?	aproximado

Como se puede ver en la imagen superior, el punto rojo es la especulación discreta sobre la posible ubicación del robot. Cada uno tiene un punto rojo coordenada x, la coordenada y, y dirección. El filtro de partículas es un robot fiabilidad posteriori trasera de miles de tal especulación de los mismos. Comenzando, las partículas se distribuyen de manera uniforme, pero permite que el filtro es proporcional a la relación de su existencia y la consistencia de la partícula medido por el sensor.

Peso (Pesos):

El filtro de partículas típicamente lleva un número de partículas discretas. Es cada partícula comprende una coordenada x, y-de coordenadas y la dirección del vector. Su supervivencia depende de la consistencia de las partículas con las mediciones del sensor. La consistencia se basa en el grado de coincidencia entre la medición real y la medida predicho a medida, se refiere al peso de este juego.

Peso de medición de partícula medio de lo cerca las medidas reales y pronosticados. En el filtro de partículas, el peso de las partículas, mayor será la probabilidad de supervivencia. En otras palabras, la probabilidad de supervivencia y el derecho de cada partícula es proporcional.

Remuestreo (remuestreo)

Remuestreo técnica es para nuevas partículas seleccionadas al azar de las partículas de N en el viejo, reemplazado y re-escalado de acuerdo con los pesos de importancia. Después de remuestreo, las partículas de los pesos más grandes pueden permanecer abajo, otras partículas pueden desaparecer. La probabilidad posterior de la agregación de partículas en las zonas más altas.

Para llevar a cabo el remuestreo, usando una rueda técnica de remuestreo.

Principio: la probabilidad de cada partícula siendo seleccionado y las partículas son parte proporcional ronda del perímetro, las partículas de peso significativas tienen más probabilidad de ser seleccionado.

El índice inicial es 6, suponiendo una beta aleatoria = 0 + azar pesos> w6, el índice + 1, beta = beta-w6. En este caso beta <w7, No. 7 partículas se añaden al almacén seleccionado. Después de que el siguiente ciclo, cuando el valor de beta y el índice de ciclo anterior aún reservados, beta = beta + pesos aleatorios> w7 + w8, por lo tanto, el índice se incrementa dos veces y llegando index = 1, esta vez w1> beta, w1 fue seleccionado en el almacén, seguido por el siguiente ciclo.

Remuestreo código:
```
p3 = []
index= int(random.random()*N)
beta=0.0
mw=max(w)
for i in range(N):
    beta +=random.random()*2.0*mw
    while beta>w[index]:
        beta-=w[index]
        index=(index+1)%N
    p3.append(p[index])
p=p3
```

Aplicación 2. Filtros de partículas

Filtro de partículas tiene cuatro pasos principales:

Inicialización pasos:

Inicialización paso: Estimamos nuestra posición desde la entrada del GPS. Los próximos pasos en el proceso será completa esta estimación para localizar nuestros vehículos
La predicción de paso: En la etapa de predicción, añadimos una entrada de control (velocidad de guiñada y tasa) de todas las partículas
Particle actualización peso paso: En la etapa de actualización, se utiliza la actualización de medición de la posición de símbolo y de partículas características mapa de pesado
Remuestreo pasos de: durante el remuestreo, nosotros m veces de remuestreo (m está en el intervalo de 0 a length_of_particleArray) trazadas partículas i (i es un índice de partículas) es proporcional a su peso. Este paso utiliza una tecnología de rueda de remuestreo.
Representa un nuevo filtro de partículas posterior probabilidad Bayesiano. Ahora tenemos una estimación precisa de la posición de entrada de vehículos basado en la prueba.

Pseudo-código:

Inicialización pasos:

Lo primero es para inicializar todos los filtro de partículas de las partículas. En este paso, hay que decidir el número de partículas que desea utilizar. En general, tenemos que llegar a una cifra buena, porque si no es demasiado pequeña, la propenso a errores, si demasiado se ralentizará la velocidad del filtro bayesiano. La forma tradicional es inicializar el espacio de estados de partículas está dividido en una cuadrícula, y una partícula colocado en cada célula, pero este enfoque sólo es adecuado para el espacio de estado pequeño, el espacio de estado si la tierra, que no es apropiado. Así que con la posición GPS de entrada a nuestra estimación inicial de la distribución de partículas es la más práctica. Vale la pena señalar que los resultados de las mediciones de todos los sensores deben estar acompañados por el ruido, con el fin de simular las circunstancias incontrolables de ruido reales, se debe considerar a la posición inicial GPS y sentido de este proyecto añadir ruido gaussiano.

La inicialización final del proyecto los pasos del Código:

void ParticleFilter::init(double x, double y, double theta, double std[]) {
    /**
     * TODO: Set the number of particles. Initialize all particles to
     *   first position (based on estimates of x, y, theta and their uncertainties
     *   from GPS) and all weights to 1.
     * TODO: Add random Gaussian noise to each particle.
     * NOTE: Consult particle_filter.h for more information about this method
     *   (and others in this file).
     */

    if (is_initialized) {
        return;
    }
    num_particles = 100;  // TODO: Set the number of particle

    double std_x = std[0];
    double std_y = std[1];
    double std_theta = std[2];

    // Normal distributions
    normal_distribution<double> dist_x(x, std_x);
    normal_distribution<double> dist_y(y, std_y);
    normal_distribution<double> dist_theta(theta, std_theta);

    // Generate particles with normal distribution with mean on GPS values.
    for (int i = 0; i < num_particles; ++i) {
        Particle pe;
        pe.id = i;
        pe.x = dist_x(gen);
        pe.y = dist_y(gen);
        pe.theta = dist_theta(gen);
        pe.weight = 1.0;
        particles.push_back(pe);
        
    }
    is_initialized = true;
}

etapa de predicción:

Ahora que hemos inicializado la partícula, se predice cuando la posición del vehículo. A continuación, vamos a utilizar la siguiente ecuación para predecir el vehículo siguiente paso del tiempo, y mediante la actualización de la tasa de guiñada basado en la velocidad, teniendo en cuenta el sensor de Gauss ruido.

El paso final del proyecto de predecir Código:

void ParticleFilter::prediction(double delta_t, double std_pos[],
    double velocity, double yaw_rate) {
    /**
     * TODO: Add measurements to each particle and add random Gaussian noise.
     * NOTE: When adding noise you may find std::normal_distribution
     *   and std::default_random_engine useful.
     *  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
     *  http://www.cplusplus.com/reference/random/default_random_engine/
     */
    //Normal distributions for sensor noise
    normal_distribution<double> disX(0, std_pos[0]);
    normal_distribution<double> disY(0, std_pos[1]);
    normal_distribution<double> angle_theta(0, std_pos[2]);
    
    for (int i = 0; i < num_particles; i++) {
        if (fabs(yaw_rate) >= 0.00001) {
            particles[i].x  += (velocity / yaw_rate) * (sin(particles[i].theta + yaw_rate * delta_t) - sin(particles[i].theta));
            particles[i].y += (velocity / yaw_rate) * (cos(particles[i].theta) - cos(particles[i].theta + yaw_rate * delta_t));
            particles[i].theta += yaw_rate * delta_t;
        }
        else {
            particles[i].x += velocity * delta_t * cos(particles[i].theta);
            particles[i].y += velocity * delta_t * sin(particles[i].theta);
        }
        // Add noise
        particles[i].x += disX(gen);
        particles[i].y += disY(gen);
        particles[i].theta += angle_theta(gen);

    }

}

Actualizar pasos:

Ahora, hemos medido la entrada tasa de velocidad de guiñada y en nuestro filtro, hay que actualizar el radar basado en el peso de las partículas y las lecturas de puntos de referencia de peso lidar.

Procedimiento de actualización consta de tres pasos principales:

Transformación
Asociación
Los pesos de actualización

Conversión (transformación)

En primer lugar, la necesidad de convertir los datos medidos del vehículo contra un coche de coordenadas local del sistema coordenadas en el mapa.

Con la aprobación de vehículo a las coordenadas de observación (xc e yc), mapa las coordenadas de partículas (xp y YP) y nuestro ángulo de rotación (- 90 grados), la matriz de transformación homogénea, valor observado del vehículo sistema de coordenadas se pueden convertir a las coordenadas del mapa (xm y ym). La matriz de transformación homogénea, como se muestra, lleva a cabo la rotación y traslación.

Los resultados de la multiplicación de matrices es:

código:
```
 double x_part, y_part, x_obs, y_obs, theta;
  double x_map;
 x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);
  double y_map;
 y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);
```
Nota: el cuadro negro es una partícula, tenemos que actualizar sus pesos (4,5) es su posición en las coordenadas del mapa, se dirige (-90 grados), ya que los resultados de medición del sensor se basa en puntos de paso de vehículos coordinarse, por lo que debemos observar los datos del vehículo se convierte en coordenadas del mapa. L1 como signos de coordenadas reales de mapa (5,3), los sensores del vehículo medidos coordenadas de vehículos OBS2 (2,2), después de que las coordenadas del mapa de una transformación de matriz homogénea es (6,3), y ahora podemos los resultados de las mediciones de contacto con los resultados reales juntos, que coinciden con los puntos de referencia en el mundo real, el derecho a actualizar las cajas negras de partículas pesadas.
Contacto (Asociación)

Contacto problema es en la medición y el objeto del mundo real hitos a juego, tales como mapas de puntos de referencia. Nuestro objetivo final es encontrar un parámetro de peso para cada partícula, el parámetro de peso representa la partícula y el grado de emparejamiento coche real en el mismo lugar.

Ahora se ha convertido en observaciones El mapa de un espacio de coordenadas, el siguiente paso es convertir el valor de cada observación con un identificador asociado a. En el mapa ejercicio anterior, un total de cinco puntos de referencia, cada uno de los cuales se identifican como L1, L2, L3, L4, L5, cada uno con una posición del mapa conocida. Tenemos que convertir cada TOBS1 observación, TOBS2, TOBS3 vinculada a uno de estos cinco identificador. Para ello, debemos ser los puntos de referencia más cercanos vinculados a observar cada transformación.

TOBS1= (6,3), TOBS2= (2,2 y) y TOBS3= (0,5). OBS1 coincidente L1, OBS2 coincidente L2, OBS3 coincidencia de L2 o L5 (la misma distancia).

En el siguiente ejemplo para explicar la cuestión de los datos asociados a ella.

En este caso, tenemos dos mediciones lidar de roca. Tenemos que encontrar los dos valores medidos, que corresponden a las rocas. Si calculamos que cualquier medida que sea cierto, la posición del coche se basa en nuestra elección de medición son diferentes. En otras palabras, dependiendo de la elección de las señales de tráfico, y en última instancia determinar la ubicación del vehículo será diferente.

Ya que tenemos múltiples mediciones de puntos de referencia, podemos utilizar la técnica del vecino más cercano para encontrar la correcta.

En este enfoque, mediremos el más cercano como una medición correcta.

Cerca de la mayoría de las ventajas y desventajas de la ley:

Actualizar pesos (pesos de actualización)

Hemos completado la conversión y medición asociada, hemos calculado todas las partes del peso final de las partículas requeridas. El peso final de la partícula se calcula para cada polihídrico medición - de densidad de probabilidad gaussiana del producto.

Pluralidad - densidad de probabilidad gaussiana tiene dos dimensiones, x e y. Mean multivariante Gaussian distribución se mide en relación con la posición del punto de referencia, la desviación estándar distribución gaussiana multivariante se determina por nuestra incertidumbre inicial en la x y rangos Y de describir. Polihídrico - evaluación de Gauss sobre la base de la posición medida después de la conversión. fórmula de distribución multivariante gaussiana como sigue.

Nota: x, y es el valor observado del mapa del sistema, coordinar [mu] x , μy recientes signos de las coordenadas del mapa. En cuanto a OBS2 (X, Y) = (2,2 y), ( [mu] x , μy ) = (2,1)

fórmula de error:

Representante en el que x, y, theta posición de la partícula mejor, X MED , Y MEDIR , theta MEDIR representan el valor verdadero

El peso final del proyecto para actualizar el código:

void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
                                   const vector<LandmarkObs> &observations,
                                   const Map &map_landmarks) {
    /**
     * TODO: Update the weights of each particle using a mult-variate Gaussian
     *   distribution. You can read more about this distribution here:
     *   https://en.wikipedia.org/wiki/Multivariate_normal_distribution
     * NOTE: The observations are given in the VEHICLE'S coordinate system.
     *   Your particles are located according to the MAP'S coordinate system.
     *   You will need to transform between the two systems. Keep in mind that
     *   this transformation requires both rotation AND translation (but no scaling).
     *   The following is a good resource for the theory:
     *   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
     *   and the following is a good resource for the actual equation to implement
     *   (look at equation 3.33) http://planning.cs.uiuc.edu/node99.html
     */
    double stdRange = std_landmark[0];
    double stdBearing = std_landmark[1];
    // Each particle for loop
    for (int i = 0; i < num_particles; i++) {
        double x = particles[i].x;
        double y = particles[i].y;
        double theta = particles[i].theta;
//        double sensor_range_2 = sensor_range * sensor_range;
        // Create a vector to hold the map landmark locations predicted to be within sensor range of the particle
        vector<LandmarkObs> validLandmarks;
        // Each map landmark for loop
        for (unsigned int j = 0; j < map_landmarks.landmark_list.size(); j++) {
            float landmarkX = map_landmarks.landmark_list[j].x_f;
            float landmarkY = map_landmarks.landmark_list[j].y_f;
            int id = map_landmarks.landmark_list[j].id_i;
            double dX = x - landmarkX;
            double dY = y - landmarkY;
            /*if (dX * dX + dY * dY <= sensor_range_2) {
                inRangeLandmarks.push_back(LandmarkObs{ id,landmarkX,landmarkY });
            }*/
            // Only consider landmarks within sensor range of the particle (rather than using the "dist" method considering a circular region around the particle, this considers a rectangular region but is computationally faster)
            if (fabs(dX) <= sensor_range && fabs(dY) <= sensor_range) {
                validLandmarks.push_back(LandmarkObs{id, landmarkX, landmarkY});
            }
        }
        // Create and populate a copy of the list of observations transformed from vehicle coordinates to map coordinates
        vector<LandmarkObs> transObs;
        for (int j = 0; j < observations.size(); j++) {
            double tx = x + cos(theta) * observations[j].x - sin(theta) * observations[j].y;
            double ty = y + sin(theta) * observations[j].x + cos(theta) * observations[j].y;
            transObs.push_back(LandmarkObs{observations[j].id, tx, ty});
        }

        // Data association for the predictions and transformed observations on current particle
        dataAssociation(validLandmarks, transObs);
        particles[i].weight = 1.0;

        for (unsigned int j = 0; j < transObs.size(); j++) {
            double observationX = transObs[j].x;
            double observationY = transObs[j].y;
            int landmarkId = transObs[j].id;

            double landmarkX, landmarkY;
            int k = 0;
            int nlandmarks = validLandmarks.size();
            bool found = false;
            // x,y coordinates of the prediction associated with the current observation
            while (!found && k < nlandmarks) {
                if (validLandmarks[k].id == landmarkId) {
                    found = true;
                    landmarkX = validLandmarks[k].x;
                    landmarkY = validLandmarks[k].y;
                }
                k++;
            }
            // Weight for this observation with multivariate Gaussian
            double dX = observationX - landmarkX;
            double dY = observationY - landmarkY;
            double weight = (1 / (2 * M_PI * stdRange * stdBearing)) *
                            exp(-(dX * dX / (2 * stdRange * stdRange) + (dY * dY / (2 * stdBearing * stdBearing))));
            // Product of this obersvation weight with total observations weight
            if (weight == 0) {
                particles[i].weight = particles[i].weight * 0.00001;

            } else {
                particles[i].weight = particles[i].weight * weight;
            }
        }
    }
}

paso Re-muestreo (etapa Resample)

Remuestreo técnica es una nueva partícula de las partículas aleatorias de edad, y la proporción en peso sustituye de acuerdo con pesos de importancia. Después de remuestreo, las partículas de los pesos más grandes pueden permanecer abajo, otras partículas pueden desaparecer. Este es el paso final en el filtro de partículas.

El proyecto final remuestreo Código:

void ParticleFilter::resample() {
    /**
        * TODO: Resample particles with replacement with probability proportional
        *   to their weight.
        * NOTE: You may find std::discrete_distribution helpful here.
        *   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution
        */
    // Get weights and max weight.
    vector<double> weights;
    double maxWeight = numeric_limits<double>::min();
    for (int i = 0; i < num_particles; i++) {
        weights.push_back(particles[i].weight);
        if (particles[i].weight > maxWeight) {
            maxWeight = particles[i].weight;
        }
    }
    uniform_real_distribution<float> dist_float(0.0, maxWeight);
    uniform_real_distribution<float> dist_int(0.0, num_particles - 1);
    int index = dist_int(gen);
    double beta = 0.0;
    vector<Particle> resampledParticles;
    for (int i = 0; i < num_particles; i++) {
        beta += dist_float(gen) * 2.0;
        while (beta > weights[index]) {
            beta -= weights[index];
            index = (index + 1) % num_particles;
        }
        resampledParticles.push_back(particles[index]);
    }
    particles = resampledParticles;

}

3. Proyecto de Demostración

citar:

[Udacity]: www.udacity.com "Udacity"

[Medio]: https://medium.com/intro-to-artificial-intelligence/kidnapped-vehicle-project-using-particle-filters-udacitys-self-driving-car-nanodegree-aa1d37c40d49 "Dhanoop Karunakaran"

Linkedin: https://linkedin.com/in/williamhyin

williamhyin

Publicado siete artículos originales · ganado elogios 3 · Vistas 553

carta privada preocupaciones

tecnología de posicionamiento Piloto automático - partícula Practice filtro

Filtro de partículas - proyecto de vehículo secuestrado

1. Definición de filtro de partículas

Aplicación 2. Filtros de partículas

3. Proyecto de Demostración

Supongo que te gusta