src/daComposant/daAlgorithms/Kalman.py

   1 #-*-coding:iso-8859-1-*-
   2 #
   3 #  Copyright (C) 2008-2009  EDF R&D
   4 #
   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.
   9 #
  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.
  14 #
  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
  18 #
  19 #  See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  20 #
  21 __doc__ = """
  22     Algorithme de Kalman pour un système discret
  23
  24     Remarque : les observations sont exploitées à partir du pas de temps 1, et
  25     sont utilisées dans Yo comme rangées selon ces indices. Donc le pas 0 n'est
  26     pas utilisé puisque la première étape de Kalman passe de 0 à 1 avec
  27     l'observation du pas 1.
  28 """
  29 __author__ = "Jean-Philippe ARGAUD - Septembre 2008"
  30
  31 import sys ; sys.path.insert(0, "../daCore")
  32 import logging
  33 import Persistence
  34 from BasicObjects import Algorithm
  35 import PlatformInfo ; m = PlatformInfo.SystemUsage()
  36
  37 # ==============================================================================
  38 class ElementaryAlgorithm(Algorithm):
  39     def __init__(self):
  40         Algorithm.__init__(self)
  41         self._name = "KALMAN"
  42         logging.debug("%s Initialisation"%self._name)
  43
  44     def run(self, Xb=None, Y=None, H=None, M=None, R=None, B=None, Q=None, Par=None):
  45         """
  46         Calcul de l'estimateur de Kalman
  47         """
  48         logging.debug("%s Lancement"%self._name)
  49         logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("Mo")))
  50         #
  51         # Opérateur d'observation
  52         # -----------------------
  53         Hm = H["Direct"].asMatrix()
  54         Ht = H["Adjoint"].asMatrix()
  55         #
  56         # Opérateur d'évolution
  57         # ---------------------
  58         Mm = M["Direct"].asMatrix()
  59         Mt = M["Adjoint"].asMatrix()
  60         #
  61         duration = Y.stepnumber()
  62         #
  63         # Initialisation
  64         # --------------
  65         Xn = Xb
  66         Pn = B
  67         self.StoredVariables["Analysis"].store( Xn.A1 )
  68         self.StoredVariables["CovarianceAPosteriori"].store( Pn )
  69         #
  70         for step in range(duration-1):
  71             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))
  72             #
  73             # Etape de prédiction
  74             # -------------------
  75             Xn_predicted = Mm * Xn
  76             Pn_predicted = Mm * Pn * Mt + Q
  77             #
  78             # Etape de correction
  79             # -------------------
  80             d  = Y.valueserie(step+1) - Hm * Xn_predicted
  81             K  = Pn_predicted * Ht * (Hm * Pn_predicted * Ht + R).I
  82             Xn = Xn_predicted + K * d
  83             Pn = Pn_predicted - K * Hm * Pn_predicted
  84             #
  85             self.StoredVariables["Analysis"].store( Xn.A1 )
  86             self.StoredVariables["CovarianceAPosteriori"].store( Pn )
  87             self.StoredVariables["Innovation"].store( d.A1 )
  88         #
  89         logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("Mo")))
  90         logging.debug("%s Terminé"%self._name)
  91         #
  92         return 0
  93
  94 # ==============================================================================
  95 if __name__ == "__main__":
  96     print '\n AUTODIAGNOSTIC \n'