Filtro de Partículas - projeto do veículo Kidnapped

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

1. Definição de Filtro de Partículas

filtro Bayesian é um filtro de partículas ou implementado filtro de localização de Markov. filtro de partículas com base no "princípio da sobrevivência do mais apto" é usado principalmente para resolver o problema de localização. filtro de partículas tem a vantagem de flexibilidade e facilidade de programação.

Comparação do desempenho de três filtros:

Comparar	espaço de estado	Crença	Eficiência	em robótico
filtro de histograma	Discreto	Multi-modal	Exponencial	aproximado
filtro de Kalman	Contínuo	unimodal	Quadrático	aproximado
Filtro de partícula	Contínuo	Multi-modal	?	aproximado

Como você pode ver na imagem acima, o ponto vermelho é especulação discreto sobre a possível localização do robô. Cada um tem um ponto vermelho coordenada x, coordenada y, e direção. O filtro de partículas é uma fiabilidade posteriori robô traseira de milhares de tais especulações dos mesmos. Começando, as partículas estão uniformemente distribuídas, mas permite que o filtro é proporcional à razão da sua existência e consistência da partícula medido pelo sensor.

Peso (Peso):

O filtro de partículas tipicamente transporta um número de partículas discretas. É cada partícula compreendendo uma coordenada x, y-coordenam e de direcção do vetor. A sua sobrevivência depende da consistência das partículas com as medies de sensores. A consistência é baseada no grau de jogo entre a medição real e o previsto para a medição medida, que se refere ao peso desta correspondência.

medição de partícula médio de peso quão próximo as medições reais e preditos. No filtro de partículas, o peso das partículas, maior a probabilidade de sobrevivência. Em outras palavras, a probabilidade de sobrevivência eo direito de cada partícula é proporcional.

Reamostragem (reamostragem)

Reamostragem técnica é para novas partículas seleccionadas aleatoriamente a partir de partículas N do velho, substituído e re-escalados de acordo com os pesos de importância. Depois de reamostragem, os pesos partículas maiores podem ficar para baixo, outras partículas podem desaparecer. A probabilidade posterior de agregação das partículas nas zonas mais elevadas.

Para realizar reamostragem, usando uma roda de técnica de reamostragem.

Princípio: a probabilidade de cada partícula a ser seleccionado e as partículas são parte proporcional rodada do perímetro, as partículas de peso significativas têm mais possibilidade de ser seleccionado.

O índice inicial é 6, assumindo um beta aleatório = 0 + aleatório pesos> w6, o índice de + 1, beta = beta-w6. Neste caso beta <w7, No. 7 partículas são adicionadas ao armazém seleccionado. Após o próximo ciclo, quando o valor de beta e o índice de ciclo anterior ainda reservado, beta beta = + pesos aleatórios> W7 W8 +, por conseguinte, o índice é incrementado duas vezes e chegada índice = 1, desta vez W1> beta, w1 ele foi seleccionado para o armazém, seguido pelo próximo ciclo.

