src/daComposant/daAlgorithms/KalmanFilter.py

   1 #-*-coding:iso-8859-1-*-
   2 #
   3 #  Copyright (C) 2008-2012 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 #  Author: Jean-Philippe Argaud, jean-philippe.argaud@edf.fr, EDF R&D
  22
  23 import logging
  24 from daCore import BasicObjects, PlatformInfo
  25 m = PlatformInfo.SystemUsage()
  26
  27 # ==============================================================================
  28 class ElementaryAlgorithm(BasicObjects.Algorithm):
  29     def __init__(self):
  30         BasicObjects.Algorithm.__init__(self, "KALMANFILTER")
  31         self.defineRequiredParameter(
  32             name     = "StoreSupplementaryCalculations",
  33             default  = [],
  34             typecast = tuple,
  35             message  = "Liste de calculs supplémentaires à stocker et/ou effectuer",
  36             listval  = ["APosterioriCovariance", "Innovation"]
  37             )
  38
  39     def run(self, Xb=None, Y=None, H=None, M=None, R=None, B=None, Q=None, Parameters=None):
  40         logging.debug("%s Lancement"%self._name)
  41         logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("M")))
  42         #
  43         # Paramètres de pilotage
  44         # ----------------------
  45         self.setParameters(Parameters)
  46         #
  47         # Opérateur d'observation
  48         # -----------------------
  49         Hm = H["Tangent"].asMatrix(None)
  50         Ha = H["Adjoint"].asMatrix(None)
  51         #
  52         if B is None:
  53             raise ValueError("Background error covariance matrix has to be properly defined!")
  54         if R is None:
  55             raise ValueError("Observation error covariance matrix has to be properly defined!")
  56         #
  57         # Opérateur d'évolution
  58         # ---------------------
  59         Mm = M["Tangent"].asMatrix(None)
  60         Mt = M["Adjoint"].asMatrix(None)
  61         #
  62         # Nombre de pas du Kalman identique au nombre de pas d'observations
  63         # -----------------------------------------------------------------
  64         duration = Y.stepnumber()
  65         #
  66         # Initialisation
  67         # --------------
  68         Xn = Xb
  69         Pn = B
  70         self.StoredVariables["Analysis"].store( Xn.A1 )
  71         if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
  72             self.StoredVariables["APosterioriCovariance"].store( Pn )
  73         #
  74         for step in range(duration-1):
  75             Xn_predicted = Mm * Xn
  76             Pn_predicted = Mm * Pn * Mt + Q
  77             #
  78             d  = Y.valueserie(step+1) - Hm * Xn_predicted
  79             K  = Pn_predicted * Ha * (Hm * Pn_predicted * Ha + R).I
  80             Xn = Xn_predicted + K * d
  81             Pn = Pn_predicted - K * Hm * Pn_predicted
  82             #
  83             self.StoredVariables["Analysis"].store( Xn.A1 )
  84             if "Innovation" in self._parameters["StoreSupplementaryCalculations"]:
  85                 self.StoredVariables["Innovation"].store( numpy.ravel( d.A1 ) )
  86             if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
  87                 self.StoredVariables["APosterioriCovariance"].store( Pn )
  88         #
  89         logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("M")))
  90         logging.debug("%s Terminé"%self._name)
  91         #
  92         return 0
  93
  94 # ==============================================================================
  95 if __name__ == "__main__":
  96     print '\n AUTODIAGNOSTIC \n'