src/daComposant/daAlgorithms/ExtendedKalmanFilter.py

   1 #-*-coding:iso-8859-1-*-
   2 #
   3 #  Copyright (C) 2008-2013 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 import numpy
  27
  28 # ==============================================================================
  29 class ElementaryAlgorithm(BasicObjects.Algorithm):
  30     def __init__(self):
  31         BasicObjects.Algorithm.__init__(self, "EXTENDEDKALMANFILTER")
  32         self.defineRequiredParameter(
  33             name     = "StoreSupplementaryCalculations",
  34             default  = [],
  35             typecast = tuple,
  36             message  = "Liste de calculs supplémentaires à stocker et/ou effectuer",
  37             listval  = ["APosterioriCovariance", "Innovation"]
  38             )
  39
  40     def run(self, Xb=None, Y=None, U=None, HO=None, EM=None, CM=None, R=None, B=None, Q=None, Parameters=None):
  41         logging.debug("%s Lancement"%self._name)
  42         logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("M")))
  43         #
  44         # Paramètres de pilotage
  45         # ----------------------
  46         self.setParameters(Parameters)
  47         #
  48         # Opérateurs
  49         # ----------
  50         if B is None:
  51             raise ValueError("Background error covariance matrix has to be properly defined!")
  52         if R is None:
  53             raise ValueError("Observation error covariance matrix has to be properly defined!")
  54         #
  55         H = HO["Direct"].appliedTo
  56         #
  57         M = EM["Direct"].appliedTo
  58         #
  59         # Nombre de pas du Kalman identique au nombre de pas d'observations
  60         # -----------------------------------------------------------------
  61         duration = Y.stepnumber()
  62         #
  63         # Initialisation
  64         # --------------
  65         Xn = Xb
  66         Pn = B
  67         self.StoredVariables["Analysis"].store( Xn.A1 )
  68         if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
  69             self.StoredVariables["APosterioriCovariance"].store( Pn )
  70         #
  71         for step in range(duration-1):
  72             Ynpu = numpy.asmatrix(numpy.ravel( Y[step+1] )).T
  73             #
  74             Ht = HO["Tangent"].asMatrix(ValueForMethodForm = Xn)
  75             Ht = Ht.reshape(Ynpu.size,Xn.size) # ADAO & check shape
  76             Ha = HO["Adjoint"].asMatrix(ValueForMethodForm = Xn)
  77             Ha = Ha.reshape(Xn.size,Ynpu.size) # ADAO & check shape
  78             #
  79             Mt = EM["Tangent"].asMatrix(ValueForMethodForm = Xn)
  80             Mt = Mt.reshape(Xn.size,Xn.size) # ADAO & check shape
  81             Ma = EM["Adjoint"].asMatrix(ValueForMethodForm = Xn)
  82             Ma = Ma.reshape(Xn.size,Xn.size) # ADAO & check shape
  83             #
  84             if U is not None:
  85                 if hasattr(U,"store") and len(U)>1:
  86                     Un = numpy.asmatrix(numpy.ravel( U[step] )).T
  87                 elif hasattr(U,"store") and len(U)==1:
  88                     Un = numpy.asmatrix(numpy.ravel( U[0] )).T
  89                 else:
  90                     Un = numpy.asmatrix(numpy.ravel( U )).T
  91             else:
  92                 Un = None
  93             #
  94             Xn_predicted = M( (Xn, Un) )
  95             Pn_predicted = Mt * Pn * Ma + Q
  96             #
  97             d  = Ynpu - H( Xn_predicted )
  98             K  = Pn_predicted * Ha * (Ht * Pn_predicted * Ha + R).I
  99             Xn = Xn_predicted + K * d
 100             Pn = Pn_predicted - K * Ht * Pn_predicted
 101             #
 102             self.StoredVariables["Analysis"].store( Xn.A1 )
 103             if "Innovation" in self._parameters["StoreSupplementaryCalculations"]:
 104                 self.StoredVariables["Innovation"].store( numpy.ravel( d.A1 ) )
 105             if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
 106                 self.StoredVariables["APosterioriCovariance"].store( Pn )
 107         #
 108         logging.debug("%s Taille mémoire utilisée de %.1f Mo"%(self._name, m.getUsedMemory("M")))
 109         logging.debug("%s Terminé"%self._name)
 110         #
 111         return 0
 112
 113 # ==============================================================================
 114 if __name__ == "__main__":
 115     print '\n AUTODIAGNOSTIC \n'