Resampling 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
```

implementação 2. Partículas Filtros

filtro de partículas tem quatro etapas principais:

Inicialização passos:

Inicialização passo: Nós estimamos a nossa posição a partir da entrada GPS. Próximas etapas do processo estará completo esta estimativa para localizar os nossos veículos
Prevendo passo: No passo de predição, que adicionada uma entrada de controlo (velocidade e taxa de guinada) de todas as partículas
Particle atualização peso passo: Na etapa de atualização, usamos a atualização de medição de posição token e partículas características Mapa de Heavy
Reamostragem passos de: durante a reamostragem, nós m vezes resample (m está na gama de 0 a length_of_particleArray) representada partículas i (i é um índice de partícula) é proporcional ao seu peso. Esta etapa usa uma tecnologia de roda de reamostragem.
Ela representa um novo filtro de partículas probabilidade posterior Bayesiano. Agora temos uma estimativa precisa da posição de entrada com base em veículo prova.

Pseudo-código:

Inicialização passos:

A primeira coisa que é para inicializar todo o filtro de partículas de partículas. Nesta etapa, temos de decidir quantas partículas que deseja usar. Em geral, temos que vir para cima com uma boa figura, porque se não for muito pequeno, o propenso a erros, se demasiado vai abrandar a velocidade do filtro Bayesian. A forma tradicional é a inicializar o espaço de estado de partículas é dividido em uma grade, e uma partícula colocado em cada célula, mas esta abordagem só é adequado para o pequeno espaço de estado, o espaço de estado, se a terra, não é apropriado. Assim, com entrada de posição GPS a nossa estimativa inicial da distribuição de partículas é o mais prático. É importante notar que os resultados da medição de todos os sensores devem ser acompanhados de ruído, a fim de simular as circunstâncias incontroláveis verdadeiro ruído, deve-se considerar para a posição inicial GPS e rumo deste projeto adicionar ruído Gaussian.

A inicialização final do projeto os passos cupom:

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;
}

passo Previsão:

Agora que já inicializada a partícula, é previsto quando a posição do veículo. Aqui, vamos usar a seguinte equação para prever o veículo próximo passo do tempo, e através da actualização da taxa de guinada com base na velocidade, tendo em conta o sensor Gaussian ruído.

A etapa final do projeto de prever cupom:

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);

    }

}

etapas de atualização:

Agora, temos medido a entrada de taxa de velocidade e de guinada em nosso filtro, é preciso atualizar o radar baseada no peso das partículas e leituras marcos peso LIDAR.

procedimento de atualização tem três etapas principais:

Transformação
Associação
Pesos de atualização

Conversão (Transformação)

Nós primeira necessidade para converter os dados medidos do carro de um carro de coordenadas local sistema de coordenadas no mapa.

Pela passagem de veículos sobre as coordenadas de observação (Xc e Yc), mapear as coordenadas de partículas (xp e YP) e a um ângulo de rotação (- 90 graus), a matriz de transformação homogénea, o valor observado do veículo sistema de coordenadas pode ser convertido para as coordenadas de mapa (xm e ym). A matriz de transformação homogénea, como mostrado, executa rotação e translação.

Os resultados de multiplicação de matrizes é:

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: a caixa preta é uma partícula, precisamos de actualizar os pesos (4,5) é a sua posição nas coordenadas do mapa, isto é posição (-90 graus), uma vez que os resultados de medição do sensor é baseado no veículo pontos de passagem coordenar-se, por isso, devemos observar os dados do veículo é convertido em coordenadas do mapa. L1 como sinais de coordenadas reais mapa (5,3), sensores de veículos medidos coordenadas veículo OBS2 (2,2), depois as coordenadas do mapa de uma transformação de matriz homogénea é (6,3), e agora podemos os resultados da medição de contato com os resultados reais em conjunto, combinando os marcos do mundo real, o direito de atualizar as caixas-pretas de partículas pesadas.
Contact (Association)

Contactar problema é no mundo real marcos correspondência medição e objeto, tais como mapas marcos. Nosso objetivo final é encontrar um parâmetro de peso para cada partícula, o parâmetro de peso representa a partícula eo grau de correspondência carro real no mesmo local.

agora foi convertido em observações mapa do espaço de coordenadas, o próximo passo é converter o valor de cada observação com um identificador para associar. No exercício mapa de cima, de um total de cinco pontos de referência, cada um dos quais é identificado como L1, L2, L3, L4, L5, cada um com uma posição do mapa conhecida. Precisamos converter cada TOBS1 observação, TOBS2, TOBS3 ligada a um destes cinco identificador. Para fazer isso, devemos ser os marcos mais próximos ligadas ao observar cada transformação.

TOBS1= (6,3), TOBS2= (2,2 &) e TOBS3= (0,5). OBS1 correspondente L1, L2 OBS2 correspondente, OBS3 correspondente L2 ou de L5 (a mesma distância).

O exemplo a seguir para explicar a questão dos dados associados.

Neste caso, temos duas medições Lidar de rock. Precisamos encontrar os dois valores medidos, que correspondem às rochas. Se estimarmos que qualquer medida para ser verdade, a posição do carro será baseado em nossa escolha de medição são diferentes. Em outras palavras, dependendo da escolha de sinais de trânsito, e, finalmente, determinar a localização do veículo será diferente.

Uma vez que temos várias medições de referência, podemos usar a técnica do vizinho mais próximo para encontrar o caminho certo.

Nesta abordagem, vamos medir o mais próximo como uma medida correta.

Perto da maior parte das vantagens e desvantagens de lei:

pesos de atualização (pesos de atualização)

Temos agora concluída a conversão e de medição associada, calculamos todas as partes do peso final de partículas necessários. O peso final da partícula será calculado para cada medição polihídrico - densidade de probabilidade Gaussiana do produto.

Pluralismo - densidade de probabilidade Gaussiana tem duas dimensões, x e y. Significativo multivariada Gaussian distribuição é medido em relação à posição do ponto de referência, o desvio padrão de distribuição gaussiana multivariada é determinada pela nossa incerteza inicial na x e y varia de descrever. Polihídrico - avaliação de Gauss com base na posição medido após a conversão. Multivariada Gaussiana fórmula distribuição como se segue.

Nota: x, y é o valor observado do mapa do sistema, de coordenadas [mu] x , μy últimos sinais de coordenadas do mapa. Quanto OBS2 (X, Y) = (2,2 &), ( [mu] x , μy ) = (2,1)

Erro fórmula:

Representativas em que x, y, teta melhor posição de partícula, X MEAS , Y MEAS , Theta MEAS representa o verdadeiro valor

O peso final do projeto para atualizar o 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;
            }
        }
    }
}

passo-a amostragem Re (passo Reamostrar)

Reamostragem técnica é uma nova partícula dos velhos partículas aleatórios, e a proporção de peso substituído de acordo com pesos de importância. Depois de reamostragem, os pesos partículas maiores podem ficar para baixo, outras partículas podem desaparecer. Este é o passo final do filtro de partículas.

O projeto final reamostragem cupom:

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. Projeto de Demonstração

a citar:

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

[Médio]: 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

Lançado sete artigos originais · ganhou elogios 3 · Visualizações 553

carta particular preocupações

tecnologia de posicionamento piloto automático - Prática filtro de partícula

Filtro de Partículas - projeto do veículo Kidnapped

1. Definição de Filtro de Partículas

implementação 2. Partículas Filtros

3. Projeto de Demonstração

Acho que você gosta