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

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

#5819 Replaced updating class object dict with setattr because the previous way did not work in Python 3. Also updated test to use numpy asserts to get proper error messages

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]])
283        N.testing.assert_allclose(yp, N.array([[1.00988634]]))
284       
285        #Assert covariance and gain
286        N.testing.assert_allclose(K, [[0.99995099], [0.00497003]])
287        N.testing.assert_allclose(P, [[1.00099995e-01, 4.97003235e-07],
288                              [4.97003235e-07, 1.00115269e+00]])
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]])
298        N.testing.assert_allclose(self.ukf.yp, [[1.00988634]])
299       
300        #Assert covariance and gain
301        N.testing.assert_allclose(self.ukf.K, [[0.99995099], [0.00497003]])
302        N.testing.assert_allclose(self.ukf.P, [[1.00099995e-01, 4.97003235e-07],
303                                       [4.97003235e-07, 1.00115269e+00]])
Note: See TracBrowser for help on using the repository browser.