In the 6’th project from the Self-Driving Car engineer program designed by Udacity, we will utilize an Extended Kalman Filter to estimate the state of a moving object of interest with noisy LIDAR and Radar measurements.
This post builds up starting with a very simple Kalman Filter implementation for the 1D motion smoothing, to a complex object motion tracking in 2D by fusing noisy measurements from LIDAR and Radar sensors:
- A minimal implementation of the Kalman Filter in Python for the simplest 1D motion model
- A formal implementation of the Kalman Filter in Python using state and covariance matrices for the simplest 1D motion model
- A formal implementation of the Kalman Filter in C++ and Eigen using state and covariance matrices for the simplest 1D motion model (same as above)
- Object motion tracking in 2D by fusing noisy measurements from LIDAR and Radar sensors
Accurate motion tracking is a critical component in any robotics application. Tracking systems suffer from noise and small distortions causing incorrect viewing perspectives.
However, most robotic motions also contain nonlinearity requiring a modification to the KF.
The extended Kalman filter (EKF) provides this modification by linearizing all nonlinear models (i.e., process and measurement models) so the traditional KF can be applied.
Unfortunately, the EKF has two important potential drawbacks. First, the derivation of the Jacobian matrices, the linear approximates to the nonlinear functions, can be complex causing implementation difficulties. Second, these linearizations can lead to instability if the time-step intervals are not sufficiently small.
A minimal implementation of the Kalman Filter in python for the simplest 1D motion model
The code bellow implements the simplest Kalman filter for estimating the motion in 1D. The input are the noisy 1D position measurements and the sigmas for weighting during the update and predict steps.
The output are estimated position (mu) and its trust (sigma) at a certain time.
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 |
# Write a program that will iteratively update and # predict based on the location measurements # and inferred motions shown below. def update(mean1, var1, mean2, var2): new_mean = float(var2 * mean1 + var1 * mean2) / (var1 + var2) new_var = 1./(1./var1 + 1./var2) return [new_mean, new_var] def predict(mean1, var1, mean2, var2): new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var] measurements = [5., 6., 7., 9., 10.] motion = [1., 1., 2., 1., 1.] measurement_sig = 4. motion_sig = 2. mu = 0. sig = 10000. #Please print out ONLY the final values of the mean #and the variance in a list [mu, sig]. # Insert code here for n in range(len(measurements)): [mu,sig] = update(mu, sig, measurements[n], measurement_sig) print 'update: ', (mu,sig) [mu,sig] = predict(mu, sig, motion[n], motion_sig) print 'predict: ', (mu,sig) print [mu, sig] |
1 2 3 4 5 6 7 8 9 10 11 |
update: (4.998000799680128, 3.9984006397441023) predict: (5.998000799680128, 5.998400639744102) update: (5.999200191953932, 2.399744061425258) predict: (6.999200191953932, 4.399744061425258) update: (6.999619127420922, 2.0951800575117594) predict: (8.999619127420921, 4.09518005751176) update: (8.999811802788143, 2.0235152416216957) predict: (9.999811802788143, 4.023515241621696) update: (9.999906177177365, 2.0058615808441944) predict: (10.999906177177365, 4.005861580844194) [10.999906177177365, 4.005861580844194] |
The initial position is set to 0 and its sigma to 10000 (very high uncertainty), and after only a few steps the position gets close to the measurement and with a smaller sigma (smaller uncertainty).
A formal implementation of the Kalman Filter in Python using state and covariance matrices for the simplest 1D motion model
The code bellow implements a multi-dimensional Kalman filter for estimating the motion in 1D, with the state defined by position and velocity.
The input is defined by the initial state x (position and velocity) both set to 0. For estimating the state at a later time the state transition matrix F is used, matrix which embeds the motion model in 1D:
x_k+1 = x_k + velocity * dt
When predicting in future the uncertainty increases and decreases when new measurements are available.
The output is defined by the state x at a certain time, and its uncertainty matrix P.
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 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 |
# Write a function 'kalman_filter' that implements a multi- # dimensional Kalman Filter for the example given from math import * class matrix: # implements basic operations of a matrix class def __init__(self, value): self.value = value self.dimx = len(value) self.dimy = len(value[0]) if value == [[]]: self.dimx = 0 def zero(self, dimx, dimy): # check if valid dimensions if dimx < 1 or dimy < 1: raise ValueError, "Invalid size of matrix" else: self.dimx = dimx self.dimy = dimy self.value = [[0 for row in range(dimy)] for col in range(dimx)] def identity(self, dim): # check if valid dimension if dim < 1: raise ValueError, "Invalid size of matrix" else: self.dimx = dim self.dimy = dim self.value = [[0 for row in range(dim)] for col in range(dim)] for i in range(dim): self.value[i][i] = 1 def show(self): for i in range(self.dimx): print self.value[i] print ' ' def __add__(self, other): # check if correct dimensions if self.dimx != other.dimx or self.dimy != other.dimy: raise ValueError, "Matrices must be of equal dimensions to add" else: # add if correct dimensions res = matrix([[]]) res.zero(self.dimx, self.dimy) for i in range(self.dimx): for j in range(self.dimy): res.value[i][j] = self.value[i][j] + other.value[i][j] return res def __sub__(self, other): # check if correct dimensions if self.dimx != other.dimx or self.dimy != other.dimy: raise ValueError, "Matrices must be of equal dimensions to subtract" else: # subtract if correct dimensions res = matrix([[]]) res.zero(self.dimx, self.dimy) for i in range(self.dimx): for j in range(self.dimy): res.value[i][j] = self.value[i][j] - other.value[i][j] return res def __mul__(self, other): # check if correct dimensions if self.dimy != other.dimx: raise ValueError, "Matrices must be m*n and n*p to multiply" else: # subtract if correct dimensions res = matrix([[]]) res.zero(self.dimx, other.dimy) for i in range(self.dimx): for j in range(other.dimy): for k in range(self.dimy): res.value[i][j] += self.value[i][k] * other.value[k][j] return res def transpose(self): # compute transpose res = matrix([[]]) res.zero(self.dimy, self.dimx) for i in range(self.dimx): for j in range(self.dimy): res.value[j][i] = self.value[i][j] return res # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions def Cholesky(self, ztol=1.0e-5): # Computes the upper triangular Cholesky factorization of # a positive definite matrix. res = matrix([[]]) res.zero(self.dimx, self.dimx) for i in range(self.dimx): S = sum([(res.value[k][i])**2 for k in range(i)]) d = self.value[i][i] - S if abs(d) < ztol: res.value[i][i] = 0.0 else: if d < 0.0: raise ValueError, "Matrix not positive-definite" res.value[i][i] = sqrt(d) for j in range(i+1, self.dimx): S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)]) if abs(S) < ztol: S = 0.0 res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] return res def CholeskyInverse(self): # Computes inverse of matrix given its Cholesky upper Triangular # decomposition of matrix. res = matrix([[]]) res.zero(self.dimx, self.dimx) # Backward step for inverse. for j in reversed(range(self.dimx)): tjj = self.value[j][j] S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) res.value[j][j] = 1.0/tjj**2 - S/tjj for i in reversed(range(j)): res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i] return res def inverse(self): aux = self.Cholesky() res = aux.CholeskyInverse() return res def __repr__(self): return repr(self.value) ######################################## # Implement the filter function below def kalman_filter(x, P): for n in range(len(measurements)): # measurement update Z = matrix([[measurements[n]]]) y = Z - (H * x) S = H * P * H.transpose() + R K = P * H.transpose() * S.inverse() x = x + (K * y) P = (I - (K * H)) * P # prediction x = (F * x) + u P = F * P * F.transpose() return x,P ############################################ ### use the code below to test your filter! ############################################ measurements = [1, 2, 3] x = matrix([[0.], [0.]]) # initial state (location and velocity) P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty u = matrix([[0.], [0.]]) # external motion F = matrix([[1., 1.], [0, 1.]]) # next state function H = matrix([[1., 0.]]) # measurement function R = matrix([[1.]]) # measurement uncertainty I = matrix([[1., 0.], [0., 1.]]) # identity matrix print kalman_filter(x, P) # output should be: # x: [[3.9996664447958645], [0.9999998335552873]] # P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]] |
1 |
([[3.9996664447958645], [0.9999998335552873]], [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]]) |
A formal implementation of the Kalman Filter in C++ and Eigen using state and covariance matrices for the simplest 1D motion model
The code bellow implements the same multi-dimensional Kalman filter as in the example before for estimating the motion in 1D, with the state defined by position and velocity.
The input is defined by the initial state x (position and velocity) both set to 0. For estimating the state at a later time the state transition matrix F is used, matrix which embeds the motion model in 1D:
x_k+1 = x_k + velocity * dt
When predicting in future the uncertainty increases and decreases when new measurements are available.
The output is defined by the state x at a certain time, and its uncertainty matrix P.
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 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 |
// Write a function 'filter()' that implements a multi- // dimensional Kalman Filter for the example given //============================================================================ #include <iostream> #include "Dense" #include <vector> using namespace std; using namespace Eigen; //Kalman Filter variables VectorXd x; // object state MatrixXd P; // object covariance matrix VectorXd u; // external motion MatrixXd F; // state transition matrix MatrixXd H; // measurement matrix MatrixXd R; // measurement covariance matrix MatrixXd I; // Identity matrix MatrixXd Q; // process covariance matrix vector<VectorXd> measurements; void filter(VectorXd &x, MatrixXd &P); int main() { /** * Code used as example to work with Eigen matrices */ // //you can create a vertical vector of two elements with a command like this // VectorXd my_vector(2); // //you can use the so called comma initializer to set all the coefficients to some values // my_vector << 10, 20; // // //and you can use the cout command to print out the vector // cout << my_vector << endl; // // //the matrices can be created in the same way. // //For example, This is an initialization of a 2 by 2 matrix // //with the values 1, 2, 3, and 4 // MatrixXd my_matrix(2,2); // my_matrix << 1, 2, // 3, 4; // cout << my_matrix << endl; // // //you can use the same comma initializer or you can set each matrix value explicitly // // For example that's how we can change the matrix elements in the second row // my_matrix(1,0) = 11; //second row, first column // my_matrix(1,1) = 12; //second row, second column // cout << my_matrix << endl; // // //Also, you can compute the transpose of a matrix with the following command // MatrixXd my_matrix_t = my_matrix.transpose(); // cout << my_matrix_t << endl; // // //And here is how you can get the matrix inverse // MatrixXd my_matrix_i = my_matrix.inverse(); // cout << my_matrix_i << endl; // // //For multiplying the matrix m with the vector b you can write this in one line as let’s say matrix c equals m times v. // // // MatrixXd another_matrix; // another_matrix = my_matrix*my_vector; // cout << another_matrix << endl; //design the KF with 1D motion x = VectorXd(2); x << 0, 0; P = MatrixXd(2, 2); P << 1000, 0, 0, 1000; u = VectorXd(2); u << 0, 0; F = MatrixXd(2, 2); F << 1, 1, 0, 1; H = MatrixXd(1, 2); H << 1, 0; R = MatrixXd(1, 1); R << 1; I = MatrixXd::Identity(2, 2); Q = MatrixXd(2, 2); Q << 0, 0, 0, 0; //create a list of measurements VectorXd single_meas(1); single_meas << 1; measurements.push_back(single_meas); single_meas << 2; measurements.push_back(single_meas); single_meas << 3; measurements.push_back(single_meas); //call Kalman filter algorithm filter(x, P); return 0; } void filter(VectorXd &x, MatrixXd &P) { for (unsigned int n = 0; n < measurements.size(); ++n) { VectorXd z = measurements[n]; //YOUR CODE HERE /* * KF Measurement update step */ VectorXd y = z - H * x; MatrixXd Ht = H.transpose(); MatrixXd S = H * P * Ht + R; MatrixXd Si = S.inverse(); MatrixXd K = P * Ht * Si; //new state x = x + (K * y); P = (I - K * H) * P; /* * KF Prediction step */ x = F * x + u; MatrixXd Ft = F.transpose(); P = F * P * Ft + Q; std::cout << "x=" << std::endl << x << std::endl; std::cout << "P=" << std::endl << P << std::endl; } } |
1 2 3 4 5 6 7 8 9 10 11 |
x= 0.999001 0 P= 1001 1000 1000 1000 x= 2.998 0.999002 P= 4.99002 2.99302 2.99302 1.99501 x= 3.99967 1 P= 2.33189 0.999168 0.999168 0.499501 |
Object motion tracking in 2D by fusing noisy measurements from LIDAR and Radar sensors
This part uses noisy measurements from Lidar and Radar sensors and the Extended Kalman Filter to estimate the 2D motion of bicycles. The source code can be found here.
The state has four elements: position in x and y, and the velocity in x and y.
The linear motion model in the matrix form:
Motion noise and process noise refer to the same case: uncertainty in the object’s position when predicting location. The model assumes velocity is constant between time intervals, but in reality we know that an object’s velocity can change due to acceleration. The model includes this uncertainty via the process noise.
Laser Measurements
The LIDAR sensor output is a point cloud but in this project, the point cloud is pre-processed and the x,y state of the bicycle is already extracted.
Definition of LIDAR variables:
- z is the measurement vector. For a lidar sensor, the z vector contains the position−x and position−y measurements.
- H is the matrix that projects your belief about the object’s current state into the measurement space of the sensor. For lidar, this is a fancy way of saying that we discard velocity information from the state variable since the lidar sensor only measures position: The state vector x contains information about [px,py,vx,vy] whereas the z vector will only contain [px,py]. Multiplying Hx allows us to compare x, our belief, with z, the sensor measurement.
- What does the prime notation in the x vector represent? The prime notation like px′ means you have already done the update step but have not done the measurement step yet. In other words, the object was at px. After time Δt, you calculate where you believe the object will be based on the motion model and get px′.
Radar Measurements
The Radar sensor output is defined by the measured distance to the object, orientation and its speed.
Definition of Radar variables:
- The range, (ρ), is the distance to the pedestrian. The range is basically the magnitude of the position vector ρ which can be defined as ρ=sqrt(px2+py2).
- φ=atan(py/px). Note that φ is referenced counter-clockwise from the x-axis, so φ from the video clip above in that situation would actually be negative.
- The range rate, ρ˙, is the projection of the velocity, v, onto the line, L.
To be able to fuse Radar measurements defined in the polar coordinate system with the LIDAR measurements defined in the cartesian coordinate system, one of the measurements must be transformed.
In this project the LIDAR measurements are transformed from the cartesian into the polar coordinate system using this formula:
Overview of the Kalman Filter Algorithm Map
The Kalman Filter algorithm will go through the following steps:
- first measurement – the filter will receive initial measurements of the bicycle’s position relative to the car. These measurements will come from a radar or lidar sensor.
- initialize state and covariance matrices – the filter will initialize the bicycle’s position based on the first measurement.
- then the car will receive another sensor measurement after a time period Δt.
- predict – the algorithm will predict where the bicycle will be after time Δt. One basic way to predict the bicycle location after Δt is to assume the bicycle’s velocity is constant; thus the bicycle will have moved velocity * Δt. In the extended Kalman filter lesson, we will assume the velocity is constant; in the unscented Kalman filter lesson, we will introduce a more complex motion model.
- update – the filter compares the “predicted” location with what the sensor measurement says. The predicted location and the measured location are combined to give an updated location. The Kalman filter will put more weight on either the predicted location or the measured location depending on the uncertainty of each value.
-
then the car will receive another sensor measurement after a time period Δt. The algorithm then does another predict and update step.
Video result
Lidar measurements are red circles, radar measurements are blue circles with an arrow pointing in the direction of the observed angle, and estimation markers are green triangles. The video below shows what the simulator looks like when a c++ script is using its Kalman filter to track the object. The simulator provides the script the measured data (either lidar or radar), and the script feeds back the measured estimation marker, and RMSE values from its Kalman filter.
Resources
https://github.com/udacity/CarND-Extended-Kalman-Filter-Project
https://github.com/paul-o-alto/CarND-Extended-Kalman-Filter-Project
https://github.com/paul-o-alto/CarND-Extended-Kalman-Filter-Project
https://en.wikipedia.org/wiki/Kalman_filter
https://en.wikipedia.org/wiki/Extended_Kalman_filter
https://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter
https://ai2-s2-pdfs.s3.amazonaws.com/df70/ce9aed1d8b6fe3a93cb4e41efcb8979428c0.pdf
https://www.udacity.com/course/artificial-intelligence-for-robotics–cs373
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/