def test_CalculateRMSE_Returns0_IfEstimationsSizeIs0(self):
estimations = []
ground_truth = []
expected_rmse = Matrix([[]])
expected_rmse.zero(4, 1)
rmse = self._tools.calculate_rmse(estimations, ground_truth)
self.assertEqual(rmse.value, expected_rmse.value,
"RMSE should be 0 when estimation size is 0.")
def test_PredictRadarMeasurement_CalculatesZsigZPredAndS_FromXsigPred(
self):
n_aug = 7
n_z = 3
#radar measurement noise standard deviation radius in m
self._ukf._std_radr = 0.3
#radar measurement noise standard deviation angle in rad
self._ukf._std_radphi = 0.0175
#radar measurement noise standard deviation radius change in m/s
self._ukf._std_radrd = 0.1
#create example matrix with predicted sigma points
self._ukf._Xsig_pred = Matrix(
[[
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374,
5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744
],
[
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48,
1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486
],
[
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204,
2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049
],
[
0.5367, 0.47338, 0.67809,
0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546,
0.51900, 0.42991, 0.530188, 0.5367, 0.535048
],
[
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352,
0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352,
0.318159
]])
Zsig, z_pred, S = self._ukf.predict_radar_measurement()
expected_z_pred = Matrix([[6.12155], [0.245993], [2.10313]])
assert_array_almost_equal(z_pred.value,
expected_z_pred.value,
decimal=4)
#create example matrix for predicted measurement covariance
expected_S = Matrix([[0.0946171, -0.000139448, 0.00407016],
[-0.000139448, 0.000617548, -0.000770652],
[0.00407016, -0.000770652, 0.0180917]])
assert_array_almost_equal(S.value, expected_S.value)
def __init__(self,
timestamp=0,
sensor_type=SensorType.LASER,
raw_measurements=Matrix([[]])):
self._timestamp = timestamp
self._sensor_type = sensor_type
self._raw_measurements = raw_measurements
def test_ProcessMeasurement_SetsXAndP_IfFirstMeasurementIsLASER(self):
meas_package = MeasurementPackage(0,
MeasurementPackage.SensorType.LASER,
Matrix([[1], [1]]))
self._ukf.process_measurement(meas_package)
initial_x = Matrix([[1], [1], [0], [0], [0]])
self.assertEqual(self._ukf._x.value, initial_x.value)
initial_P = Matrix(
[[self._ukf._std_laspx * self._ukf._std_laspx, 0, 0, 0, 0],
[0, self._ukf._std_laspy * self._ukf._std_laspy, 0, 0, 0],
[0, 0, 9, 0, 0], [0, 0, 0, 2.25, 0], [0, 0, 0, 0, 0.0625]])
self.assertEqual(self._ukf._P.value, initial_P.value)
def test_AugmentedSigmaPoints_CalculatesXsigAug_FromXAndP(self):
n_aug = 7
self._ukf._std_a = 0.2
self._ukf._std_yawdd = 0.2
self._ukf._x = Matrix([[5.7441], [1.3800], [2.2049], [0.5015],
[0.3528]])
self._ukf._P = Matrix([[0.0043, -0.0013, 0.0030, -0.0022, -0.0020],
[-0.0013, 0.0077, 0.0011, 0.0071, 0.0060],
[0.0030, 0.0011, 0.0054, 0.0007, 0.0008],
[-0.0022, 0.0071, 0.0007, 0.0098, 0.0100],
[-0.0020, 0.0060, 0.0008, 0.0100, 0.0123]])
Xsig_aug = self._ukf.augmented_sigma_points()
expected_Xsig_aug = Matrix(
[[
5.7441, 5.85768, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441,
5.7441, 5.63052, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441
],
[
1.38, 1.34566, 1.52806, 1.38, 1.38, 1.38, 1.38, 1.38, 1.41434,
1.23194, 1.38, 1.38, 1.38, 1.38, 1.38
],
[
2.2049, 2.28414, 2.24557, 2.29582, 2.2049, 2.2049, 2.2049,
2.2049, 2.12566, 2.16423, 2.11398, 2.2049, 2.2049, 2.2049,
2.2049
],
[
0.5015, 0.44339, 0.631886, 0.516923, 0.595227, 0.5015, 0.5015,
0.5015, 0.55961, 0.371114, 0.486077, 0.407773, 0.5015, 0.5015,
0.5015
],
[
0.3528, 0.299973, 0.462123, 0.376339, 0.48417, 0.418721,
0.3528, 0.3528, 0.405627, 0.243477, 0.329261, 0.22143,
0.286879, 0.3528, 0.3528
], [0, 0, 0, 0, 0, 0, 0.34641, 0, 0, 0, 0, 0, 0, -0.34641, 0],
[0, 0, 0, 0, 0, 0, 0, 0.34641, 0, 0, 0, 0, 0, 0, -0.34641]])
assert_array_almost_equal(Xsig_aug.value,
expected_Xsig_aug.value,
decimal=4)
def parse_measurement(line):
row = line.split()
meas_package = MeasurementPackage()
i = 0
# reads first element from the current line
sensor_type = row[i]
i += 1
if(sensor_type == "L"):
# LASER MEASUREMENT
# read measurements at this timestamp
meas_package._sensor_type = MeasurementPackage.SensorType.LASER
px = float(row[i])
py = float(row[i+1])
meas_package._raw_measurements = Matrix([[px], [py]])
timestamp = int(row[i+2])
meas_package._timestamp = timestamp
i += 3
elif (sensor_type == "R"):
# RADAR MEASUREMENT
# read measurements at this timestamp
meas_package._sensor_type = MeasurementPackage.SensorType.RADAR
ro = float(row[i])
theta = float(row[i+1])
ro_dot = float(row[i+2])
meas_package._raw_measurements = Matrix([[ro], [theta], [ro_dot]])
timestamp = int(row[i+3])
meas_package._timestamp = timestamp
i += 4
# read ground truth data to compare later
x_gt = float(row[i])
y_gt = float(row[i+1])
vx_gt = float(row[i+2])
vy_gt = float(row[i+3])
gt_values = Matrix([[x_gt], [y_gt], [vx_gt], [vy_gt]])
return (meas_package, gt_values)
def test_PredictMeanAndCovariance_CalculatesXAndP_FromXsigPred(self):
# create example matrix with predicted sigma points
self._ukf._Xsig_pred = Matrix(
[[
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374,
5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744
],
[
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48,
1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486
],
[
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204,
2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049
],
[
0.5367, 0.47338, 0.67809,
0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546,
0.51900, 0.42991, 0.530188, 0.5367, 0.535048
],
[
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352,
0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352,
0.318159
]])
x, P = self._ukf.predict_mean_and_covariance()
expected_x = Matrix([[5.93637], [1.49035], [2.20528], [0.536853],
[0.353577]])
assert_array_almost_equal(x.value, expected_x.value, decimal=5)
expected_P = Matrix(
[[0.00543425, -0.0024053, 0.00341576, -0.00348196, -0.00299378],
[-0.0024053, 0.010845, 0.0014923, 0.00980182, 0.00791091],
[0.00341576, 0.0014923, 0.00580129, 0.000778632, 0.000792973],
[-0.00348196, 0.00980182, 0.000778632, 0.0119238, 0.0112491],
[-0.00299378, 0.00791091, 0.000792973, 0.0112491, 0.0126972]])
assert_array_almost_equal(P.value, expected_P.value)
def test_ProcessMeasurement_SetsXAndP_IfFirstMeasurementIsRADAR(self):
ro = 1.0
theta = 2.0
meas_package = MeasurementPackage(0,
MeasurementPackage.SensorType.RADAR,
Matrix([[ro], [theta], [0.5]]))
self._ukf.process_measurement(meas_package)
initial_x = Matrix([[ro * cos(theta)], [ro * sin(theta)], [0], [0],
[0]])
self.assertEqual(self._ukf._x.value, initial_x.value)
std_rad = max(self._ukf._std_radr, self._ukf._std_radphi * ro) * 2
initial_P = Matrix([[std_rad * std_rad, 0, 0, 0, 0],
[0, std_rad * std_rad, 0, 0, 0], [0, 0, 9, 0, 0],
[0, 0, 0, 2.25, 0], [0, 0, 0, 0, 0.0625]])
self.assertEqual(self._ukf._P.value, initial_P.value)
def calculate_rmse(self, estimations, ground_truth):
"""A helper method to calculate RMSE."""
rmse = Matrix([[]])
rmse.zero(4, 1)
# check the validity of the following inputs:
# * the estimation vector size should not be zero
# * the estimation vector size should equal ground truth vector size
if len(estimations) != len(ground_truth) or not estimations:
print("Invalid estimation or ground_truth data")
return rmse
#accumulate squared residuals
for i in range(len(estimations)):
residual = estimations[i] - ground_truth[i]
#coefficient-wise multiplication
residual = residual.cwise_product(residual)
rmse += residual
#calculate the mean
rmse.value = [[i[0] / len(estimations)] for i in rmse.value]
#calculate the squared root
rmse.value = [[sqrt(i[0])] for i in rmse.value]
#return the result
return rmse
def telemetry(sid, data):
if data:
j = data
sensor_measurment = j["sensor_measurement"]
tokens = sensor_measurment.split()
i = 0
# reads first element from the current line
sensor_type = tokens[i]
meas_package, gt_values = parse_measurement(tokens)
ground_truth.append(gt_values)
#Call ProcessMeasurment(meas_package) for Kalman filter
ukf.process_measurement(meas_package)
#Push the current estimated x,y positon from the Kalman filter's state vector
p_x = ukf._x.value[0][0]
p_y = ukf._x.value[1][0]
v = ukf._x.value[2][0]
yaw = ukf._x.value[3][0]
v1 = cos(yaw)*v
v2 = sin(yaw)*v
estimate = Matrix([[p_x], [p_y], [v1], [v2]])
estimations.append(estimate)
if(sensor_type == "L") and ukf._use_laser:
lidar_nis.append(ukf._lidar_nis)
elif(sensor_type == "R") and ukf._use_radar:
radar_nis.append(ukf._radar_nis)
rmse = tools.calculate_rmse(estimations, ground_truth)
msgJson = {}
msgJson["estimate_x"] = p_x
msgJson["estimate_y"] = p_y
msgJson["rmse_x"] = rmse.value[0][0]
msgJson["rmse_y"] = rmse.value[1][0]
msgJson["rmse_vx"] = rmse.value[2][0]
msgJson["rmse_vy"] = rmse.value[3][0]
#msg = "42[\"estimate_marker\"," + msgJson.dump() + "]"
# print(msg)
sio.emit('estimate_marker', data=msgJson, skip_sid=True)
else:
# NOTE: DON'T EDIT THIS.
sio.emit('manual', data={}, skip_sid=True)
def test_CalculateRMSE_Returns0_IfEstimationsSizeDiffer(self):
estimations = [Matrix([[1], [1], [0.2], [0.1]])]
ground_truth = [
Matrix([[1.1], [1.1], [0.3], [0.2]]),
Matrix([[2.1], [2.1], [0.4], [0.3]])
]
expected_rmse = Matrix([[]])
expected_rmse.zero(4, 1)
rmse = self._tools.calculate_rmse(estimations, ground_truth)
self.assertEqual(rmse.value, expected_rmse.value,
"RMSE should be 0 when estimation size differs.")
def test_ProcessMeasurement_CallsPredictThenUpdateRadar_ForSubsequentRADARMeasurements(
self):
first_measurement = MeasurementPackage(
1477010443050000, MeasurementPackage.SensorType.RADAR,
Matrix([[0.898658], [0.617674], [1.7986]]))
second_measurement = MeasurementPackage(
1477010443150000, MeasurementPackage.SensorType.RADAR,
Matrix([[0.910574], [0.610537], [1.46233]]))
self._ukf.prediction = MagicMock(wraps=self._ukf.prediction)
self._ukf.update_radar = MagicMock(wraps=self._ukf.update_radar)
ukf_sequence = Mock()
ukf_sequence.attach_mock(self._ukf.prediction, 'prediction')
ukf_sequence.attach_mock(self._ukf.update_radar, 'update_radar')
self._ukf.process_measurement(first_measurement)
self._ukf.process_measurement(second_measurement)
assert ukf_sequence.mock_calls == [
call.prediction(0.1),
call.update_radar(second_measurement)
]
def test_ProcessMeasurement_CallsPredictThenUpdateLidar_ForSubsequentLASERMeasurements(
self):
first_measurement = MeasurementPackage(
1477010443000000, MeasurementPackage.SensorType.LASER,
Matrix([[0.463227], [0.607415]]))
second_measurement = MeasurementPackage(
1477010443100000, MeasurementPackage.SensorType.LASER,
Matrix([[0.968521], [0.40545]]))
self._ukf.prediction = MagicMock(wraps=self._ukf.prediction)
self._ukf.update_lidar = MagicMock(wraps=self._ukf.update_lidar)
ukf_sequence = Mock()
ukf_sequence.attach_mock(self._ukf.prediction, 'prediction')
ukf_sequence.attach_mock(self._ukf.update_lidar, 'update_lidar')
self._ukf.process_measurement(first_measurement)
self._ukf.process_measurement(second_measurement)
assert ukf_sequence.mock_calls == [
call.prediction(0.1),
call.update_lidar(second_measurement)
]
def update_lidar(self, meas_package):
"""Updates the state and the state covariance matrix using a laser measurement
Args:
meas_package (:obj:`MeasurementPackage`): The measurement at k+1
"""
"""
Use lidar data to update the belief
about the object's position. Modify the state vector, self._x, and
covariance, self._P.
You can also calculate the lidar NIS, if desired.
"""
"""
Update Lidar Measurement
"""
# The mapping from state space to Lidar is linear. Fill this out with
# appropriate update steps
z_pred = self._H * self._x
y = meas_package._raw_measurements - z_pred
#Ht = H.transpose()
S = self._H * self._P * self._Ht + self._R
Si = S.inverse()
PHt = self._P * self._Ht
K = PHt * Si
# new estimate
self._x = self._x + (K * y)
x_size = self._x.dimx
I = Matrix([[]])
I.identity(x_size)
self._P = (I - K * self._H) * self._P
nis = y.transpose() * Si * y
self._lidar_nis = nis.value[0][0]
def run(ukf, p):
ukf._std_a = p[0]
ukf._std_yawdd = p[1]
in_file_name_ = "../data/obj_pose-laser-radar-synthetic-input.txt"
in_file = open(in_file_name_, newline='')
data_reader = csv.reader(in_file, delimiter='\t', quotechar='|')
tools = Tools()
# used to compute the RMSE later
estimations = []
ground_truth =[]
# prep the measurement packages (each line represents a measurement at a
# timestamp)
for row in data_reader:
meas_package, gt_values = parse_measurement(row)
ground_truth.append(gt_values)
ukf.process_measurement(meas_package)
p_x = ukf._x.value[0][0]
p_y = ukf._x.value[1][0]
v = ukf._x.value[2][0]
yaw = ukf._x.value[3][0]
v1 = cos(yaw)*v
v2 = sin(yaw)*v
estimate = Matrix([[p_x], [p_y], [v1], [v2]])
estimations.append(estimate)
# compute the accuracy (RMSE)
rmse = tools.calculate_rmse(estimations, ground_truth)
in_file.close()
err = 0
for i in rmse.value:
err += i[0]*i[0]
err = sqrt(err)
return err
def test_CalculateRMSE_ReturnsCorrectRMSE_ForTestEstimations(self):
for estimations, ground_truth, expected_rmse in [([
Matrix([[1], [1], [0.2], [0.1]]),
Matrix([[2], [2], [0.3], [0.2]]),
Matrix([[3], [3], [0.4], [0.3]])
], [
Matrix([[1.1], [1.1], [0.3], [0.2]]),
Matrix([[2.1], [2.1], [0.4], [0.3]]),
Matrix([[3.1], [3.1], [0.5], [0.4]])
], Matrix([[0.1], [0.1], [0.1], [0.1]]))]:
with self.subTest():
rmse = self._tools.calculate_rmse(estimations, ground_truth)
assert_array_almost_equal(
rmse.value,
expected_rmse.value,
err_msg="RMSE should be correctly calculated.")
def predict_mean_and_covariance(self):
#Lesson 7, section 24: Predicted Mean and Covariance Assignment 2
# create vector for predicted state
x = Matrix([[]])
# create covariance matrix for prediction
P = Matrix([[]])
# predicted state mean
x.zero(self._n_x, 1)
for i in range(2 * self._n_aug + 1): # iterate over sigma points
for row in range(x.dimx):
x.value[row] = [
x.value[row][0] +
self._weights.value[i][0] * self._Xsig_pred.value[row][i]
]
# predicted state covariance matrix
P.zero(self._n_x, self._n_x)
for i in range(2 * self._n_aug + 1): # iterate over sigma points
# state difference
x_diff = Matrix([[]])
x_diff.zero(self._n_x, 1)
for row in range(self._n_x):
x_diff.value[row][0] = self._Xsig_pred.value[row][i]
x_diff = x_diff - x
# angle normalization
x_diff.value[3][0] = fmod(x_diff.value[3][0], pi)
x_diff2 = x_diff * x_diff.transpose()
x_diff2.value = [[xv * self._weights.value[i][0] for xv in row]
for row in x_diff2.value]
P = P + x_diff2
return (x, P)
def test_UKF_PassesProjectRubric_ForDataSet1(self):
in_file_name_ = "../data/obj_pose-laser-radar-synthetic-input.txt"
in_file = open(in_file_name_, newline='')
data_reader = csv.reader(in_file, delimiter='\t', quotechar='|')
self.assertFalse(in_file.closed)
tools = Tools()
# used to compute the RMSE later
estimations = []
ground_truth = []
# prep the measurement packages (each line represents a measurement at a
# timestamp)
for row in data_reader:
meas_package = MeasurementPackage()
i = 0
# reads first element from the current line
sensor_type = row[i]
i += 1
if (sensor_type == "L"):
# LASER MEASUREMENT
# read measurements at this timestamp
meas_package._sensor_type = MeasurementPackage.SensorType.LASER
px = float(row[i])
py = float(row[i + 1])
meas_package._raw_measurements = Matrix([[px], [py]])
timestamp = int(row[i + 2])
meas_package._timestamp = timestamp
i += 3
elif (sensor_type == "R"):
# RADAR MEASUREMENT
# read measurements at this timestamp
meas_package._sensor_type = MeasurementPackage.SensorType.RADAR
ro = float(row[i])
theta = float(row[i + 1])
ro_dot = float(row[i + 2])
meas_package._raw_measurements = Matrix([[ro], [theta],
[ro_dot]])
timestamp = int(row[i + 3])
meas_package._timestamp = timestamp
i += 4
# read ground truth data to compare later
x_gt = float(row[i])
y_gt = float(row[i + 1])
vx_gt = float(row[i + 2])
vy_gt = float(row[i + 3])
gt_values = Matrix([[x_gt], [y_gt], [vx_gt], [vy_gt]])
ground_truth.append(gt_values)
self._ukf.process_measurement(meas_package)
p_x = self._ukf._x.value[0][0]
p_y = self._ukf._x.value[1][0]
v = self._ukf._x.value[2][0]
yaw = self._ukf._x.value[3][0]
v1 = cos(yaw) * v
v2 = sin(yaw) * v
estimate = Matrix([[p_x], [p_y], [v1], [v2]])
estimations.append(estimate)
# compute the accuracy (RMSE)
expected_rmse = Matrix([[0.09], [0.10], [0.40], [0.30]])
rmse = tools.calculate_rmse(estimations, ground_truth)
assert_array_less(rmse.value, expected_rmse.value)
in_file.close()
class UKF:
def __init__(self):
"""Initializes Unscented Kalman filter"""
# initially set to false, set to true in first call of ProcessMeasurement
self._is_initialized = False
# predicted sigma points matrix
self._Xsig_pred = Matrix([[]])
# time when the state is true, in us
self._time_us = 0
# Weights of sigma points
self._weights = Matrix([[]])
# State dimension
self._n_x = 5
# Augmented state dimension
self._n_aug = 7
# Sigma point spreading parameter
self._lambda = 3 - self._n_aug
self._lambda_sqrt = sqrt(self._lambda + self._n_aug)
#if this is false, laser measurements will be ignored (except during init)
self._use_laser = True
#if this is false, radar measurements will be ignored (except during init)
self._use_radar = True
# initial state vector
# state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
self._x = Matrix([[]])
self._x.zero(5, 1)
# initial covariance matrix
self._P = Matrix([[]])
self._P.zero(5, 5)
# Process noise standard deviation longitudinal acceleration in m/s^2
self._std_a = 0.58
# Process noise standard deviation yaw acceleration in rad/s^2
self._std_yawdd = 0.57
"""
DO NOT MODIFY measurement noise values below.
These are provided by the sensor manufacturer.
"""
# Laser measurement noise standard deviation position1 in m
self._std_laspx = 0.15
# Laser measurement noise standard deviation position2 in m
self._std_laspy = 0.15
# Radar measurement noise standard deviation radius in m
self._std_radr = 0.3
# Radar measurement noise standard deviation angle in rad
self._std_radphi = 0.03
# Radar measurement noise standard deviation radius change in m/s
self._std_radrd = 0.3
"""
End DO NOT MODIFY section for measurement noise values
"""
#set vector for weights
self._weights.zero(2 * self._n_aug + 1, 1)
# set weights
weight_0 = self._lambda / (self._lambda + self._n_aug)
weight = 0.5 / (self._n_aug + self._lambda)
self._weights.value[0] = [weight_0]
for i in range(1, 2 * self._n_aug + 1):
self._weights.value[i] = [weight]
#create matrix with predicted sigma points as columns
self._Xsig_pred.zero(self._n_x, 2 * self._n_aug + 1)
# Lidar measurement covariance
self._R = Matrix([[self._std_laspx * self._std_laspx, 0],
[0, self._std_laspy * self._std_laspy]])
# Lidar measurement matrix
self._H = Matrix([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]])
self._Ht = self._H.transpose()
self._lidar_nis = 0
self._radar_nis = 0
def process_measurement(self, meas_package):
"""ProcessMeasurement
Args:
meas_package (:obj:`MeasurementPackage`): The measurement at k+1
"""
"""
Initialization structure similar to EKF project
"""
if not self._is_initialized:
#Initialize x_, P_, previous_time, anything else needed.
if meas_package._sensor_type == MeasurementPackage.SensorType.LASER:
#Initialize here
self._x.zero(self._n_x, 1)
self._x.value[0:2] = meas_package._raw_measurements.value
self._P = Matrix([
[self._std_laspx * self._std_laspx, 0, 0, 0, 0],
[0, self._std_laspy * self._std_laspy, 0, 0, 0],
[0, 0, 9, 0, 0], #Assumming 95% of time speed is +/- 6m/s
[0, 0, 0, 2.25,
0], #Assuming 95% of the time angle is +/- 3rad
[0, 0, 0, 0, 0.0625]
]) #Assuming 95% of the time angular speed +/- 0.5 rad/s
elif meas_package._sensor_type == MeasurementPackage.SensorType.RADAR:
#Initialize here
#Convert radar from polar to cartesian coordinates
# and initialize state.
ro = meas_package._raw_measurements.value[0][0]
theta = meas_package._raw_measurements.value[1][0]
self._x.zero(self._n_x, 1)
self._x.value[0:2] = [[ro * cos(theta)], [ro * sin(theta)]]
#max(standard deviation radius, standard deviation angle*radar distance)
#a crude estimation of maximum standard deviation in cartesian coordinates
std_rad = max(self._std_radr, self._std_radphi * ro) * 2
self._P = Matrix([
[std_rad * std_rad, 0, 0, 0, 0],
[0, std_rad * std_rad, 0, 0, 0],
[0, 0, 9, 0, 0], #Assumming 95% of time speed is +/- 6m/s
[0, 0, 0, 2.25,
0], #Assuming 95% of the time angle is +/- 3rad
[0, 0, 0, 0, 0.0625]
]) #Assuming 95% of the time angular speed +/- 0.5 rad/s
#Initialize anything else here
self._time_us = meas_package._timestamp
self._is_initialized = True
return
"""
Control structure similar to EKF project
"""
delta_t = (meas_package._timestamp - self._time_us) / 1000000.0
self.prediction(delta_t)
if ((meas_package._sensor_type == MeasurementPackage.SensorType.LASER)
and self._use_laser):
self.update_lidar(meas_package)
elif (
(meas_package._sensor_type == MeasurementPackage.SensorType.RADAR)
and self._use_radar):
self.update_radar(meas_package)
self._time_us = meas_package._timestamp
def prediction(self, delta_t):
"""Predicts sigma points, the state, and the state covariance matrix
Args:
delta_t (float): Time between k and k+1 in s
"""
"""
* Estimate the object's location.
* Modify the state vector, x_. Predict sigma points, the state,
and the state covariance matrix.
"""
"""
Create Augmented Sigma Points
"""
#create sigma point matrix
Xsig_aug = self.augmented_sigma_points()
"""
Predict Sigma Points
"""
#Lesson 7, section 21: Sigma Point Prediction Assignment 2
# may be it's very little gain. but to avoid couple of arithmatic operations
delta_t_2 = delta_t * delta_t
# predict sigma points
for i in range(2 * self._n_aug + 1):
#extract values for better readability
p_x = Xsig_aug.value[0][i]
p_y = Xsig_aug.value[1][i]
v = Xsig_aug.value[2][i]
yaw = Xsig_aug.value[3][i]
yawd = Xsig_aug.value[4][i]
nu_a = Xsig_aug.value[5][i]
nu_yawdd = Xsig_aug.value[6][i]
# avoid division by zero
if (abs(yawd) > 0.001):
px_p = p_x + v / yawd * (sin(yaw + yawd * delta_t) - sin(yaw))
py_p = p_y + v / yawd * (cos(yaw) - cos(yaw + yawd * delta_t))
else:
px_p = p_x + v * delta_t * cos(yaw)
py_p = p_y + v * delta_t * sin(yaw)
v_p = v
yaw_p = yaw + yawd * delta_t
yawd_p = yawd
# add noise
px_p = px_p + 0.5 * nu_a * delta_t_2 * cos(yaw)
py_p = py_p + 0.5 * nu_a * delta_t_2 * sin(yaw)
v_p = v_p + nu_a * delta_t
yaw_p = yaw_p + 0.5 * nu_yawdd * delta_t_2
yawd_p = yawd_p + nu_yawdd * delta_t
# write predicted sigma point into right column
self._Xsig_pred.value[0][i] = px_p
self._Xsig_pred.value[1][i] = py_p
self._Xsig_pred.value[2][i] = v_p
self._Xsig_pred.value[3][i] = yaw_p
self._Xsig_pred.value[4][i] = yawd_p
"""
Predict mean and covariance
"""
self._x, self._P = self.predict_mean_and_covariance()
def update_lidar(self, meas_package):
"""Updates the state and the state covariance matrix using a laser measurement
Args:
meas_package (:obj:`MeasurementPackage`): The measurement at k+1
"""
"""
Use lidar data to update the belief
about the object's position. Modify the state vector, self._x, and
covariance, self._P.
You can also calculate the lidar NIS, if desired.
"""
"""
Update Lidar Measurement
"""
# The mapping from state space to Lidar is linear. Fill this out with
# appropriate update steps
z_pred = self._H * self._x
y = meas_package._raw_measurements - z_pred
#Ht = H.transpose()
S = self._H * self._P * self._Ht + self._R
Si = S.inverse()
PHt = self._P * self._Ht
K = PHt * Si
# new estimate
self._x = self._x + (K * y)
x_size = self._x.dimx
I = Matrix([[]])
I.identity(x_size)
self._P = (I - K * self._H) * self._P
nis = y.transpose() * Si * y
self._lidar_nis = nis.value[0][0]
def update_radar(self, meas_package):
"""Updates the state and the state covariance matrix using a radar measurement
Args:
meas_package (:obj:`MeasurementPackage`): The measurement at k+1
"""
"""
Use radar data to update the belief
about the object's position. Modify the state vector, self._x, and
covariance, self._P.
You can also calculate the radar NIS, if desired.
"""
"""
Predict Radar Sigma Points
"""
#set measurement dimension, radar can measure r, phi, and r_dot
n_z = 3
Zsig, z_pred, S = self.predict_radar_measurement()
"""
Update Radar
"""
#Lesson 7, section 30: UKF Update Assignment 2
# create matrix for cross correlation Tc
Tc = Matrix([[]])
# calculate cross correlation matrix
Tc.zero(self._n_x, n_z)
for i in range(2 * self._n_aug + 1): #2n+1 simga points
# residual
z_diff = Matrix([[]])
z_diff.zero(n_z, 1)
for row in range(n_z):
z_diff.value[row][0] = Zsig.value[row][i]
z_diff = z_diff - z_pred
# angle normalization
z_diff.value[1][0] = fmod(z_diff.value[1][0], pi)
# state difference
x_diff = Matrix([[]])
x_diff.zero(self._n_x, 1)
for row in range(self._n_x):
x_diff.value[row][0] = self._Xsig_pred.value[row][i]
x_diff = x_diff - self._x
# angle normalization
x_diff.value[3][0] = fmod(x_diff.value[3][0], pi)
x_diff_z_diff_t = x_diff * z_diff.transpose()
x_diff_z_diff_t.value = [[
xzv * self._weights.value[i][0] for xzv in row
] for row in x_diff_z_diff_t.value]
Tc = Tc + x_diff_z_diff_t
# Kalman gain K;
K = Tc * S.inverse()
# residual
z_diff = meas_package._raw_measurements - z_pred
# angle normalization
z_diff.value[1][0] = fmod(z_diff.value[1][0], pi)
#update state mean and covariance matrix
self._x = self._x + K * z_diff
self._P = self._P - K * S * K.transpose()
nis = z_diff.transpose() * S.inverse() * z_diff
self._radar_nis = nis.value[0][0]
"""
Student assignment functions
"""
def augmented_sigma_points(self):
#Lesson 7, section 18: Augmentation Assignment 2
#create augmented mean vector
x_aug = Matrix([[]])
x_aug.zero(self._n_aug, 1)
# create augmented state covariance
P_aug = Matrix([[]])
P_aug.zero(self._n_aug, self._n_aug)
# create sigma point matrix
Xsig_aug = Matrix([[]])
Xsig_aug.zero(self._n_aug, 2 * self._n_aug + 1)
# create augmented mean state, remember mean of noise is zero
x_aug.value = self._x.value + [[0], [0]]
# create augmented covariance matrix
P_aug.value = ([row + [0, 0] for row in self._P.value] +
[[0, 0, 0, 0, 0, self._std_a * self._std_a, 0],
[0, 0, 0, 0, 0, 0, self._std_yawdd * self._std_yawdd]])
# create square root matrix
L = P_aug.Cholesky().transpose()
# create augmented sigma points
for row in range(len(x_aug.value)):
Xsig_aug.value[row][0] = x_aug.value[row][0]
for i in range(self._n_aug):
for row in range(len(x_aug.value)):
Xsig_aug.value[row][i + 1] = (
x_aug.value[row][0] + self._lambda_sqrt * L.value[row][i]
) #+ sqrt(self._lambda+self._n_aug)*L.value[row][i])
Xsig_aug.value[row][i + 1 + self._n_aug] = (
x_aug.value[row][0] - self._lambda_sqrt * L.value[row][i]
) #- sqrt(self._lambda+self._n_aug)*L.value[row][i])
return Xsig_aug
def predict_mean_and_covariance(self):
#Lesson 7, section 24: Predicted Mean and Covariance Assignment 2
# create vector for predicted state
x = Matrix([[]])
# create covariance matrix for prediction
P = Matrix([[]])
# predicted state mean
x.zero(self._n_x, 1)
for i in range(2 * self._n_aug + 1): # iterate over sigma points
for row in range(x.dimx):
x.value[row] = [
x.value[row][0] +
self._weights.value[i][0] * self._Xsig_pred.value[row][i]
]
# predicted state covariance matrix
P.zero(self._n_x, self._n_x)
for i in range(2 * self._n_aug + 1): # iterate over sigma points
# state difference
x_diff = Matrix([[]])
x_diff.zero(self._n_x, 1)
for row in range(self._n_x):
x_diff.value[row][0] = self._Xsig_pred.value[row][i]
x_diff = x_diff - x
# angle normalization
x_diff.value[3][0] = fmod(x_diff.value[3][0], pi)
x_diff2 = x_diff * x_diff.transpose()
x_diff2.value = [[xv * self._weights.value[i][0] for xv in row]
for row in x_diff2.value]
P = P + x_diff2
return (x, P)
def predict_radar_measurement(self):
# Lesson 7, section 27: Predict Radar Measurement Assignment 2
# set measurement dimension, radar can measure r, phi, and r_dot
n_z = 3
# create matrix for sigma points in measurement space
Zsig = Matrix([[]])
Zsig.zero(n_z, 2 * self._n_aug + 1)
# mean predicted measurement
z_pred = Matrix([[]])
# measurement covariance matrix S
S = Matrix([[]])
S.zero(n_z, n_z)
# transform sigma points into measurement space
for i in range(2 * self._n_aug + 1): # 2n+1 simga points
# extract values for better readability
p_x = self._Xsig_pred.value[0][i]
p_y = self._Xsig_pred.value[1][i]
v = self._Xsig_pred.value[2][i]
yaw = self._Xsig_pred.value[3][i]
v1 = cos(yaw) * v
v2 = sin(yaw) * v
# measurement model
Zsig.value[0][i] = sqrt(p_x * p_x + p_y * p_y) # r
Zsig.value[1][i] = atan2(p_y, p_x) # phi
Zsig.value[2][i] = (p_x * v1 + p_y * v2) / Zsig.value[0][
i] #sqrt(p_x*p_x + p_y*p_y) # r_dot
# mean predicted measurement
z_pred.zero(n_z, 1)
for i in range(2 * self._n_aug + 1):
for row in range(n_z):
z_pred.value[row] = [
z_pred.value[row][0] +
self._weights.value[i][0] * Zsig.value[row][i]
]
# innovation covariance matrix S
S.zero(n_z, n_z)
for i in range(2 * self._n_aug + 1): # 2n+1 simga points
# residual
z_diff = Matrix([[]])
z_diff.zero(n_z, 1)
for row in range(n_z):
z_diff.value[row][0] = Zsig.value[row][i]
z_diff = z_diff - z_pred
# angle normalization
z_diff.value[1][0] = fmod(z_diff.value[1][0], pi)
z_diff2 = z_diff * z_diff.transpose()
z_diff2.value = [[zv * self._weights.value[i][0] for zv in row]
for row in z_diff2.value]
S = S + z_diff2
# add measurement noise covariance matrix
R = Matrix([[self._std_radr * self._std_radr, 0, 0],
[0, self._std_radphi * self._std_radphi, 0],
[0, 0, self._std_radrd * self._std_radrd]])
S = S + R
return (Zsig, z_pred, S)
def test_UpdateRadar_CallsPredictRadarMeasurementThenCalculatesXAndP_ForGivenMeasurement(
self):
#radar measurement noise standard deviation radius in m
self._ukf._std_radr = 0.3
#radar measurement noise standard deviation angle in rad
self._ukf._std_radphi = 0.0175
#radar measurement noise standard deviation radius change in m/s
self._ukf._std_radrd = 0.1
self._ukf._Xsig_pred = Matrix(
[[
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374,
5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744
],
[
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48,
1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486
],
[
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204,
2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049
],
[
0.5367, 0.47338, 0.67809,
0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546,
0.51900, 0.42991, 0.530188, 0.5367, 0.535048
],
[
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352,
0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352,
0.318159
]])
#create example vector for predicted state mean
self._ukf._x = Matrix([[5.93637], [1.49035], [2.20528], [0.536853],
[0.353577]])
#create example matrix for predicted state covariance
self._ukf._P = Matrix(
[[0.0054342, -0.002405, 0.0034157, -0.0034819, -0.00299378],
[-0.002405, 0.01084, 0.001492, 0.0098018, 0.00791091],
[0.0034157, 0.001492, 0.0058012, 0.00077863, 0.000792973],
[-0.0034819, 0.0098018, 0.00077863, 0.011923, 0.0112491],
[-0.0029937, 0.0079109, 0.00079297, 0.011249, 0.0126972]])
# create example matrix with sigma points in measurement space
Zsig = Matrix([[
6.1190, 6.2334, 6.1531, 6.1283, 6.1143, 6.1190, 6.1221, 6.1190,
6.0079, 6.0883, 6.1125, 6.1248, 6.1190, 6.1188, 6.12057
],
[
0.24428, 0.2337, 0.27316, 0.24616, 0.24846, 0.24428,
0.24530, 0.24428, 0.25700, 0.21692, 0.24433,
0.24193, 0.24428, 0.24515, 0.245239
],
[
2.1104, 2.2188, 2.0639, 2.187, 2.0341, 2.1061,
2.1450, 2.1092, 2.0016, 2.129, 2.0346, 2.1651,
2.1145, 2.0786, 2.11295
]])
# create example vector for mean predicted measurement
z_pred = Matrix([[6.12155], [0.245993], [2.10313]])
# create example matrix for predicted measurement covariance
S = Matrix([[0.0946171, -0.000139448, 0.00407016],
[-0.000139448, 0.000617548, -0.000770652],
[0.00407016, -0.000770652, 0.0180917]])
self._ukf.predict_radar_measurement = MagicMock(return_value=(Zsig,
z_pred,
S))
meas_package = MeasurementPackage(
0,
MeasurementPackage.SensorType.RADAR,
Matrix([
[5.9214], # rho in m
[0.2187], # phi in rad
[2.0062]
])) # rho_dot in m/s
self._ukf.update_radar(meas_package)
# expected result x:
expected_x = Matrix([[5.92276], [1.41823], [2.15593], [0.489274],
[0.321338]])
assert_array_almost_equal(self._ukf._x.value,
expected_x.value,
decimal=4)
expected_P = Matrix(
[[0.00361579, -0.000357881, 0.00208316, -0.000937196, -0.00071727],
[-0.000357881, 0.00539867, 0.00156846, 0.00455342, 0.00358885],
[0.00208316, 0.00156846, 0.00410651, 0.00160333, 0.00171811],
[-0.000937196, 0.00455342, 0.00160333, 0.00652634, 0.00669436],
[-0.00071719, 0.00358884, 0.00171811, 0.00669426, 0.00881797]])
assert_array_almost_equal(self._ukf._P.value,
expected_P.value,
decimal=5)
self.assertTrue(self._ukf.predict_radar_measurement.called)
def update_radar(self, meas_package):
"""Updates the state and the state covariance matrix using a radar measurement
Args:
meas_package (:obj:`MeasurementPackage`): The measurement at k+1
"""
"""
Use radar data to update the belief
about the object's position. Modify the state vector, self._x, and
covariance, self._P.
You can also calculate the radar NIS, if desired.
"""
"""
Predict Radar Sigma Points
"""
#set measurement dimension, radar can measure r, phi, and r_dot
n_z = 3
Zsig, z_pred, S = self.predict_radar_measurement()
"""
Update Radar
"""
#Lesson 7, section 30: UKF Update Assignment 2
# create matrix for cross correlation Tc
Tc = Matrix([[]])
# calculate cross correlation matrix
Tc.zero(self._n_x, n_z)
for i in range(2 * self._n_aug + 1): #2n+1 simga points
# residual
z_diff = Matrix([[]])
z_diff.zero(n_z, 1)
for row in range(n_z):
z_diff.value[row][0] = Zsig.value[row][i]
z_diff = z_diff - z_pred
# angle normalization
z_diff.value[1][0] = fmod(z_diff.value[1][0], pi)
# state difference
x_diff = Matrix([[]])
x_diff.zero(self._n_x, 1)
for row in range(self._n_x):
x_diff.value[row][0] = self._Xsig_pred.value[row][i]
x_diff = x_diff - self._x
# angle normalization
x_diff.value[3][0] = fmod(x_diff.value[3][0], pi)
x_diff_z_diff_t = x_diff * z_diff.transpose()
x_diff_z_diff_t.value = [[
xzv * self._weights.value[i][0] for xzv in row
] for row in x_diff_z_diff_t.value]
Tc = Tc + x_diff_z_diff_t
# Kalman gain K;
K = Tc * S.inverse()
# residual
z_diff = meas_package._raw_measurements - z_pred
# angle normalization
z_diff.value[1][0] = fmod(z_diff.value[1][0], pi)
#update state mean and covariance matrix
self._x = self._x + K * z_diff
self._P = self._P - K * S * K.transpose()
nis = z_diff.transpose() * S.inverse() * z_diff
self._radar_nis = nis.value[0][0]
def augmented_sigma_points(self):
#Lesson 7, section 18: Augmentation Assignment 2
#create augmented mean vector
x_aug = Matrix([[]])
x_aug.zero(self._n_aug, 1)
# create augmented state covariance
P_aug = Matrix([[]])
P_aug.zero(self._n_aug, self._n_aug)
# create sigma point matrix
Xsig_aug = Matrix([[]])
Xsig_aug.zero(self._n_aug, 2 * self._n_aug + 1)
# create augmented mean state, remember mean of noise is zero
x_aug.value = self._x.value + [[0], [0]]
# create augmented covariance matrix
P_aug.value = ([row + [0, 0] for row in self._P.value] +
[[0, 0, 0, 0, 0, self._std_a * self._std_a, 0],
[0, 0, 0, 0, 0, 0, self._std_yawdd * self._std_yawdd]])
# create square root matrix
L = P_aug.Cholesky().transpose()
# create augmented sigma points
for row in range(len(x_aug.value)):
Xsig_aug.value[row][0] = x_aug.value[row][0]
for i in range(self._n_aug):
for row in range(len(x_aug.value)):
Xsig_aug.value[row][i + 1] = (
x_aug.value[row][0] + self._lambda_sqrt * L.value[row][i]
) #+ sqrt(self._lambda+self._n_aug)*L.value[row][i])
Xsig_aug.value[row][i + 1 + self._n_aug] = (
x_aug.value[row][0] - self._lambda_sqrt * L.value[row][i]
) #- sqrt(self._lambda+self._n_aug)*L.value[row][i])
return Xsig_aug
def test_Prediction_CallsAugmentedSigmaPointsThenUpdateXsigPredAndCallsPredictMeanAndCovariance_ForGivenDeltaT(
self):
self._ukf._std_a = 0.2
self._ukf._std_yawdd = 0.2
self._ukf._x = Matrix([[5.7441], [1.3800], [2.2049], [0.5015],
[0.3528]])
self._ukf._P = Matrix([[0.0043, -0.0013, 0.0030, -0.0022, -0.0020],
[-0.0013, 0.0077, 0.0011, 0.0071, 0.0060],
[0.0030, 0.0011, 0.0054, 0.0007, 0.0008],
[-0.0022, 0.0071, 0.0007, 0.0098, 0.0100],
[-0.0020, 0.0060, 0.0008, 0.0100, 0.0123]])
self._ukf.augmented_sigma_points = MagicMock(
wraps=self._ukf.augmented_sigma_points)
self._ukf.predict_mean_and_covariance = MagicMock(
wraps=self._ukf.predict_mean_and_covariance)
ukf_sequence = Mock()
ukf_sequence.attach_mock(self._ukf.augmented_sigma_points,
'augmented_sigma_points')
ukf_sequence.attach_mock(self._ukf.predict_mean_and_covariance,
'predict_mean_and_covariance')
delta_t = 0.1 #time diff in sec
self._ukf.prediction(delta_t)
expected_Xsig_pred = Matrix(
[[
5.93553, 6.06251, 5.92217, 5.9415, 5.92361, 5.93516, 5.93705,
5.93553, 5.80832, 5.94481, 5.92935, 5.94553, 5.93589, 5.93401,
5.93553
],
[
1.48939, 1.44673, 1.66484, 1.49719, 1.508, 1.49001, 1.49022,
1.48939, 1.5308, 1.31287, 1.48182, 1.46967, 1.48876, 1.48855,
1.48939
],
[
2.2049, 2.28414, 2.24557, 2.29582, 2.2049, 2.2049, 2.23954,
2.2049, 2.12566, 2.16423, 2.11398, 2.2049, 2.2049, 2.17026,
2.2049
],
[
0.53678, 0.473387, 0.678098, 0.554557, 0.643644, 0.543372,
0.53678, 0.538512, 0.600173, 0.395462, 0.519003, 0.429916,
0.530188, 0.53678, 0.535048
],
[
0.3528, 0.299973, 0.462123, 0.376339, 0.48417, 0.418721,
0.3528, 0.387441, 0.405627, 0.243477, 0.329261, 0.22143,
0.286879, 0.3528, 0.318159
]])
assert_array_almost_equal(self._ukf._Xsig_pred.value,
expected_Xsig_pred.value,
decimal=4)
assert ukf_sequence.mock_calls == [
call.augmented_sigma_points(),
call.predict_mean_and_covariance()
]
def __init__(self):
"""Initializes Unscented Kalman filter"""
# initially set to false, set to true in first call of ProcessMeasurement
self._is_initialized = False
# predicted sigma points matrix
self._Xsig_pred = Matrix([[]])
# time when the state is true, in us
self._time_us = 0
# Weights of sigma points
self._weights = Matrix([[]])
# State dimension
self._n_x = 5
# Augmented state dimension
self._n_aug = 7
# Sigma point spreading parameter
self._lambda = 3 - self._n_aug
self._lambda_sqrt = sqrt(self._lambda + self._n_aug)
#if this is false, laser measurements will be ignored (except during init)
self._use_laser = True
#if this is false, radar measurements will be ignored (except during init)
self._use_radar = True
# initial state vector
# state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
self._x = Matrix([[]])
self._x.zero(5, 1)
# initial covariance matrix
self._P = Matrix([[]])
self._P.zero(5, 5)
# Process noise standard deviation longitudinal acceleration in m/s^2
self._std_a = 0.58
# Process noise standard deviation yaw acceleration in rad/s^2
self._std_yawdd = 0.57
"""
DO NOT MODIFY measurement noise values below.
These are provided by the sensor manufacturer.
"""
# Laser measurement noise standard deviation position1 in m
self._std_laspx = 0.15
# Laser measurement noise standard deviation position2 in m
self._std_laspy = 0.15
# Radar measurement noise standard deviation radius in m
self._std_radr = 0.3
# Radar measurement noise standard deviation angle in rad
self._std_radphi = 0.03
# Radar measurement noise standard deviation radius change in m/s
self._std_radrd = 0.3
"""
End DO NOT MODIFY section for measurement noise values
"""
#set vector for weights
self._weights.zero(2 * self._n_aug + 1, 1)
# set weights
weight_0 = self._lambda / (self._lambda + self._n_aug)
weight = 0.5 / (self._n_aug + self._lambda)
self._weights.value[0] = [weight_0]
for i in range(1, 2 * self._n_aug + 1):
self._weights.value[i] = [weight]
#create matrix with predicted sigma points as columns
self._Xsig_pred.zero(self._n_x, 2 * self._n_aug + 1)
# Lidar measurement covariance
self._R = Matrix([[self._std_laspx * self._std_laspx, 0],
[0, self._std_laspy * self._std_laspy]])
# Lidar measurement matrix
self._H = Matrix([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]])
self._Ht = self._H.transpose()
self._lidar_nis = 0
self._radar_nis = 0
def parse_vector(line):
return Matrix([[float(i)] for i in line.split()])
def predict_radar_measurement(self):
# Lesson 7, section 27: Predict Radar Measurement Assignment 2
# set measurement dimension, radar can measure r, phi, and r_dot
n_z = 3
# create matrix for sigma points in measurement space
Zsig = Matrix([[]])
Zsig.zero(n_z, 2 * self._n_aug + 1)
# mean predicted measurement
z_pred = Matrix([[]])
# measurement covariance matrix S
S = Matrix([[]])
S.zero(n_z, n_z)
# transform sigma points into measurement space
for i in range(2 * self._n_aug + 1): # 2n+1 simga points
# extract values for better readability
p_x = self._Xsig_pred.value[0][i]
p_y = self._Xsig_pred.value[1][i]
v = self._Xsig_pred.value[2][i]
yaw = self._Xsig_pred.value[3][i]
v1 = cos(yaw) * v
v2 = sin(yaw) * v
# measurement model
Zsig.value[0][i] = sqrt(p_x * p_x + p_y * p_y) # r
Zsig.value[1][i] = atan2(p_y, p_x) # phi
Zsig.value[2][i] = (p_x * v1 + p_y * v2) / Zsig.value[0][
i] #sqrt(p_x*p_x + p_y*p_y) # r_dot
# mean predicted measurement
z_pred.zero(n_z, 1)
for i in range(2 * self._n_aug + 1):
for row in range(n_z):
z_pred.value[row] = [
z_pred.value[row][0] +
self._weights.value[i][0] * Zsig.value[row][i]
]
# innovation covariance matrix S
S.zero(n_z, n_z)
for i in range(2 * self._n_aug + 1): # 2n+1 simga points
# residual
z_diff = Matrix([[]])
z_diff.zero(n_z, 1)
for row in range(n_z):
z_diff.value[row][0] = Zsig.value[row][i]
z_diff = z_diff - z_pred
# angle normalization
z_diff.value[1][0] = fmod(z_diff.value[1][0], pi)
z_diff2 = z_diff * z_diff.transpose()
z_diff2.value = [[zv * self._weights.value[i][0] for zv in row]
for row in z_diff2.value]
S = S + z_diff2
# add measurement noise covariance matrix
R = Matrix([[self._std_radr * self._std_radr, 0, 0],
[0, self._std_radphi * self._std_radphi, 0],
[0, 0, self._std_radrd * self._std_radrd]])
S = S + R
return (Zsig, z_pred, S)
def parse_matrix(text):
return Matrix([[float(i) for i in line.split()] for line in text.splitlines()])
def process_measurement(self, meas_package):
"""ProcessMeasurement
Args:
meas_package (:obj:`MeasurementPackage`): The measurement at k+1
"""
"""
Initialization structure similar to EKF project
"""
if not self._is_initialized:
#Initialize x_, P_, previous_time, anything else needed.
if meas_package._sensor_type == MeasurementPackage.SensorType.LASER:
#Initialize here
self._x.zero(self._n_x, 1)
self._x.value[0:2] = meas_package._raw_measurements.value
self._P = Matrix([
[self._std_laspx * self._std_laspx, 0, 0, 0, 0],
[0, self._std_laspy * self._std_laspy, 0, 0, 0],
[0, 0, 9, 0, 0], #Assumming 95% of time speed is +/- 6m/s
[0, 0, 0, 2.25,
0], #Assuming 95% of the time angle is +/- 3rad
[0, 0, 0, 0, 0.0625]
]) #Assuming 95% of the time angular speed +/- 0.5 rad/s
elif meas_package._sensor_type == MeasurementPackage.SensorType.RADAR:
#Initialize here
#Convert radar from polar to cartesian coordinates
# and initialize state.
ro = meas_package._raw_measurements.value[0][0]
theta = meas_package._raw_measurements.value[1][0]
self._x.zero(self._n_x, 1)
self._x.value[0:2] = [[ro * cos(theta)], [ro * sin(theta)]]
#max(standard deviation radius, standard deviation angle*radar distance)
#a crude estimation of maximum standard deviation in cartesian coordinates
std_rad = max(self._std_radr, self._std_radphi * ro) * 2
self._P = Matrix([
[std_rad * std_rad, 0, 0, 0, 0],
[0, std_rad * std_rad, 0, 0, 0],
[0, 0, 9, 0, 0], #Assumming 95% of time speed is +/- 6m/s
[0, 0, 0, 2.25,
0], #Assuming 95% of the time angle is +/- 3rad
[0, 0, 0, 0, 0.0625]
]) #Assuming 95% of the time angular speed +/- 0.5 rad/s
#Initialize anything else here
self._time_us = meas_package._timestamp
self._is_initialized = True
return
"""
Control structure similar to EKF project
"""
delta_t = (meas_package._timestamp - self._time_us) / 1000000.0
self.prediction(delta_t)
if ((meas_package._sensor_type == MeasurementPackage.SensorType.LASER)
and self._use_laser):
self.update_lidar(meas_package)
elif (
(meas_package._sensor_type == MeasurementPackage.SensorType.RADAR)
and self._use_radar):
self.update_radar(meas_package)
self._time_us = meas_package._timestamp