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 setUp(self):
self._tools = Tools()
class TestTools(unittest.TestCase):
def setUp(self):
self._tools = Tools()
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_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 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 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_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.")
import socketio
import eventlet
import eventlet.wsgi
from flask import Flask
import json
from ekf.matrix import Matrix
from ekf.measurement_package import MeasurementPackage
from ekf.kalman_filter import KalmanFilter
from ekf.fusion_ekf import FusionEKF
from ekf.tools import Tools
sio = socketio.Server()
app = Flask(__name__)
# used to compute the RMSE later
tools = Tools()
ekf = KalmanFilter()
# Create a Kalman Filter instance
fusionEKF = FusionEKF(ekf, tools)
estimations = []
ground_truth = []
@sio.on('telemetry')
def telemetry(sid, data):
if data:
j = data
sensor_measurment = j["sensor_measurement"]
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 setUp(self):
self._tools = Tools()
self._ekf = KalmanFilter()
sys.stdout = open(os.devnull, 'w')