Object tracking with LIDAR, Radar, sensor fusion and Extended Kalman Filter

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.

To handle these imperfections, filtering is often applied to the tracked measurements so the application can obtain more accurate estimates of the motion.

The Kalman filter (KF) is a popular choice for estimating motion in robotics. Since position information is linear, standard Kalman filtering can be easily applied to the tracking problem without much difficulty.

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.

To address these limitations, the unscented Kalman filter (UKF) was developed. The UKF operates on the premise that it is easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function. Instead of linearizing using Jacobian matrices, the UKF using a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of sample points.

The UKF is a powerful nonlinear estimation technique and has been shown to be a superior alternative to the EKF in a variety of applications.

This post describes in details how the standard linear Kalman filter works. The Udacity platform provides a class Artificial Intelligence for robotics where a big topic is dedicated to the linear Kalman filter.

A comparison of the three versions can be found in this publication A Comparison of Unscented and Extended Kalman Filtering for Estimating Quaternion Motion.

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.

# 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]

# 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]

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]

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.

# 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]]

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]]

([[3.9996664447958645], [0.9999998335552873]], [[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.

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.

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

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;

}

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

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.

Linear motion model

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.

Laser Measurements

Definition of LIDAR variables:

z is the measurement vector. For a lidar sensor, the $z$ vector contains the $p o s i t i o n - x$ and $p o s i t i o n - 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 $[p x, p y, v x, v y]$ whereas the $z$ vector will only contain $[p x, p y]$ . 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 $p x'$ means you have already done the update step but have not done the measurement step yet. In other words, the object was at $p x$ . After time $Δ t$ , you calculate where you believe the object will be based on the motion model and get $p x'$ .

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 $ρ = s q r t (p x 2 + p y 2)$ .
$φ = a t a n (p y / p x)$ . 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:

Cartesian to polar coordinate system

Overview of the Kalman Filter Algorithm Map

The Kalman Filter algorithm

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.

Sources:

https://bitbucket.org/coldvisionio/udacity-carnd-class/src/d068af49c1d316dcdd39cea7d0579944237487ac/CarND-Extended-Kalman-Filter-Project/

Resources

https://github.com/udacity/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/

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

Object motion tracking in 2D by fusing noisy measurements from LIDAR and Radar sensors

Laser Measurements

Radar Measurements

Overview of the Kalman Filter Algorithm Map

then the car will receive another sensor measurement after a time period $Δ t$ . The algorithm then does another predict and update step.

Video result

Sources:

Resources

Leave a Reply Cancel reply

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

Object motion tracking in 2D by fusing noisy measurements from LIDAR and Radar sensors

Laser Measurements

Radar Measurements

Overview of the Kalman Filter Algorithm Map

then the car will receive another sensor measurement after a time period Δt. The algorithm then does another predict and update step.

Video result

Sources:

Resources

Leave a Reply Cancel reply

then the car will receive another sensor measurement after a time period $Δ t$ . The algorithm then does another predict and update step.