source: branches/dev-5819/Python/src/tests_jmodelica/test_ukf.py @ 13819

Last change on this file since 13819 was 13819, checked in by randersson, 8 weeks ago

#5819 Updated relative tolerance in test from 1e-7 (default) to 1e-6 due to updates in both python and assimulo

File size: 12.6 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3
4#    Copyright (C) 2015 Modelon AB
5#
6#    This program is free software: you can redistribute it and/or modify
7#    it under the terms of the GNU General Public License as published by
8#    the Free Software Foundation, version 3 of the License.
9#
10#    This program is distributed in the hope that it will be useful,
11#    but WITHOUT ANY WARRANTY; without even the implied warranty of
12#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13#    GNU General Public License for more details.
14#
15#    You should have received a copy of the GNU General Public License
16#    along with this program.  If not, see <http://www.gnu.org/licenses/>.
17"""
18Tests for the UKF, UKFOptions and ScaledVariable classes.
19"""
20import sys
21from pyjmi.ukf import UKF, ScaledVariable, UKFOptions
22from tests_jmodelica import testattr, get_files_path
23from pymodelica import compile_fmu
24from pyfmi import load_fmu
25import numpy as N
26import random
27import os
28from pyfmi.common.algorithm_drivers import OptionBase, InvalidAlgorithmOptionException, UnrecognizedOptionError
29from nose.tools import assert_raises
30
31
32fmu = None
33def get_fmu():
34    global fmu
35    if fmu is None:
36    #Compile model used in tests to fmu
37        fmu = compile_fmu('VDP_pack.VDP',get_files_path()+'/Modelica/VDP.mop', separate_process = True)
38    return fmu
39
40
41@testattr(stddist_full = True)
42class Test_ScaledVariable:
43    def setUp(self):
44        self.var = ScaledVariable('x',100,8)
45   
46    def tearDown(self):
47        self.var = None
48       
49    def test_get(self): 
50        #Test get-methods
51        assert self.var.get_name() == 'x'
52        assert self.var.get_scaled_value() == 12.5
53        assert self.var.get_actual_value() == 100.0
54        assert self.var.get_nominal_value() == 8.0
55   
56    def test_set_actual(self):
57        #Test setting new actual value
58        self.var.set_actual_value(200)
59        assert self.var.get_name() == 'x'
60        assert self.var.get_scaled_value() == 25.0
61        assert self.var.get_actual_value() == 200.0
62        assert self.var.get_nominal_value() == 8.0
63       
64    def test_set_scaled(self):
65        #Test setting new scaled value
66        self.var.set_scaled_value(12.5)
67        assert self.var.get_name() == 'x'
68        assert self.var.get_scaled_value() == 12.5
69        assert self.var.get_actual_value() == 100
70        assert self.var.get_actual_value() == 100.0
71        assert self.var.get_nominal_value() == 8
72        assert self.var.get_nominal_value() == 8.0
73       
74    def test_string(self):
75        #Test string representation
76        assert str(self.var) == '{x | Value : 100.0 | Nominal : 8.0}\n'
77
78
79@testattr(stddist_full = True)
80class Test_UKFOptions:
81    def setUp(self):
82        self.opt = UKFOptions()
83   
84    def tearDown(self):
85        self.opt = None
86       
87    def test_setitem_correct_args(self):
88        #Test setting float and int values on parameters and covariances
89        self.opt.__setitem__('alpha', 1)
90        self.opt.__setitem__('beta', 3)
91        self.opt.__setitem__('kappa', -0.3)
92        self.opt.__setitem__('P_0', {'x1':0.1, 'x2':3.5})
93        self.opt.__setitem__('P_v', {'x1':0.3, 'x2':3.7})
94        self.opt.__setitem__('P_n', {'measurement':10})
95       
96        assert self.opt['alpha'] == 1.0
97        assert type(self.opt['alpha']) == float
98        assert self.opt['beta'] == 3.0
99        assert type(self.opt['beta']) == float
100        assert self.opt['kappa'] == -0.3
101        assert self.opt['P_0']['x1'] == 0.1
102        assert self.opt['P_0']['x2'] == 3.5
103        assert self.opt['P_v']['x1'] == 0.3
104        assert self.opt['P_v']['x2'] == 3.7
105        assert self.opt['P_n']['measurement'] == 10.0
106        assert type(self.opt['P_n']['measurement']) == float
107
108    def test_setitem_incorrect_args(self):
109        #Test setting string value instead of float/int
110        assert_raises(InvalidAlgorithmOptionException, self.opt.__setitem__, 'alpha', 'a')
111       
112        #Test setting list instead of dict
113        assert_raises(InvalidAlgorithmOptionException, self.opt.__setitem__, 'P_0', ['x1','1.0'])
114       
115        #Test setting non-existent argument
116        assert_raises(UnrecognizedOptionError, self.opt.__setitem__, 'P', {'x1':0.3})
117       
118        #Test setting wrong value in covariance dict
119        assert_raises(InvalidAlgorithmOptionException, self.opt.__setitem__, 'P_0', {'x1':'a'})
120
121    def test_update_correct_args(self):
122        #Test updating float and int values on parameters and covariances
123        self.opt.update(alpha=1)
124        self.opt.update(beta=3)
125        self.opt.update(kappa=-0.3)
126        self.opt.update(P_0= {'x1':0.1, 'x2':3.5})
127        self.opt.update(P_v={'x1':0.3, 'x2':3.7})
128        self.opt.update(P_n={'measurement':10})
129       
130        assert self.opt['alpha'] == 1.0
131        assert type(self.opt['alpha']) == float
132        assert self.opt['beta'] == 3.0
133        assert type(self.opt['beta']) == float
134        assert self.opt['kappa'] == -0.3
135        assert self.opt['P_0']['x1'] == 0.1
136        assert self.opt['P_0']['x2'] == 3.5
137        assert self.opt['P_v']['x1'] == 0.3
138        assert self.opt['P_v']['x2'] == 3.7
139        assert self.opt['P_n']['measurement'] == 10.0
140        assert type(self.opt['P_n']['measurement']) == float
141
142
143    def test_update_incorrect_args(self):
144        #Test setting string value instead of float/int
145        assert_raises(InvalidAlgorithmOptionException, self.opt.update, alpha='a')
146       
147        #Test setting list instead of dict
148        assert_raises(InvalidAlgorithmOptionException, self.opt.update, P_0=['x1','1.0'])
149       
150        #Test setting non-existent argument
151        assert_raises(UnrecognizedOptionError, self.opt.update, P={'x1':0.3})
152       
153        #Test setting wrong value in covariance dict
154        assert_raises(InvalidAlgorithmOptionException, self.opt.update, P_0={'x1':'a'})
155
156
157@testattr(stddist_full = True)
158class Test_Create_UKF:
159    def setUp(self):
160        self.model = load_fmu(get_fmu())
161        self.x_0 = {'x2':0, 'x1':1}
162        measurements = ['x1']
163        h = 0.01
164        self.options = UKFOptions()
165        self.options.update(P_0={'x2':1, 'x1':2})
166        self.options.update(P_v={'x2':1e-3, 'x1':1e-1})
167        self.options.update(P_n={'x1':1e-4})
168        self.ukf = UKF(self.model, self.x_0, measurements, h, self.options)
169       
170    def tearDown(self):
171        self.model = None
172        self.ukf = None
173        self.x_0 = None
174        self.options = None
175       
176    def test_create_ukf(self):
177       
178        #Test that initialization of scaled state estimate vector is correct
179        names = []
180        for state in self.ukf.x:
181            names = names + [state.get_name()]
182        assert names == ['x1','x2']
183       
184        for state in self.ukf.x:
185            assert state.get_actual_value() == self.x_0[state.get_name()]
186            assert type(state.get_actual_value()) == float 
187            assert state.get_nominal_value() == self.model.get_variable_nominal(state.get_name())
188            assert type(state.get_nominal_value()) == float
189       
190        #Test that initialization of measurement vector is correct
191        names = []
192        for meas in self.ukf.mes:
193            names = names + [meas.get_name()]
194        assert names == ['x1']
195       
196        for meas in self.ukf.mes:
197            assert meas.get_nominal_value() == self.model.get_variable_nominal(meas.get_name())
198            assert type(meas.get_nominal_value()) == float
199       
200        #Test that initialization of covariance matrices is correct
201        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')]]))
202        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')]]))
203        assert N.all(self.ukf.P_n == N.array([1e-4/self.model.get_variable_nominal('x1')]))
204       
205    def test_calc_weights(self):
206        #Test that the weights are calculated correctly
207        [Wm, Wc] = self.ukf._calc_weights(self.options)
208        assert N.all(Wm == N.array([-9.999989999712444e05, 2.499999999928111e05, 2.499999999928111e05,
209            2.499999999928111e05, 2.499999999928111e05]))
210        assert N.all(Wc == N.array([-9.999959999722444e+05, 2.499999999928111e05, 2.499999999928111e05,
211            2.499999999928111e05, 2.499999999928111e05]))
212       
213    def test_update_options(self):
214        #Test that weights and covariances are updated correctly
215        self.ukf.update_options(alpha=1.0, beta=0.0, P_v={'x2':1e-2, 'x1':1e-3})
216        assert N.all(self.ukf.Wm == N.array([0.0, 0.25, 0.25, 0.25, 0.25]))
217        assert N.all(self.ukf.Wc == N.array([0.0, 0.25, 0.25, 0.25, 0.25]))
218        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')]]))
219       
220    def test_get_options(self):
221        assert self.options == self.ukf.get_options()
222
223     
224@testattr(stddist_full = True)
225class Test_AlgorithmMethods_UKF:
226    def setUp(self):
227        self.model = load_fmu(get_fmu())
228        self.x_0 = {'x2':0, 'x1':1}
229        measurements = ['x1']
230        h = 0.01
231        self.options = UKFOptions()
232        self.options.update(P_0={'x2':1, 'x1':2})
233        self.options.update(P_v={'x2':1e-3, 'x1':1e-1})
234        self.options.update(P_n={'x1':1e-4})
235        self.ukf = UKF(self.model, self.x_0, measurements, h, self.options)
236       
237    def tearDown(self):
238        self.model = None
239        self.ukf = None
240        self.x_0 = None
241        self.options = None 
242   
243    def test_calc_sigma(self):
244        #Test that sigma points are calculated correctly
245        sigma = self.ukf._calc_sigma(self.ukf.x, self.ukf.P, self.ukf.P_v, 
246            self.ukf.P_n, self.ukf.options)
247        N.testing.assert_allclose(sigma, N.array([[1.0,   1.002000000000029, 1.0,  0.997999999999971, 1.0],
248            [0.0, 0.0,   0.001414213562393, 0.0, -0.001414213562393]]))
249           
250    def test_update_private(self):
251        #Test that the private update method is performed correctly
252        K = N.array([[0.1], [0.3]])
253        xp = N.array([[1.2],[0.1]])
254        yp = N.array([[1.18]])
255        y = N.array([[1.24]])
256        N.testing.assert_allclose(self.ukf._update(y, xp, yp, K), N.array([[1.206000000000000],
257            [0.118000000000000]]))
258   
259    def test_update_public(self):
260        #Test that the public update method calculates estimates correctly
261        self.ukf.K = N.array([[0.1], [0.3]])
262        self.ukf.xp = N.array([[1.2],[0.1]])
263        self.ukf.yp = N.array([[1.18]])
264        x = self.ukf.update({'x1':1.24})
265        N.testing.assert_allclose(x['x1'], 1.206000000000000)
266        N.testing.assert_allclose(x['x2'], 0.118000000000000)
267   
268    def test_predict_private(self):
269        #Produce sigma points
270        sigma = self.ukf._calc_sigma(self.ukf.x, self.ukf.P, self.ukf.P_v, 
271            self.ukf.P_n, self.ukf.options)
272        #Have a constant input of 0.1 over the sample interval
273        u = (['u'], N.transpose(N.vstack((0.0,0.1))))
274        #No known state values
275        known_values = {}
276        #Do prediction
277        [xp, yp, K, P] = self.ukf._predict(sigma, self.ukf.model, self.ukf.x, u, 
278            known_values, self.ukf.P_v, self.ukf.P_n, self.ukf.currTime, self.ukf.h, 
279            self.ukf.mes, self.ukf.Wm, self.ukf.Wc)
280       
281        #Assert predicted mean and measurement
282        N.testing.assert_allclose(xp, [[1.00988634], [0.0172094]], rtol=1e-6)
283        N.testing.assert_allclose(yp, N.array([[1.00988634]]), rtol=1e-6)
284       
285        #Assert covariance and gain
286        N.testing.assert_allclose(K, [[0.99995099], [0.00497003]], rtol=1e-6)
287        N.testing.assert_allclose(P, [[1.00099995e-01, 4.97003235e-07],
288                              [4.97003235e-07, 1.00115269e+00]], rtol=1e-6)
289   
290    def test_predict_public(self):
291        #Have a constant input of 0.1 over the sample interval
292        u = (['u'], N.transpose(N.vstack((0.0,0.1))))
293        #No known state values
294        known_values = {}
295        self.ukf.predict(u, known_values)
296        #Assert predicted mean and measurement
297        N.testing.assert_allclose(self.ukf.xp, [[1.00988634], [0.0172094]], rtol=1e-6)
298        N.testing.assert_allclose(self.ukf.yp, [[1.00988634]], rtol=1e-6)
299       
300        #Assert covariance and gain
301        N.testing.assert_allclose(self.ukf.K, [[0.99995099], [0.00497003]], rtol=1e-6)
302        N.testing.assert_allclose(self.ukf.P, [[1.00099995e-01, 4.97003235e-07],
303                                       [4.97003235e-07, 1.00115269e+00]], rtol=1e-6)
Note: See TracBrowser for help on using the repository browser.