def test_process_measurement_calls_predict_then_update_on_ekf_for_subsequent_LASER_measurements(self):
self._ekf.predict = MagicMock()
self._ekf.update = MagicMock()
ekf_sequence = Mock()
ekf_sequence.attach_mock(self._ekf.predict, 'predict')
ekf_sequence.attach_mock(self._ekf.update, 'update')
fusionEKF = FusionEKF(self._ekf, self._tools)
meas_package = MeasurementPackage(1477010443000000,
MeasurementPackage.SensorType.LASER,
Matrix([[0.463227], [0.607415]]))
fusionEKF.process_measurement(meas_package)
meas_package = MeasurementPackage(1477010443100000,
MeasurementPackage.SensorType.LASER,
Matrix([[0.968521], [0.40545]]))
fusionEKF.process_measurement(meas_package)
expected_R = Matrix([[0.0225, 0],
[0, 0.0225]])
expected_H = Matrix([[1, 0, 0, 0],
[0, 1, 0, 0]])
self.assertEqual(self._ekf._R.value, expected_R.value)
self.assertEqual(self._ekf._H.value, expected_H.value)
assert ekf_sequence.mock_calls == [call.predict(), call.update(Matrix([[0.968521], [0.40545]]))]
def test_process_measurement_sets_F_and_Q_of_ekf_for_subsequent_measurements(self):
fusionEKF = FusionEKF(self._ekf, self._tools)
meas_package = MeasurementPackage(1477010443000000,
MeasurementPackage.SensorType.LASER,
Matrix([[0.463227], [0.607415]]))
fusionEKF.process_measurement(meas_package)
meas_package = MeasurementPackage(1477010443100000,
MeasurementPackage.SensorType.LASER,
Matrix([[0.968521], [0.40545]]))
fusionEKF.process_measurement(meas_package)
self.assertAlmostEqual(self._ekf._F.value[0][2], 0.1)
self.assertAlmostEqual(self._ekf._F.value[1][3], 0.1)
dt_2 = 0.01
dt_3 = 0.001
dt_4 = 0.0001
#acceleration noise components
noise_ax = 9
noise_ay = 9
#set the process covariance matrix Q
#Lesson 5 Section 9
expected_Q = Matrix([[dt_4/4*noise_ax, 0, dt_3/2*noise_ax, 0],
[0, dt_4/4*noise_ay, 0, dt_3/2*noise_ay],
[dt_3/2*noise_ax, 0, dt_2*noise_ax, 0],
[0, dt_3/2*noise_ay, 0, dt_2*noise_ay]])
assert_array_almost_equal(self._ekf._Q.value, expected_Q.value)
def test_calculate_rmse_returns0_if_estimations_size_is0(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_process_measurement_sets_x_of_ekf_if_first_measurement_is_LASER(self):
meas_package = MeasurementPackage(0,
MeasurementPackage.SensorType.LASER,
Matrix([[1], [1]]))
fusionEKF = FusionEKF(self._ekf, self._tools)
fusionEKF.process_measurement(meas_package)
initial_x = Matrix([[1], [1], [0], [0]])
self.assertEqual(self._ekf._x.value, initial_x.value)
def test_calculate_jacobian_returns_correct_Hj_for_test_state(self):
for state, expected_Hj in [(Matrix([[1], [2], [0.2], [0.4]]),
Matrix([[0.447214, 0.894427, 0, 0],
[-0.4, 0.2, 0, 0],
[0, 0, 0.447214, 0.894427]]))]:
with self.subTest():
Hj = self._tools.calculate_jacobian(state)
assert_array_almost_equal(
Hj.value,
expected_Hj.value,
err_msg="RMSE should be correctly calculated.")
def test_process_measurement_sets_x_of_ekf_if_first_measurement_is_RADAR(self):
ro = 1
theta = 2
meas_package = MeasurementPackage(0,
MeasurementPackage.SensorType.RADAR,
Matrix([[ro], [theta], [0.5]]))
fusionEKF = FusionEKF(self._ekf, self._tools)
fusionEKF.process_measurement(meas_package)
initial_x = Matrix([[ro*cos(theta)], [ro*sin(theta)], [0], [0]])
self.assertEqual(self._ekf._x.value, initial_x.value)
def test_calculate_jacobian_returns0_if_px_and_py_is0(self):
state = Matrix([[]])
state.zero(4, 1)
expected_Hj = Matrix([[]])
expected_Hj.zero(3, 4)
Hj = self._tools.calculate_jacobian(state)
self.assertEqual(Hj.value, expected_Hj.value,
"Jacobian should be 0 when px and py is 0.")
def update_ekf(self, z):
"""Updates the state by using Extended Kalman Filter equations
Args:
z (:obj:`Matrix`): The measurement at k+1
"""
"""
Todo:
* update the state by using Extended Kalman Filter equations
"""
#Lesson 5 Section 14
px = self._x.value[0][0]
py = self._x.value[1][0]
vx = self._x.value[2][0]
vy = self._x.value[3][0]
rho = sqrt(px * px + py * py)
theta = atan2(py, px)
ro_dot = (px * vx + py * vy) / rho
z_pred = Matrix([[rho], [theta], [ro_dot]])
y = z - z_pred
if (y.value[1][0] > pi):
y.value[1][0] -= 2 * pi
elif (y.value[1][0] < (-pi)):
y.value[1][0] += 2 * pi
#Lesson 5 Section 7
Ht = self._H.transpose()
S = self._H * self._P * Ht + self._R
Si = S.inverse()
PHt = self._P * 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
def test_process_measurement_x_and_P_of_ekf_calculated_for_test_measurements(self):
for measurements, expected_x, expected_P in [([MeasurementPackage(1477010443000000,
MeasurementPackage.SensorType.LASER,
Matrix([[0.463227], [0.607415]])),
MeasurementPackage(1477010443100000,
MeasurementPackage.SensorType.LASER,
Matrix([[0.968521], [0.40545]])),
MeasurementPackage(1477010443200000,
MeasurementPackage.SensorType.LASER,
Matrix([[0.947752], [0.636824]])),
MeasurementPackage(1477010443300000,
MeasurementPackage.SensorType.LASER,
Matrix([[1.42287], [0.264328]]))],
Matrix([[1.34291], [0.364408], [2.32002], [-0.722813]]),
Matrix([[0.0185328, 0, 0.109639, 0],
[0, 0.0185328, 0, 0.109639],
[0.109639, 0, 1.10798, 0],
[0, 0.109639, 0, 1.10798]]))]:
with self.subTest():
tools = Tools()
ekf = KalmanFilter()
fusionEKF = FusionEKF(ekf, tools)
for measurement in measurements:
fusionEKF.process_measurement(measurement)
assert_array_almost_equal(ekf._x.value, expected_x.value, decimal=2)
assert_array_almost_equal(ekf._P.value, expected_P.value, decimal=1)
def calculate_rmse(self, estimations, ground_truth):
"""
Todo:
* Calculate the RMSE here.
"""
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 calculate_jacobian(self, x_state):
"""
Todo:
* Calculate a Jacobian here.
"""
Hj = Matrix([[]])
Hj.zero(3, 4)
#recover state parameters
px = x_state.value[0][0]
py = x_state.value[1][0]
vx = x_state.value[2][0]
vy = x_state.value[3][0]
#pre-compute a set of terms to avoid repeated calculation
c1 = px * px + py * py
c2 = sqrt(c1)
c3 = (c1 * c2)
#check division by zero
if (abs(c1) < 0.0001):
print("CalculateJacobian () - Error - Division by Zero")
return Hj
#compute the Jacobian matrix
Hj = Matrix([[(px / c2), (py / c2), 0, 0],
[-(py / c1), (px / c1), 0, 0],
[
py * (vx * py - vy * px) / c3,
px * (px * vy - py * vx) / c3, px / c2, py / c2
]])
return Hj
def test_calculate_rmse_returns0_if_estimations_size_differ(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 update(self, z):
"""Updates the state by using standard Kalman Filter equations
Args:
z (:obj:`Matrix`): The measurement at k+1
"""
"""
Todo:
* update the state by using Kalman Filter equations
"""
z_pred = self._H * self._x
y = z - z_pred
Ht = self._H.transpose()
S = self._H * self._P * Ht + self._R
Si = S.inverse()
PHt = self._P * 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
def __init__(self):
"""Constructor"""
#: obj`Matrix`: state vector
self._x = Matrix([[]])
#: obj`Matrix`: state covariance matrix
self._P = Matrix([[]])
#: obj`Matrix`: state transition matrix
self._F = Matrix([[]])
#: obj`Matrix`: process covariance matrix
self._Q = Matrix([[]])
#: obj`Matrix`: measurement matrix
self._H = Matrix([[]])
#: obj`Matrix`: measurement covariance matrix
self._R = Matrix([[]])
def test_constructor_calls_init_on_ekf(self):
self._ekf.init = MagicMock()
fusionEKF = FusionEKF(self._ekf, self._tools)
self._ekf.init.assert_called_with(
Matrix([[0], [0], [0], [0]]),
Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1000, 0],
[0, 0, 0, 1000]]),
Matrix([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]),
Matrix([[1, 0, 0, 0], [0, 1, 0, 0]]),
Matrix([[0.0225, 0], [0, 0.0225]]),
Matrix([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]))
def test_calculate_rmse_returns_correct_rmse_for_test_estimations(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 test_process_measurement_calls_predict_then_update_ekf_on_ekf_for_subsequent_RADAR_measurements(
self):
self._ekf.predict = MagicMock()
self._ekf.update_ekf = MagicMock()
self._tools.calculate_jacobian = MagicMock(return_value=Matrix(
[[0.8, 0.6, 0, 0], [-0.6, 0.8, 0, 0], [0, 0, 0.8, 0.6]]))
ekf_sequence = Mock()
ekf_sequence.attach_mock(self._ekf.predict, 'predict')
ekf_sequence.attach_mock(self._tools.calculate_jacobian,
'calculate_jacobian')
ekf_sequence.attach_mock(self._ekf.update_ekf, 'update_ekf')
fusionEKF = FusionEKF(self._ekf, self._tools)
meas_package = MeasurementPackage(
1477010443050000, MeasurementPackage.SensorType.RADAR,
Matrix([[0.898658], [0.617674], [1.7986]]))
fusionEKF.process_measurement(meas_package)
meas_package = MeasurementPackage(
1477010443150000, MeasurementPackage.SensorType.RADAR,
Matrix([[0.910574], [0.610537], [1.46233]]))
fusionEKF.process_measurement(meas_package)
expected_R = Matrix([[0.09, 0, 0], [0, 0.0009, 0], [0, 0, 0.09]])
expected_H = Matrix([[0.8, 0.6, 0, 0], [-0.6, 0.8, 0, 0],
[0, 0, 0.8, 0.6]])
self.assertEqual(self._ekf._R.value, expected_R.value)
self.assertEqual(self._ekf._H.value, expected_H.value)
assert ekf_sequence.mock_calls == [
call.predict(),
call.calculate_jacobian(
Matrix([[0.7326109317880749], [0.5204492516937732], [0],
[0]])),
call.update_ekf(Matrix([[0.910574], [0.610537], [1.46233]]))
]
class FusionEKF:
def __init__(self, ekf, tools):
"""Constructor"""
#: obj`KalmanFilter`: Kalman Filter update and prediction math lives in here.
self._ekf = ekf
#: obj`Tools`: tool object used to compute Jacobian and RMSE
self._tools = tools
#: bool: check whether the tracking toolbox was initialized or not (first measurement)
self._is_initialized = False
#: long: previous timestamp
self._previous_timestamp = 0
#initializing matrices
self._R_laser = Matrix([[]])
self._R_radar = Matrix([[]])
self._H_laser_ = Matrix([[]])
self._Hj = Matrix([[]])
self._Hj.zero(3, 4)
#measurement covariance matrix - laser
self._R_laser = Matrix([[0.0225, 0], [0, 0.0225]])
#measurement covariance matrix - radar
self._R_radar = Matrix([[0.09, 0, 0], [0, 0.0009, 0], [0, 0, 0.09]])
"""
Todo:
* Finish initializing the FusionEKF.
* Set the process and measurement noises
"""
#initialize variables and matrices (x, F, H_laser, H_jacobian, P, etc.)
#create a 4D state vector, we don't know yet the values of the x state
x_in = Matrix([[]])
x_in.zero(4, 1)
#state covariance matrix P
P_in = Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1000, 0],
[0, 0, 0, 1000]])
#measurement matrix
self._H_laser = Matrix([[1, 0, 0, 0],
[0, 1, 0, 0]]) # From Lesson 5 Section 10.
#the initial transition matrix F_
F_in = Matrix([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]])
Q_in = Matrix([[]])
Q_in.zero(4, 4)
self._ekf.init(x_in, P_in, F_in, self._H_laser, self._R_laser, Q_in)
def process_measurement(self, measurement_pack):
"""Run the whole flow of the Kalman Filter from here."""
"""
Initialization
"""
if not self._is_initialized:
"""
Todo:
* Initialize the state ekf_.x_ with the first measurement.
* Create the covariance matrix.
You'll need to convert radar from polar to cartesian coordinates.
"""
# first measurement
print("EKF: ")
self._ekf._x = Matrix([[1], [1], [1], [1]])
#initialize the Kalman filter position vector with the first sensor measurements
if measurement_pack._sensor_type == MeasurementPackage.SensorType.RADAR:
"""
Convert radar from polar to cartesian coordinates and initialize state.
"""
ro = measurement_pack._raw_measurements.value[0][0]
theta = measurement_pack._raw_measurements.value[1][0]
self._ekf._x = Matrix([[ro * cos(theta)], [ro * sin(theta)],
[0], [0]])
elif measurement_pack._sensor_type == MeasurementPackage.SensorType.LASER:
"""
Initialize state.
"""
#set the state with the initial location and zero velocity
self._ekf._x = Matrix([
measurement_pack._raw_measurements.value[0],
measurement_pack._raw_measurements.value[1], [0], [0]
])
self._previous_timestamp = measurement_pack._timestamp
# done initializing, no need to predict or update
self._is_initialized = True
return
"""
Prediction
"""
"""
Todo:
* Update the state transition matrix F according to the new elapsed time.
Time is measured in seconds.
Todo:
* Update the process noise covariance matrix.
Use noise_ax = 9 and noise_ay = 9 for your Q matrix.
"""
# modify the F and Q matrices prior to the prediction step based on the
# elapsed time between measurements
# compute the time elapsed between the current and previous measurements
dt = (measurement_pack._timestamp -
self._previous_timestamp) / 1000000.0 #dt - expressed in seconds
self._previous_timestamp = measurement_pack._timestamp
dt_2 = dt * dt
dt_3 = dt_2 * dt
dt_4 = dt_3 * dt
#Modify the F matrix so that the time is integrated
#Lesson 5 Section 8
self._ekf._F.value[0][2] = dt
self._ekf._F.value[1][3] = dt
#acceleration noise components
noise_ax = 9
noise_ay = 9
#set the process covariance matrix Q
#Lesson 5 Section 9
self._ekf._Q = Matrix(
[[dt_4 / 4 * noise_ax, 0, dt_3 / 2 * noise_ax, 0],
[0, dt_4 / 4 * noise_ay, 0, dt_3 / 2 * noise_ay],
[dt_3 / 2 * noise_ax, 0, dt_2 * noise_ax, 0],
[0, dt_3 / 2 * noise_ay, 0, dt_2 * noise_ay]])
self._ekf.predict()
"""
Update
"""
"""
Todo:
* Use the sensor type to perform the update step.
* Update the state and covariance matrices.
"""
if measurement_pack._sensor_type == MeasurementPackage.SensorType.RADAR:
# Radar updates
#set ekf_.H_ by setting to Hj which is the calculated the jacobian
#set ekf_.R_ by just using R_radar_
self._Hj = self._tools.calculate_jacobian(self._ekf._x)
self._ekf._H = self._Hj
self._ekf._R = self._R_radar
self._ekf.update_ekf(measurement_pack._raw_measurements)
else:
# Laser updates
#set ekf_.H_ by just using H_laser_
#set ekf_.R_ by just using R_laser_
self._ekf._H = self._H_laser
self._ekf._R = self._R_laser
self._ekf.update(measurement_pack._raw_measurements)
# print the output
print("x_ = ", self._ekf._x)
print("P_ = ", self._ekf._P)
def process_measurement(self, measurement_pack):
"""Run the whole flow of the Kalman Filter from here."""
"""
Initialization
"""
if not self._is_initialized:
"""
Todo:
* Initialize the state ekf_.x_ with the first measurement.
* Create the covariance matrix.
You'll need to convert radar from polar to cartesian coordinates.
"""
# first measurement
print("EKF: ")
self._ekf._x = Matrix([[1], [1], [1], [1]])
#initialize the Kalman filter position vector with the first sensor measurements
if measurement_pack._sensor_type == MeasurementPackage.SensorType.RADAR:
"""
Convert radar from polar to cartesian coordinates and initialize state.
"""
ro = measurement_pack._raw_measurements.value[0][0]
theta = measurement_pack._raw_measurements.value[1][0]
self._ekf._x = Matrix([[ro * cos(theta)], [ro * sin(theta)],
[0], [0]])
elif measurement_pack._sensor_type == MeasurementPackage.SensorType.LASER:
"""
Initialize state.
"""
#set the state with the initial location and zero velocity
self._ekf._x = Matrix([
measurement_pack._raw_measurements.value[0],
measurement_pack._raw_measurements.value[1], [0], [0]
])
self._previous_timestamp = measurement_pack._timestamp
# done initializing, no need to predict or update
self._is_initialized = True
return
"""
Prediction
"""
"""
Todo:
* Update the state transition matrix F according to the new elapsed time.
Time is measured in seconds.
Todo:
* Update the process noise covariance matrix.
Use noise_ax = 9 and noise_ay = 9 for your Q matrix.
"""
# modify the F and Q matrices prior to the prediction step based on the
# elapsed time between measurements
# compute the time elapsed between the current and previous measurements
dt = (measurement_pack._timestamp -
self._previous_timestamp) / 1000000.0 #dt - expressed in seconds
self._previous_timestamp = measurement_pack._timestamp
dt_2 = dt * dt
dt_3 = dt_2 * dt
dt_4 = dt_3 * dt
#Modify the F matrix so that the time is integrated
#Lesson 5 Section 8
self._ekf._F.value[0][2] = dt
self._ekf._F.value[1][3] = dt
#acceleration noise components
noise_ax = 9
noise_ay = 9
#set the process covariance matrix Q
#Lesson 5 Section 9
self._ekf._Q = Matrix(
[[dt_4 / 4 * noise_ax, 0, dt_3 / 2 * noise_ax, 0],
[0, dt_4 / 4 * noise_ay, 0, dt_3 / 2 * noise_ay],
[dt_3 / 2 * noise_ax, 0, dt_2 * noise_ax, 0],
[0, dt_3 / 2 * noise_ay, 0, dt_2 * noise_ay]])
self._ekf.predict()
"""
Update
"""
"""
Todo:
* Use the sensor type to perform the update step.
* Update the state and covariance matrices.
"""
if measurement_pack._sensor_type == MeasurementPackage.SensorType.RADAR:
# Radar updates
#set ekf_.H_ by setting to Hj which is the calculated the jacobian
#set ekf_.R_ by just using R_radar_
self._Hj = self._tools.calculate_jacobian(self._ekf._x)
self._ekf._H = self._Hj
self._ekf._R = self._R_radar
self._ekf.update_ekf(measurement_pack._raw_measurements)
else:
# Laser updates
#set ekf_.H_ by just using H_laser_
#set ekf_.R_ by just using R_laser_
self._ekf._H = self._H_laser
self._ekf._R = self._R_laser
self._ekf.update(measurement_pack._raw_measurements)
# print the output
print("x_ = ", self._ekf._x)
print("P_ = ", self._ekf._P)
def telemetry(sid, data):
if data:
j = data
sensor_measurment = j["sensor_measurement"]
meas_package = MeasurementPackage()
tokens = sensor_measurment.split()
i = 0
# reads first element from the current line
sensor_type = tokens[i]
i += 1
if (sensor_type == "L"):
meas_package._sensor_type = MeasurementPackage.SensorType.LASER
px = float(tokens[i])
py = float(tokens[i + 1])
meas_package._raw_measurements = Matrix([[px], [py]])
timestamp = int(tokens[i + 2])
meas_package._timestamp = timestamp
i += 3
elif (sensor_type == "R"):
meas_package._sensor_type = MeasurementPackage.SensorType.RADAR
ro = float(tokens[i])
theta = float(tokens[i + 1])
ro_dot = float(tokens[i + 2])
meas_package._raw_measurements = Matrix([[ro], [theta], [ro_dot]])
timestamp = int(tokens[i + 3])
meas_package._timestamp = timestamp
i += 4
x_gt = float(tokens[i])
y_gt = float(tokens[i + 1])
vx_gt = float(tokens[i + 2])
vy_gt = float(tokens[i + 3])
gt_values = Matrix([[x_gt], [y_gt], [vx_gt], [vy_gt]])
ground_truth.append(gt_values)
#Call ProcessMeasurment(meas_package) for Kalman filter
fusionEKF.process_measurement(meas_package)
#Push the current estimated x,y positon from the Kalman filter's state vector
p_x = fusionEKF._ekf._x.value[0][0]
p_y = fusionEKF._ekf._x.value[1][0]
v1 = fusionEKF._ekf._x.value[2][0]
v2 = fusionEKF._ekf._x.value[3][0]
estimate = Matrix([[p_x], [p_y], [v1], [v2]])
estimations.append(estimate)
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 __init__(self, ekf, tools):
"""Constructor"""
#: obj`KalmanFilter`: Kalman Filter update and prediction math lives in here.
self._ekf = ekf
#: obj`Tools`: tool object used to compute Jacobian and RMSE
self._tools = tools
#: bool: check whether the tracking toolbox was initialized or not (first measurement)
self._is_initialized = False
#: long: previous timestamp
self._previous_timestamp = 0
#initializing matrices
self._R_laser = Matrix([[]])
self._R_radar = Matrix([[]])
self._H_laser_ = Matrix([[]])
self._Hj = Matrix([[]])
self._Hj.zero(3, 4)
#measurement covariance matrix - laser
self._R_laser = Matrix([[0.0225, 0], [0, 0.0225]])
#measurement covariance matrix - radar
self._R_radar = Matrix([[0.09, 0, 0], [0, 0.0009, 0], [0, 0, 0.09]])
"""
Todo:
* Finish initializing the FusionEKF.
* Set the process and measurement noises
"""
#initialize variables and matrices (x, F, H_laser, H_jacobian, P, etc.)
#create a 4D state vector, we don't know yet the values of the x state
x_in = Matrix([[]])
x_in.zero(4, 1)
#state covariance matrix P
P_in = Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1000, 0],
[0, 0, 0, 1000]])
#measurement matrix
self._H_laser = Matrix([[1, 0, 0, 0],
[0, 1, 0, 0]]) # From Lesson 5 Section 10.
#the initial transition matrix F_
F_in = Matrix([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]])
Q_in = Matrix([[]])
Q_in.zero(4, 4)
self._ekf.init(x_in, P_in, F_in, self._H_laser, self._R_laser, Q_in)
class KalmanFilter:
def __init__(self):
"""Constructor"""
#: obj`Matrix`: state vector
self._x = Matrix([[]])
#: obj`Matrix`: state covariance matrix
self._P = Matrix([[]])
#: obj`Matrix`: state transition matrix
self._F = Matrix([[]])
#: obj`Matrix`: process covariance matrix
self._Q = Matrix([[]])
#: obj`Matrix`: measurement matrix
self._H = Matrix([[]])
#: obj`Matrix`: measurement covariance matrix
self._R = Matrix([[]])
def init(self, x_in, P_in, F_in, H_in, R_in, Q_in):
"""Initializes Kalman filter
Args:
x_in (:obj:`Matrix`): Initial state
P_in (:obj:`Matrix`): Initial state covariance
F_in (:obj:`Matrix`): Transition matrix
H_in (:obj:`Matrix`): Measurement matrix
R_in (:obj:`Matrix`): Measurement covariance matrix
Q_in (:obj:`Matrix`): Process covariance matrix
"""
self._x = x_in
self._P = P_in
self._F = F_in
self._H = H_in
self._R = R_in
self._Q = Q_in
def predict(self):
"""Predicts the state and the state covariance using the process model
Args:
delta_T (long): Time between k and k+1 in s
"""
"""
Todo:
* predict the state
"""
self._x = self._F * self._x # Lesson 5 Section 8
Ft = self._F.transpose()
self._P = self._F * self._P * Ft + self._Q # Lesson 5 Section 9
def update(self, z):
"""Updates the state by using standard Kalman Filter equations
Args:
z (:obj:`Matrix`): The measurement at k+1
"""
"""
Todo:
* update the state by using Kalman Filter equations
"""
z_pred = self._H * self._x
y = z - z_pred
Ht = self._H.transpose()
S = self._H * self._P * Ht + self._R
Si = S.inverse()
PHt = self._P * 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
def update_ekf(self, z):
"""Updates the state by using Extended Kalman Filter equations
Args:
z (:obj:`Matrix`): The measurement at k+1
"""
"""
Todo:
* update the state by using Extended Kalman Filter equations
"""
#Lesson 5 Section 14
px = self._x.value[0][0]
py = self._x.value[1][0]
vx = self._x.value[2][0]
vy = self._x.value[3][0]
rho = sqrt(px * px + py * py)
theta = atan2(py, px)
ro_dot = (px * vx + py * vy) / rho
z_pred = Matrix([[rho], [theta], [ro_dot]])
y = z - z_pred
if (y.value[1][0] > pi):
y.value[1][0] -= 2 * pi
elif (y.value[1][0] < (-pi)):
y.value[1][0] += 2 * pi
#Lesson 5 Section 7
Ht = self._H.transpose()
S = self._H * self._P * Ht + self._R
Si = S.inverse()
PHt = self._P * 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
def test_fusion_ekf_passes_project_rubric_for_dataset1(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='|')
tools = Tools()
ekf = KalmanFilter()
# Create a Fusion EKF instance
fusionEKF = FusionEKF(ekf, 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)
fusionEKF.process_measurement(meas_package)
p_x = fusionEKF._ekf._x.value[0][0]
p_y = fusionEKF._ekf._x.value[1][0]
v1 = fusionEKF._ekf._x.value[2][0]
v2 = fusionEKF._ekf._x.value[3][0]
estimate = Matrix([[p_x], [p_y], [v1], [v2]])
estimations.append(estimate)
# compute the accuracy (RMSE)
expected_rmse = Matrix([[0.11], [0.11], [0.52], [0.52]])
rmse = tools.calculate_rmse(estimations, ground_truth)
assert_array_less(rmse.value, expected_rmse.value)
in_file.close()
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