def test_project_sigma_points(self):
"""
This method tests the function that projects the sigma points
by running a simulation.
"""
# Initialize the first order model
self.set_first_order_model()
# Associate inputs and outputs
self.set_first_order_model_input_outputs(noisy=False)
# Define the variables to estimate
self.set_state_to_estimate_first_order()
# Initialize the simulator
self.m.initialize_simulator()
# Instantiate with a proper model
ukf_FMU = UkfFmu(self.m)
# Define the sigma points
x0 = np.array([2.5])
sigma_points = ukf_FMU.compute_sigma_points(x0, np.array([]),
np.diag(np.ones(1)))
# Propagate the points by simulating from 0 to 14.5 seconds
t0 = pd.to_datetime(0.0, unit="s", utc=True)
t1 = pd.to_datetime(14.5, unit="s", utc=True)
X_proj, Z_proj, Xfull_proj, Zfull_proj = ukf_FMU.sigma_point_proj(
sigma_points, t0, t1)
# Verify that they started from different initial coditions and that they converged
# at the same value after 12 seconds
np.testing.assert_almost_equal(
X_proj, 2.5 * np.ones((3, 1)), 3,
"Verify that the solutions all converge to 2.5")
# Compute their average using the method provided by the object and verify its value
x_avg = ukf_FMU.average_proj(X_proj)
np.testing.assert_almost_equal(
x_avg, np.array([[2.5]]), 4,
"Average of the propagated points is not correct")
return
def test_create_sigma_points(self):
"""
This method tests the Cholesky update method that is used to compute
the squared root covariance matrix by the filter.
"""
# Initialize the first order model
self.set_first_order_model()
# Associate inputs and outputs
self.set_first_order_model_input_outputs()
# Define the variables to estimate
self.set_state_to_estimate_first_order()
# Instantiate with a proper model
ukf_FMU = UkfFmu(self.m)
# Verify that the method raises an exception if the
# inputs are wrong
self.assertRaises(ValueError, ukf_FMU.compute_sigma_points,
np.zeros(3), np.zeros(3), np.zeros((3, 3)))
self.assertRaises(ValueError, ukf_FMU.compute_sigma_points,
np.zeros(2), np.zeros(3), np.zeros((3, 3)))
self.assertRaises(ValueError, ukf_FMU.compute_sigma_points,
np.zeros(1), np.array([]), np.zeros((3, 3)))
# Create the sigma points
x0 = np.array([2.5])
sigma_points = ukf_FMU.compute_sigma_points(x0, np.array([]),
np.diag(np.ones(1)))
# Check the size
self.assertTrue(sigma_points.shape == (3, 1),
"The size of the sigma points is not correct")
# Verify that the first sigma point is equal to the center
self.assertEqual(x0, sigma_points[0, :],
"First sigma point is not [0]")
# Verify that the second and the last sigma points are symmetric
self.assertEqual(
0.5 * (sigma_points[1, :] + sigma_points[2, :]), x0,
"The sigma points 1,2 are not symmetric with respect to 0")
return
def test_project_sigma_points(self):
"""
This method tests the function that projects the sigma points
by running a simulation.
"""
# Initialize the first order model
self.set_first_order_model()
# Associate inputs and outputs
self.set_first_order_model_input_outputs(noisy = False)
# Define the variables to estimate
self.set_state_to_estimate_first_order()
# Initialize the simulator
self.m.initialize_simulator()
# Instantiate with a proper model
ukf_FMU = UkfFmu(self.m)
# Define the sigma points
x0 = np.array([2.5])
sigma_points = ukf_FMU.compute_sigma_points(x0, np.array([]), np.diag(np.ones(1)))
# Propagate the points by simulating from 0 to 14.5 seconds
t0 = pd.to_datetime(0.0, unit = "s", utc = True)
t1 = pd.to_datetime(14.5, unit = "s", utc = True)
X_proj, Z_proj, Xfull_proj, Zfull_proj = ukf_FMU.sigma_point_proj(sigma_points, t0, t1)
# Verify that they started from different initial coditions and that they converged
# at the same value after 12 seconds
np.testing.assert_almost_equal(X_proj, 2.5*np.ones((3,1)), 3, "Verify that the solutions all converge to 2.5")
# Compute their average using the method provided by the object and verify its value
x_avg = ukf_FMU.average_proj(X_proj)
np.testing.assert_almost_equal(x_avg, np.array([[2.5]]), 4, "Average of the propagated points is not correct")
return
def test_create_sigma_points(self):
"""
This method tests the Cholesky update method that is used to compute
the squared root covariance matrix by the filter.
"""
# Initialize the first order model
self.set_first_order_model()
# Associate inputs and outputs
self.set_first_order_model_input_outputs()
# Define the variables to estimate
self.set_state_to_estimate_first_order()
# Instantiate with a proper model
ukf_FMU = UkfFmu(self.m)
# Verify that the method raises an exception if the
# inputs are wrong
self.assertRaises(ValueError, ukf_FMU.compute_sigma_points, np.zeros(3), np.zeros(3), np.zeros((3,3)))
self.assertRaises(ValueError, ukf_FMU.compute_sigma_points, np.zeros(2), np.zeros(3), np.zeros((3,3)))
self.assertRaises(ValueError, ukf_FMU.compute_sigma_points, np.zeros(1), np.array([]), np.zeros((3,3)))
# Create the sigma points
x0 = np.array([2.5])
sigma_points = ukf_FMU.compute_sigma_points(x0, np.array([]), np.diag(np.ones(1)))
# Check the size
self.assertTrue(sigma_points.shape == (3,1), "The size of the sigma points is not correct")
# Verify that the first sigma point is equal to the center
self.assertEqual(x0, sigma_points[0,:], "First sigma point is not [0]")
# Verify that the second and the last sigma points are symmetric
self.assertEqual(0.5*(sigma_points[1,:] + sigma_points[2,:]), x0, "The sigma points 1,2 are not symmetric with respect to 0")
return
