class Test_AlgorithmMethods_UKF:
def setUp(self):
self.model = load_fmu(get_fmu())
self.x_0 = {'x2': 0, 'x1': 1}
measurements = ['x1']
h = 0.01
self.options = UKFOptions()
self.options.update(P_0={'x2': 1, 'x1': 2})
self.options.update(P_v={'x2': 1e-3, 'x1': 1e-1})
self.options.update(P_n={'x1': 1e-4})
self.ukf = UKF(self.model, self.x_0, measurements, h, self.options)
def tearDown(self):
self.model = None
self.ukf = None
self.x_0 = None
self.options = None
def test_calc_sigma(self):
#Test that sigma points are calculated correctly
sigma = self.ukf._calc_sigma(self.ukf.x, self.ukf.P, self.ukf.P_v,
self.ukf.P_n, self.ukf.options)
assert N.allclose(
sigma,
N.array([[1.0, 1.002000000000029, 1.0, 0.997999999999971, 1.0],
[0.0, 0.0, 0.001414213562393, 0.0, -0.001414213562393]]))
def test_update_private(self):
#Test that the private update method is performed correctly
K = N.array([[0.1], [0.3]])
xp = N.array([[1.2], [0.1]])
yp = N.array([[1.18]])
y = N.array([[1.24]])
assert N.allclose(self.ukf._update(y, xp, yp, K),
N.array([[1.206000000000000], [0.118000000000000]]))
def test_update_public(self):
#Test that the public update method calculates estimates correctly
self.ukf.K = N.array([[0.1], [0.3]])
self.ukf.xp = N.array([[1.2], [0.1]])
self.ukf.yp = N.array([[1.18]])
x = self.ukf.update({'x1': 1.24})
assert N.allclose(x['x1'], 1.206000000000000)
assert N.allclose(x['x2'], 0.118000000000000)
def test_predict_private(self):
#Produce sigma points
sigma = self.ukf._calc_sigma(self.ukf.x, self.ukf.P, self.ukf.P_v,
self.ukf.P_n, self.ukf.options)
#Have a constant input of 0.1 over the sample interval
u = (['u'], N.transpose(N.vstack((0.0, 0.1))))
#No known state values
known_values = {}
#Do prediction
[xp, yp, K,
P] = self.ukf._predict(sigma, self.ukf.model, self.ukf.x, u,
known_values, self.ukf.P_v, self.ukf.P_n,
self.ukf.currTime, self.ukf.h, self.ukf.mes,
self.ukf.Wm, self.ukf.Wc)
#Assert predicted mean and measurement
assert N.allclose(xp, [[1.00988634], [0.0172094]])
assert N.allclose(yp, N.array([[1.00988634]]))
#Assert covariance and gain
assert N.allclose(K, [[0.99995099], [0.00497003]])
assert N.allclose(P, [[1.00099995e-01, 4.97003235e-07],
[4.97003235e-07, 1.00115269e+00]])
def test_predict_public(self):
#Have a constant input of 0.1 over the sample interval
u = (['u'], N.transpose(N.vstack((0.0, 0.1))))
#No known state values
known_values = {}
self.ukf.predict(u, known_values)
#Assert predicted mean and measurement
assert N.allclose(self.ukf.xp, [[1.00988634], [0.0172094]])
assert N.allclose(self.ukf.yp, [[1.00988634]])
#Assert covariance and gain
assert N.allclose(self.ukf.K, [[0.99995099], [0.00497003]])
assert N.allclose(self.ukf.P, [[1.00099995e-01, 4.97003235e-07],
[4.97003235e-07, 1.00115269e+00]])
class Test_Create_UKF:
def setUp(self):
self.model = load_fmu(get_fmu())
self.x_0 = {'x2': 0, 'x1': 1}
measurements = ['x1']
h = 0.01
self.options = UKFOptions()
self.options.update(P_0={'x2': 1, 'x1': 2})
self.options.update(P_v={'x2': 1e-3, 'x1': 1e-1})
self.options.update(P_n={'x1': 1e-4})
self.ukf = UKF(self.model, self.x_0, measurements, h, self.options)
def tearDown(self):
self.model = None
self.ukf = None
self.x_0 = None
self.options = None
def test_create_ukf(self):
#Test that initialization of scaled state estimate vector is correct
names = []
for state in self.ukf.x:
names = names + [state.get_name()]
assert names == ['x1', 'x2']
for state in self.ukf.x:
assert state.get_actual_value() == self.x_0[state.get_name()]
assert type(state.get_actual_value()) == float
assert state.get_nominal_value(
) == self.model.get_variable_nominal(state.get_name())
assert type(state.get_nominal_value()) == float
#Test that initialization of measurement vector is correct
names = []
for meas in self.ukf.mes:
names = names + [meas.get_name()]
assert names == ['x1']
for meas in self.ukf.mes:
assert meas.get_nominal_value() == self.model.get_variable_nominal(
meas.get_name())
assert type(meas.get_nominal_value()) == float
#Test that initialization of covariance matrices is correct
assert N.all(self.ukf.P == N.array([[
2.0 / self.model.get_variable_nominal('x1'), 0.0
], [0.0, 1.0 / self.model.get_variable_nominal('x2')]]))
assert N.all(self.ukf.P_v == N.array([[
1e-1 / self.model.get_variable_nominal('x1'), 0.0
], [0.0, 1e-3 / self.model.get_variable_nominal('x2')]]))
assert N.all(self.ukf.P_n == N.array(
[1e-4 / self.model.get_variable_nominal('x1')]))
def test_calc_weights(self):
#Test that the weights are calculated correctly
[Wm, Wc] = self.ukf._calc_weights(self.options)
assert N.all(Wm == N.array([
-9.999989999712444e05, 2.499999999928111e05, 2.499999999928111e05,
2.499999999928111e05, 2.499999999928111e05
]))
assert N.all(Wc == N.array([
-9.999959999722444e+05, 2.499999999928111e05, 2.499999999928111e05,
2.499999999928111e05, 2.499999999928111e05
]))
def test_update_options(self):
#Test that weights and covariances are updated correctly
self.ukf.update_options(alpha=1.0,
beta=0.0,
P_v={
'x2': 1e-2,
'x1': 1e-3
})
assert N.all(self.ukf.Wm == N.array([0.0, 0.25, 0.25, 0.25, 0.25]))
assert N.all(self.ukf.Wc == N.array([0.0, 0.25, 0.25, 0.25, 0.25]))
assert N.all(self.ukf.P_v == N.array([[
1e-3 / self.model.get_variable_nominal('x1'), 0.0
], [0.0, 1e-2 / self.model.get_variable_nominal('x2')]]))
def test_get_options(self):
assert self.options == self.ukf.get_options()
def run_demo():
#Compile model used in tests to fmu
fmu = compile_fmu('VDP_pack.VDP',
get_files_path() + '/Modelica/VDP.mop',
separate_process=True)
model = load_fmu(fmu) #Observer model
process = load_fmu(fmu) #Actual process
#Set options for UKF
opt = UKFOptions()
v1 = 1e-3
v2 = 1e-3
n1 = 0.5
P_v = {'x1': v1, 'x2': v2} #Covariance of process noise
P_n = {'x1': n1} #covariance of measurement noise
P_0 = {'x1': 1.0, 'x2': 1.0} #Initial state covariance
alpha = 1.0
beta = 2.0
kappa = 0.0
opt.update(alpha=alpha, beta=beta, kappa=kappa, P_0=P_0, P_v=P_v, P_n=P_n)
#Set initial state estimate, measured variables, and sampling interval
x1_0 = 1.0
x2_0 = -1.0
x_0 = {'x1': x1_0, 'x2': x2_0}
measurements = ['x1']
h = 0.1
#Create a UKF object
ukf = UKF(model, x_0, measurements, h, opt)
#Retrieve simulation options for the process
processOpt = process.simulate_options()
processOpt['CVode_options']['atol'] = 1e-8
processOpt['CVode_options']['rtol'] = 1e-6
#Save time vectors and state values for post-processing
t_sampled = [0.0]
t_sim = [0.0]
x1_process_sim = [process.get('x1_0')[-1]]
x2_process_sim = [process.get('x2_0')[-1]]
x1_process = [process.get('x1_0')[-1]]
x2_process = [process.get('x2_0')[-1]]
x1_estimate = [x1_0]
x2_estimate = [x2_0]
measurement_vector = [x1_0]
#Perform simulate iteratively for T samples, and estimate.
T = 100
tStart = 0.0
for i in range(0, T):
#Make a prediction with the UKF. No input or known state values
ukf.predict()
#Simulate process for one time-step
simResProcess = process.simulate(start_time=tStart,
final_time=tStart + h,
options=processOpt)
#Retrieve last state value of simulation and apply
#gaussian process noise
x1 = simResProcess['x1'][-1] + random.gauss(0, N.sqrt(v1))
x2 = simResProcess['x2'][-1] + random.gauss(0, N.sqrt(v2))
#Apply measurement noise to measurement. Form measurement input
meas = x1 + random.gauss(0, N.sqrt(n1))
y = {'x1': meas}
#Perform a measurement update and retrieve the estimates
x = ukf.update(y)
#Update the time-step
tStart = tStart + h
#Update post-processing vectors
t_sampled = t_sampled + [tStart]
t_sim = t_sim + simResProcess['time'].tolist()
x1_process = x1_process + [x1]
x2_process = x2_process + [x2]
x1_process_sim = x1_process_sim + simResProcess['x1'].tolist()
x2_process_sim = x2_process_sim + simResProcess['x2'].tolist()
x1_estimate = x1_estimate + [x['x1']]
x2_estimate = x2_estimate + [x['x2']]
measurement_vector = measurement_vector + [meas]
#Update process starting states for next iteration
process.reset()
process.set('x1_0', x1)
process.set('x2_0', x2)
#Compute MSE
MSEx1 = N.sum(
((N.array(x1_estimate) - N.array(x1_process))**2)) / len(x1_estimate)
MSEx2 = N.sum(
((N.array(x2_estimate) - N.array(x2_process))**2)) / len(x2_estimate)
print
print 'Mean square error x1: ' + str(MSEx1)
print 'Mean square error x2: ' + str(MSEx2)
print
#Plot simulation results
fig1 = plt.figure(1)
fig1.clear()
fig1.hold(True)
ax1 = fig1.add_subplot(211)
ax1.set_title('UKF Estimation of the VDP-process')
actualx1, = ax1.plot(t_sim, x1_process_sim, 'b', label='true')
estimatex1, = ax1.plot(t_sampled, x1_estimate, 'go', label='UKF')
measurex1, = ax1.plot(t_sampled,
measurement_vector,
'rx',
label='Measurement')
ax1.set_xlabel('time (sec)')
ax1.set_ylabel('x1')
ax1.set_xlim([0, h * T])
ax1.legend([actualx1, estimatex1, measurex1],
["true", "UKF", "Measurement"])
ax2 = fig1.add_subplot(212)
ax2.plot(t_sim, x2_process_sim, 'b')
ax2.plot(t_sampled, x2_estimate, 'go')
ax2.set_ylabel('x2')
ax2.set_xlim([0, h * T])
ax2.set_xlabel('time (sec)')
fig1.show()
fig2 = plt.figure(2)
fig2.clear()
fig2.hold(True)
ax3 = fig2.add_subplot(211)
ax3.set_title('Estimation Errors')
x1err = ax3.plot(t_sampled,
N.array(x1_estimate) - N.array(x1_process),
label='x1err')
ax3.set_xlabel('time (sec)')
ax3.set_ylabel('Error x1')
ax3.set_xlim([0, h * T])
ax4 = fig2.add_subplot(212)
x2err = plt.plot(t_sampled,
N.array(x2_estimate) - N.array(x2_process),
label='x2err')
ax4.set_ylabel('Error x2')
ax4.set_xlim([0, h * T])
ax4.set_xlabel('time (sec)')
fig2.show()