1 #-*-coding:iso-8859-1-*-
3 # Copyright (C) 2008-2011 EDF R&D
5 # This library is free software; you can redistribute it and/or
6 # modify it under the terms of the GNU Lesser General Public
7 # License as published by the Free Software Foundation; either
8 # version 2.1 of the License.
10 # This library 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 GNU
13 # Lesser General Public License for more details.
15 # You should have received a copy of the GNU Lesser General Public
16 # License along with this library; if not, write to the Free Software
17 # Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 # See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
23 from daCore import BasicObjects, PlatformInfo
24 m = PlatformInfo.SystemUsage()
26 # ==============================================================================
27 class ElementaryAlgorithm(BasicObjects.Algorithm):
29 BasicObjects.Algorithm.__init__(self)
30 self._name = "KALMANFILTER"
31 logging.debug("%s Initialisation"%self._name)
33 def run(self, Xb=None, Y=None, H=None, M=None, R=None, B=None, Q=None, Parameters=None):
35 Calcul de l'estimateur du filtre de Kalman
37 Remarque : les observations sont exploitées à partir du pas de temps 1,
38 et sont utilisées dans Yo comme rangées selon ces indices. Donc le pas 0
39 n'est pas utilisé puisque la première étape de Kalman passe de 0 à 1
40 avec l'observation du pas 1.
42 logging.debug("%s Lancement"%self._name)
43 logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("Mo")))
45 # Opérateur d'observation
46 # -----------------------
47 Hm = H["Direct"].asMatrix()
48 Ht = H["Adjoint"].asMatrix()
50 # Opérateur d'évolution
51 # ---------------------
52 Mm = M["Direct"].asMatrix()
53 Mt = M["Adjoint"].asMatrix()
55 duration = Y.stepnumber()
61 self.StoredVariables["Analysis"].store( Xn.A1 )
62 self.StoredVariables["CovarianceAPosteriori"].store( Pn )
64 for step in range(duration-1):
65 logging.debug("%s Etape de Kalman %i (i.e. %i->%i) sur un total de %i"%(self._name, step+1, step,step+1, duration-1))
69 Xn_predicted = Mm * Xn
70 Pn_predicted = Mm * Pn * Mt + Q
74 d = Y.valueserie(step+1) - Hm * Xn_predicted
75 K = Pn_predicted * Ht * (Hm * Pn_predicted * Ht + R).I
76 Xn = Xn_predicted + K * d
77 Pn = Pn_predicted - K * Hm * Pn_predicted
79 self.StoredVariables["Analysis"].store( Xn.A1 )
80 self.StoredVariables["CovarianceAPosteriori"].store( Pn )
81 self.StoredVariables["Innovation"].store( d.A1 )
83 logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("Mo")))
84 logging.debug("%s Terminé"%self._name)
88 # ==============================================================================
89 if __name__ == "__main__":
90 print '\n AUTODIAGNOSTIC \n'