src/daComposant/daAlgorithms/KalmanFilter.py

   1 # -*- coding: utf-8 -*-
   2 #
   3 # Copyright (C) 2008-2019 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
  25 import numpy
  26
  27 # ==============================================================================
  28 class ElementaryAlgorithm(BasicObjects.Algorithm):
  29     def __init__(self):
  30         BasicObjects.Algorithm.__init__(self, "KALMANFILTER")
  31         self.defineRequiredParameter(
  32             name     = "EstimationOf",
  33             default  = "State",
  34             typecast = str,
  35             message  = "Estimation d'etat ou de parametres",
  36             listval  = ["State", "Parameters"],
  37             )
  38         self.defineRequiredParameter(
  39             name     = "StoreInternalVariables",
  40             default  = False,
  41             typecast = bool,
  42             message  = "Stockage des variables internes ou intermédiaires du calcul",
  43             )
  44         self.defineRequiredParameter(
  45             name     = "StoreSupplementaryCalculations",
  46             default  = [],
  47             typecast = tuple,
  48             message  = "Liste de calculs supplémentaires à stocker et/ou effectuer",
  49             listval  = [
  50                 "Analysis",
  51                 "APosterioriCorrelations",
  52                 "APosterioriCovariance",
  53                 "APosterioriStandardDeviations",
  54                 "APosterioriVariances",
  55                 "BMA",
  56                 "CostFunctionJ",
  57                 "CostFunctionJAtCurrentOptimum",
  58                 "CostFunctionJb",
  59                 "CostFunctionJbAtCurrentOptimum",
  60                 "CostFunctionJo",
  61                 "CostFunctionJoAtCurrentOptimum",
  62                 "CurrentOptimum",
  63                 "CurrentState",
  64                 "IndexOfOptimum",
  65                 "InnovationAtCurrentAnalysis",
  66                 "InnovationAtCurrentState",
  67                 "PredictedState",
  68                 "SimulatedObservationAtCurrentAnalysis",
  69                 "SimulatedObservationAtCurrentOptimum",
  70                 "SimulatedObservationAtCurrentState",
  71                 ]
  72             )
  73         self.requireInputArguments(
  74             mandatory= ("Xb", "Y", "HO", "R", "B"),
  75             optional = ("U", "EM", "CM", "Q"),
  76             )
  77
  78     def run(self, Xb=None, Y=None, U=None, HO=None, EM=None, CM=None, R=None, B=None, Q=None, Parameters=None):
  79         self._pre_run(Parameters, Xb, Y, R, B, Q)
  80         #
  81         if self._parameters["EstimationOf"] == "Parameters":
  82             self._parameters["StoreInternalVariables"] = True
  83         #
  84         # Opérateurs
  85         # ----------
  86         Ht = HO["Tangent"].asMatrix(Xb)
  87         Ha = HO["Adjoint"].asMatrix(Xb)
  88         #
  89         if self._parameters["EstimationOf"] == "State":
  90             Mt = EM["Tangent"].asMatrix(Xb)
  91             Ma = EM["Adjoint"].asMatrix(Xb)
  92         #
  93         if CM is not None and "Tangent" in CM and U is not None:
  94             Cm = CM["Tangent"].asMatrix(Xb)
  95         else:
  96             Cm = None
  97         #
  98         # Nombre de pas identique au nombre de pas d'observations
  99         # -------------------------------------------------------
 100         if hasattr(Y,"stepnumber"):
 101             duration = Y.stepnumber()
 102         else:
 103             duration = 2
 104         #
 105         # Précalcul des inversions de B et R
 106         # ----------------------------------
 107         if self._parameters["StoreInternalVariables"] \
 108             or self._toStore("CostFunctionJ") \
 109             or self._toStore("CostFunctionJb") \
 110             or self._toStore("CostFunctionJo") \
 111             or self._toStore("CurrentOptimum") \
 112             or self._toStore("APosterioriCovariance"):
 113             BI = B.getI()
 114             RI = R.getI()
 115         #
 116         # Initialisation
 117         # --------------
 118         Xn = Xb
 119         Pn = B
 120         #
 121         if len(self.StoredVariables["Analysis"])==0 or not self._parameters["nextStep"]:
 122             self.StoredVariables["Analysis"].store( numpy.ravel(Xn) )
 123             if self._toStore("APosterioriCovariance"):
 124                 self.StoredVariables["APosterioriCovariance"].store( Pn.asfullmatrix(Xn.size) )
 125                 covarianceXa = Pn
 126         #
 127         Xa               = Xn
 128         previousJMinimum = numpy.finfo(float).max
 129         #
 130         for step in range(duration-1):
 131             if hasattr(Y,"store"):
 132                 Ynpu = numpy.asmatrix(numpy.ravel( Y[step+1] )).T
 133             else:
 134                 Ynpu = numpy.asmatrix(numpy.ravel( Y )).T
 135             #
 136             if U is not None:
 137                 if hasattr(U,"store") and len(U)>1:
 138                     Un = numpy.asmatrix(numpy.ravel( U[step] )).T
 139                 elif hasattr(U,"store") and len(U)==1:
 140                     Un = numpy.asmatrix(numpy.ravel( U[0] )).T
 141                 else:
 142                     Un = numpy.asmatrix(numpy.ravel( U )).T
 143             else:
 144                 Un = None
 145             #
 146             if self._parameters["EstimationOf"] == "State":
 147                 Xn_predicted = Mt * Xn
 148                 if Cm is not None and Un is not None: # Attention : si Cm est aussi dans M, doublon !
 149                     Cm = Cm.reshape(Xn.size,Un.size) # ADAO & check shape
 150                     Xn_predicted = Xn_predicted + Cm * Un
 151                 Pn_predicted = Q + Mt * Pn * Ma
 152             elif self._parameters["EstimationOf"] == "Parameters":
 153                 # --- > Par principe, M = Id, Q = 0
 154                 Xn_predicted = Xn
 155                 Pn_predicted = Pn
 156             #
 157             if self._parameters["EstimationOf"] == "State":
 158                 _HX          = Ht * Xn_predicted
 159                 _Innovation  = Ynpu - _HX
 160             elif self._parameters["EstimationOf"] == "Parameters":
 161                 _HX          = Ht * Xn_predicted
 162                 _Innovation  = Ynpu - _HX
 163                 if Cm is not None and Un is not None: # Attention : si Cm est aussi dans H, doublon !
 164                     _Innovation = _Innovation - Cm * Un
 165             #
 166             _A = R + numpy.dot(Ht, Pn_predicted * Ha)
 167             _u = numpy.linalg.solve( _A , _Innovation )
 168             Xn = Xn_predicted + Pn_predicted * Ha * _u
 169             Kn = Pn_predicted * Ha * (R + numpy.dot(Ht, Pn_predicted * Ha)).I
 170             Pn = Pn_predicted - Kn * Ht * Pn_predicted
 171             Xa, _HXa = Xn, _HX # Pointeurs
 172             #
 173             # ---> avec analysis
 174             self.StoredVariables["Analysis"].store( Xa )
 175             if self._toStore("SimulatedObservationAtCurrentAnalysis"):
 176                 self.StoredVariables["SimulatedObservationAtCurrentAnalysis"].store( _HXa )
 177             if self._toStore("InnovationAtCurrentAnalysis"):
 178                 self.StoredVariables["InnovationAtCurrentAnalysis"].store( _Innovation )
 179             # ---> avec current state
 180             if self._parameters["StoreInternalVariables"] \
 181                 or self._toStore("CurrentState"):
 182                 self.StoredVariables["CurrentState"].store( Xn )
 183             if self._toStore("PredictedState"):
 184                 self.StoredVariables["PredictedState"].store( Xn_predicted )
 185             if self._toStore("BMA"):
 186                 self.StoredVariables["BMA"].store( Xn_predicted - Xa )
 187             if self._toStore("InnovationAtCurrentState"):
 188                 self.StoredVariables["InnovationAtCurrentState"].store( _Innovation )
 189             if self._toStore("SimulatedObservationAtCurrentState") \
 190                 or self._toStore("SimulatedObservationAtCurrentOptimum"):
 191                 self.StoredVariables["SimulatedObservationAtCurrentState"].store( _HX )
 192             # ---> autres
 193             if self._parameters["StoreInternalVariables"] \
 194                 or self._toStore("CostFunctionJ") \
 195                 or self._toStore("CostFunctionJb") \
 196                 or self._toStore("CostFunctionJo") \
 197                 or self._toStore("CurrentOptimum") \
 198                 or self._toStore("APosterioriCovariance"):
 199                 Jb  = float( 0.5 * (Xa - Xb).T * BI * (Xa - Xb) )
 200                 Jo  = float( 0.5 * _Innovation.T * RI * _Innovation )
 201                 J   = Jb + Jo
 202                 self.StoredVariables["CostFunctionJb"].store( Jb )
 203                 self.StoredVariables["CostFunctionJo"].store( Jo )
 204                 self.StoredVariables["CostFunctionJ" ].store( J )
 205                 #
 206                 if self._toStore("IndexOfOptimum") \
 207                     or self._toStore("CurrentOptimum") \
 208                     or self._toStore("CostFunctionJAtCurrentOptimum") \
 209                     or self._toStore("CostFunctionJbAtCurrentOptimum") \
 210                     or self._toStore("CostFunctionJoAtCurrentOptimum") \
 211                     or self._toStore("SimulatedObservationAtCurrentOptimum"):
 212                     IndexMin = numpy.argmin( self.StoredVariables["CostFunctionJ"][nbPreviousSteps:] ) + nbPreviousSteps
 213                 if self._toStore("IndexOfOptimum"):
 214                     self.StoredVariables["IndexOfOptimum"].store( IndexMin )
 215                 if self._toStore("CurrentOptimum"):
 216                     self.StoredVariables["CurrentOptimum"].store( self.StoredVariables["Analysis"][IndexMin] )
 217                 if self._toStore("SimulatedObservationAtCurrentOptimum"):
 218                     self.StoredVariables["SimulatedObservationAtCurrentOptimum"].store( self.StoredVariables["SimulatedObservationAtCurrentAnalysis"][IndexMin] )
 219                 if self._toStore("CostFunctionJbAtCurrentOptimum"):
 220                     self.StoredVariables["CostFunctionJbAtCurrentOptimum"].store( self.StoredVariables["CostFunctionJb"][IndexMin] )
 221                 if self._toStore("CostFunctionJoAtCurrentOptimum"):
 222                     self.StoredVariables["CostFunctionJoAtCurrentOptimum"].store( self.StoredVariables["CostFunctionJo"][IndexMin] )
 223                 if self._toStore("CostFunctionJAtCurrentOptimum"):
 224                     self.StoredVariables["CostFunctionJAtCurrentOptimum" ].store( self.StoredVariables["CostFunctionJ" ][IndexMin] )
 225             if self._toStore("APosterioriCovariance"):
 226                 self.StoredVariables["APosterioriCovariance"].store( Pn )
 227             if self._parameters["EstimationOf"] == "Parameters" \
 228                 and J < previousJMinimum:
 229                 previousJMinimum    = J
 230                 XaMin               = Xa
 231                 if self._toStore("APosterioriCovariance"):
 232                     covarianceXaMin = Pn
 233         #
 234         # Stockage final supplémentaire de l'optimum en estimation de paramètres
 235         # ----------------------------------------------------------------------
 236         if self._parameters["EstimationOf"] == "Parameters":
 237             self.StoredVariables["Analysis"].store( XaMin )
 238             if self._toStore("APosterioriCovariance"):
 239                 self.StoredVariables["APosterioriCovariance"].store( covarianceXaMin )
 240             if self._toStore("BMA"):
 241                 self.StoredVariables["BMA"].store( numpy.ravel(Xb) - numpy.ravel(XaMin) )
 242         #
 243         self._post_run(HO)
 244         return 0
 245
 246 # ==============================================================================
 247 if __name__ == "__main__":
 248     print('\n AUTODIAGNOSTIC\n')