39. Coding the Full Filter
In previous lessons we learned the basis of our filter, tried some example calculations by hand, and implemented critical steps and models for a single time step and vector of sensor observations. In this final coding exercise we will implement the entire filter using the pieces we have already developed for multiple time steps and sensor observations.
Sensor observations are provided in a 2D vector where each inner vector represents the sensor observations, in meters, at a time step.
{{1,7,12,21}, {0,6,11,20}, {5,10,19}, {4,9,18}, {3,8,17}, {2,7,16},
{1,6,15}, {0,5,14}, {4,13}, {3,12},{2,11},{1,10},{0,9},{8},{7},{6},{5},
{4},{3},{2},{1},{0}, {}, {}, {}};
Implement the Bayes' localization filter by first initializing priors, then doing the following within each time step:
-
extract sensor observations
-
for each pseudo-position:
- get the motion model probability
- determine pseudo ranges
- get the observation model probability
- use the motion and observation model probabilities to calculate the posterior probability
-
normalize posteriors (see helpers.h for a normalization function)
-
update priors (priors --> posteriors)
-
All tasks are within the main function and are labeled as
TODO
.
Troubleshooting:
To help troubleshoot print statements have been placed throughout the code below and commented out. Uncommenting these statements will help to follow each step of the filter.
Start Quiz:
#include <algorithm>
#include <iostream>
#include <vector>
#include "helpers.h"
using std::vector;
using std::cout;
using std::endl;
vector<float> initialize_priors(int map_size, vector<float> landmark_positions,
float position_stdev);
float motion_model(float pseudo_position, float movement, vector<float> priors,
int map_size, int control_stdev);
// function to get pseudo ranges
vector<float> pseudo_range_estimator(vector<float> landmark_positions,
float pseudo_position);
// observation model: calculate likelihood prob term based on landmark proximity
float observation_model(vector<float> landmark_positions,
vector<float> observations, vector<float> pseudo_ranges,
float distance_max, float observation_stdev);
int main() {
// set standard deviation of control
float control_stdev = 1.0f;
// set standard deviation of position
float position_stdev = 1.0f;
// meters vehicle moves per time step
float movement_per_timestep = 1.0f;
// set observation standard deviation
float observation_stdev = 1.0f;
// number of x positions on map
int map_size = 25;
// set distance max
float distance_max = map_size;
// define landmarks
vector<float> landmark_positions {3, 9, 14, 23};
// define observations vector, each inner vector represents a set
// of observations for a time step
vector<vector<float> > sensor_obs {{1,7,12,21}, {0,6,11,20}, {5,10,19},
{4,9,18}, {3,8,17}, {2,7,16}, {1,6,15},
{0,5,14}, {4,13}, {3,12}, {2,11}, {1,10},
{0,9}, {8}, {7}, {6}, {5}, {4}, {3}, {2},
{1}, {0}, {}, {}, {}};
/**
* TODO: initialize priors
*/
// UNCOMMENT TO SEE THIS STEP OF THE FILTER
//cout << "-----------PRIORS INIT--------------" << endl;
//for (int p = 0; p < priors.size(); ++p){
// cout << priors[p] << endl;
//}
// initialize posteriors
vector<float> posteriors(map_size, 0.0);
// specify time steps
int time_steps = sensor_obs.size();
// declare observations vector
vector<float> observations;
// cycle through time steps
for (int t = 0; t < time_steps; ++t) {
// UNCOMMENT TO SEE THIS STEP OF THE FILTER
//cout << "---------------TIME STEP---------------" << endl;
//cout << "t = " << t << endl;
//cout << "-----Motion----------OBS----------------PRODUCT--" << endl;
if (!sensor_obs[t].empty()) {
observations = sensor_obs[t];
} else {
observations = {float(distance_max)};
}
// step through each pseudo position x (i)
for (unsigned int i = 0; i < map_size; ++i) {
float pseudo_position = float(i);
/**
* TODO: get the motion model probability for each x position
*/
/**
* TODO: get pseudo ranges
*/
/**
* TODO: get observation probability
*/
/**
* TODO: calculate the ith posterior and pass to posteriors vector
*/
// UNCOMMENT TO SEE THIS STEP OF THE FILTER
//cout << motion_prob << "\t" << observation_prob << "\t"
// << "\t" << motion_prob * observation_prob << endl;
}
// UNCOMMENT TO SEE THIS STEP OF THE FILTER
//cout << "----------RAW---------------" << endl;
//for (int p = 0; p < posteriors.size(); ++p) {
// cout << posteriors[p] << endl;
//}
/**
* TODO: normalize posteriors (see helpers.h for a helper function)
*/
// print to stdout
//cout << posteriors[t] << "\t" << priors[t] << endl;
// UNCOMMENT TO SEE THIS STEP OF THE FILTER
//cout << "----------NORMALIZED---------------" << endl;
/**
* TODO: update priors
*/
// UNCOMMENT TO SEE THIS STEP OF THE FILTER
//for (int p = 0; p < posteriors.size(); ++p) {
// cout << posteriors[p] << endl;
//}
// print posteriors vectors to stdout
for (int p = 0; p < posteriors.size(); ++p) {
cout << posteriors[p] << endl;
}
}
return 0;
}
// observation model: calculate likelihood prob term based on landmark proximity
float observation_model(vector<float> landmark_positions,
vector<float> observations, vector<float> pseudo_ranges,
float distance_max, float observation_stdev) {
// initialize observation probability
float distance_prob = 1.0f;
// run over current observation vector
for (int z=0; z< observations.size(); ++z) {
// define min distance
float pseudo_range_min;
// check, if distance vector exists
if (pseudo_ranges.size() > 0) {
// set min distance
pseudo_range_min = pseudo_ranges[0];
// remove this entry from pseudo_ranges-vector
pseudo_ranges.erase(pseudo_ranges.begin());
} else { // no or negative distances: set min distance to a large number
pseudo_range_min = std::numeric_limits<const float>::infinity();
}
// estimate the probability for observation model, this is our likelihood
distance_prob *= Helpers::normpdf(observations[z], pseudo_range_min,
observation_stdev);
}
return distance_prob;
}
vector<float> pseudo_range_estimator(vector<float> landmark_positions,
float pseudo_position) {
// define pseudo observation vector
vector<float> pseudo_ranges;
// loop over number of landmarks and estimate pseudo ranges
for (int l=0; l< landmark_positions.size(); ++l) {
// estimate pseudo range for each single landmark
// and the current state position pose_i:
float range_l = landmark_positions[l] - pseudo_position;
// check if distances are positive:
if (range_l > 0.0f) {
pseudo_ranges.push_back(range_l);
}
}
// sort pseudo range vector
sort(pseudo_ranges.begin(), pseudo_ranges.end());
return pseudo_ranges;
}
// motion model: calculates prob of being at an estimated position at time t
float motion_model(float pseudo_position, float movement, vector<float> priors,
int map_size, int control_stdev) {
// initialize probability
float position_prob = 0.0f;
// loop over state space for all possible positions x (convolution):
for (float j=0; j< map_size; ++j) {
float next_pseudo_position = j;
// distance from i to j
float distance_ij = pseudo_position-next_pseudo_position;
// transition probabilities:
float transition_prob = Helpers::normpdf(distance_ij, movement,
control_stdev);
// estimate probability for the motion model, this is our prior
position_prob += transition_prob*priors[j];
}
return position_prob;
}
// initialize priors assuming vehicle at landmark +/- 1.0 meters position stdev
vector<float> initialize_priors(int map_size, vector<float> landmark_positions,
float position_stdev) {
// set all priors to 0.0
vector<float> priors(map_size, 0.0);
// set each landmark positon +/-1 to 1.0/9.0 (9 possible postions)
float norm_term = landmark_positions.size() * (position_stdev * 2 + 1);
for (int i=0; i < landmark_positions.size(); ++i) {
for (float j=1; j <= position_stdev; ++j) {
priors.at(int(j+landmark_positions[i]+map_size)%map_size) += 1.0/norm_term;
priors.at(int(-j+landmark_positions[i]+map_size)%map_size) += 1.0/norm_term;
}
priors.at(landmark_positions[i]) += 1.0/norm_term;
}
return priors;
}
//=================================================================================
// Name : help_functions.h
// Version : 2.0.0
// Copyright : Udacity
//=================================================================================
#ifndef HELP_FUNCTIONS_H_
#define HELP_FUNCTIONS_H_
#include <math.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <iomanip>
using namespace std;
class Helpers {
public:
//definition of one over square root of 2*pi:
constexpr static float STATIC_ONE_OVER_SQRT_2PI = 1/sqrt(2*M_PI) ;
float ONE_OVER_SQRT_2PI = 1/sqrt(2*M_PI) ;
/*****************************************************************************
* normpdf(X,mu,sigma) computes the probability function at values x using the
* normal distribution with mean mu and standard deviation std. x, mue and
* sigma must be scalar! The parameter std must be positive.
* The normal pdf is y=f(x;mu,std)= 1/(std*sqrt(2pi)) e[ -(x−mu)^2 / 2*std^2 ]
*****************************************************************************/
static float normpdf(float x, float mu, float std) {
return (STATIC_ONE_OVER_SQRT_2PI/std)*exp(-0.5*pow((x-mu)/std,2));
}
//static function to normalize a vector:
static std::vector<float> normalize_vector(std::vector<float> inputVector){
//declare sum:
float sum = 0.0f;
//declare and resize output vector:
std::vector<float> outputVector ;
outputVector.resize(inputVector.size());
//estimate the sum:
for (unsigned int i = 0; i < inputVector.size(); ++i) {
sum += inputVector[i];
}
//normalize with sum:
for (unsigned int i = 0; i < inputVector.size(); ++i) {
outputVector[i] = inputVector[i]/sum;
}
//return normalized vector:
return outputVector;
}
};
#endif /* HELP_FUNCTIONS_H_ */