src/daComposant/daAlgorithms/UnscentedKalmanFilter.py

   1 #-*-coding:iso-8859-1-*-
   2 #
   3 # Copyright (C) 2008-2016 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, math
  26
  27 # ==============================================================================
  28 class ElementaryAlgorithm(BasicObjects.Algorithm):
  29     def __init__(self):
  30         BasicObjects.Algorithm.__init__(self, "UNSCENTEDKALMANFILTER")
  31         self.defineRequiredParameter(
  32             name     = "ConstrainedBy",
  33             default  = "EstimateProjection",
  34             typecast = str,
  35             message  = "Prise en compte des contraintes",
  36             listval  = ["EstimateProjection"],
  37             )
  38         self.defineRequiredParameter(
  39             name     = "EstimationOf",
  40             default  = "State",
  41             typecast = str,
  42             message  = "Estimation d'etat ou de parametres",
  43             listval  = ["State", "Parameters"],
  44             )
  45         self.defineRequiredParameter(
  46             name     = "Alpha",
  47             default  = 1.,
  48             typecast = float,
  49             message  = "",
  50             minval   = 1.e-4,
  51             maxval   = 1.,
  52             )
  53         self.defineRequiredParameter(
  54             name     = "Beta",
  55             default  = 2,
  56             typecast = float,
  57             message  = "",
  58             )
  59         self.defineRequiredParameter(
  60             name     = "Kappa",
  61             default  = 0,
  62             typecast = int,
  63             message  = "",
  64             maxval   = 2,
  65             )
  66         self.defineRequiredParameter(
  67             name     = "Reconditioner",
  68             default  = 1.,
  69             typecast = float,
  70             message  = "",
  71             minval   = 1.e-3,
  72             maxval   = 1.e+1,
  73             )
  74         self.defineRequiredParameter(
  75             name     = "StoreInternalVariables",
  76             default  = False,
  77             typecast = bool,
  78             message  = "Stockage des variables internes ou intermédiaires du calcul",
  79             )
  80         self.defineRequiredParameter(
  81             name     = "StoreSupplementaryCalculations",
  82             default  = [],
  83             typecast = tuple,
  84             message  = "Liste de calculs supplémentaires à stocker et/ou effectuer",
  85             listval  = ["APosterioriCorrelations", "APosterioriCovariance", "APosterioriStandardDeviations", "APosterioriVariances", "BMA", "CurrentState", "CostFunctionJ", "Innovation"]
  86             )
  87         self.defineRequiredParameter( # Pas de type
  88             name     = "Bounds",
  89             message  = "Liste des valeurs de bornes",
  90             )
  91
  92     def run(self, Xb=None, Y=None, U=None, HO=None, EM=None, CM=None, R=None, B=None, Q=None, Parameters=None):
  93         self._pre_run()
  94         #
  95         # Paramètres de pilotage
  96         # ----------------------
  97         self.setParameters(Parameters)
  98         #
  99         if self._parameters.has_key("Bounds") and (type(self._parameters["Bounds"]) is type([]) or type(self._parameters["Bounds"]) is type(())) and (len(self._parameters["Bounds"]) > 0):
 100             Bounds = self._parameters["Bounds"]
 101             logging.debug("%s Prise en compte des bornes effectuee"%(self._name,))
 102         else:
 103             Bounds = None
 104         if self._parameters["EstimationOf"] == "Parameters":
 105             self._parameters["StoreInternalVariables"] = True
 106         #
 107         L     = Xb.size
 108         Alpha = self._parameters["Alpha"]
 109         Beta  = self._parameters["Beta"]
 110         if self._parameters["Kappa"] == 0:
 111             if self._parameters["EstimationOf"] == "State":
 112                 Kappa = 0
 113             elif self._parameters["EstimationOf"] == "Parameters":
 114                 Kappa = 3 - L
 115         else:
 116             Kappa = self._parameters["Kappa"]
 117         Lambda = float( Alpha**2 ) * ( L + Kappa ) - L
 118         Gamma  = math.sqrt( L + Lambda )
 119         #
 120         Ww = []
 121         Ww.append( 0. )
 122         for i in range(2*L):
 123             Ww.append( 1. / (2.*(L + Lambda)) )
 124         #
 125         Wm = numpy.array( Ww )
 126         Wm[0] = Lambda / (L + Lambda)
 127         Wc = numpy.array( Ww )
 128         Wc[0] = Lambda / (L + Lambda) + (1. - Alpha**2 + Beta)
 129         #
 130         # Opérateurs
 131         # ----------
 132         if B is None:
 133             raise ValueError("Background error covariance matrix has to be properly defined!")
 134         if R is None:
 135             raise ValueError("Observation error covariance matrix has to be properly defined!")
 136         #
 137         H = HO["Direct"].appliedControledFormTo
 138         #
 139         if self._parameters["EstimationOf"] == "State":
 140             M = EM["Direct"].appliedControledFormTo
 141         #
 142         if CM is not None and CM.has_key("Tangent") and U is not None:
 143             Cm = CM["Tangent"].asMatrix(Xb)
 144         else:
 145             Cm = None
 146         #
 147         # Nombre de pas identique au nombre de pas d'observations
 148         # -------------------------------------------------------
 149         if hasattr(Y,"stepnumber"):
 150             duration = Y.stepnumber()
 151         else:
 152             duration = 2
 153         #
 154         # Précalcul des inversions de B et R
 155         # ----------------------------------
 156         if self._parameters["StoreInternalVariables"]:
 157             BI = B.getI()
 158             RI = R.getI()
 159         #
 160         # Initialisation
 161         # --------------
 162         Xn = Xb
 163         if hasattr(B,"asfullmatrix"):
 164             Pn = B.asfullmatrix(Xn.size)
 165         else:
 166             Pn = B
 167         #
 168         self.StoredVariables["Analysis"].store( Xn.A1 )
 169         if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
 170             self.StoredVariables["APosterioriCovariance"].store( Pn )
 171             covarianceXa = Pn
 172         Xa               = Xn
 173         previousJMinimum = numpy.finfo(float).max
 174         #
 175         for step in range(duration-1):
 176             if hasattr(Y,"store"):
 177                 Ynpu = numpy.asmatrix(numpy.ravel( Y[step+1] )).T
 178             else:
 179                 Ynpu = numpy.asmatrix(numpy.ravel( Y )).T
 180             #
 181             if U is not None:
 182                 if hasattr(U,"store") and len(U)>1:
 183                     Un = numpy.asmatrix(numpy.ravel( U[step] )).T
 184                 elif hasattr(U,"store") and len(U)==1:
 185                     Un = numpy.asmatrix(numpy.ravel( U[0] )).T
 186                 else:
 187                     Un = numpy.asmatrix(numpy.ravel( U )).T
 188             else:
 189                 Un = None
 190             #
 191             Pndemi = numpy.linalg.cholesky(Pn)
 192             Xnp = numpy.hstack([Xn, Xn+Gamma*Pndemi, Xn-Gamma*Pndemi])
 193             nbSpts = 2*Xn.size+1
 194             #
 195             if Bounds is not None and self._parameters["ConstrainedBy"] == "EstimateProjection":
 196                 for point in range(nbSpts):
 197                     Xnp[:,point] = numpy.max(numpy.hstack((Xnp[:,point],numpy.asmatrix(Bounds)[:,0])),axis=1)
 198                     Xnp[:,point] = numpy.min(numpy.hstack((Xnp[:,point],numpy.asmatrix(Bounds)[:,1])),axis=1)
 199             #
 200             XEtnnp = []
 201             for point in range(nbSpts):
 202                 if self._parameters["EstimationOf"] == "State":
 203                     XEtnnpi = numpy.asmatrix(numpy.ravel( M( (Xnp[:,point], Un) ) )).T
 204                     if Cm is not None and Un is not None: # Attention : si Cm est aussi dans M, doublon !
 205                         Cm = Cm.reshape(Xn.size,Un.size) # ADAO & check shape
 206                         XEtnnpi = XEtnnpi + Cm * Un
 207                     if Bounds is not None and self._parameters["ConstrainedBy"] == "EstimateProjection":
 208                         XEtnnpi = numpy.max(numpy.hstack((XEtnnpi,numpy.asmatrix(Bounds)[:,0])),axis=1)
 209                         XEtnnpi = numpy.min(numpy.hstack((XEtnnpi,numpy.asmatrix(Bounds)[:,1])),axis=1)
 210                 elif self._parameters["EstimationOf"] == "Parameters":
 211                     # --- > Par principe, M = Id, Q = 0
 212                     XEtnnpi = Xnp[:,point]
 213                 XEtnnp.append( XEtnnpi )
 214             XEtnnp = numpy.hstack( XEtnnp )
 215             #
 216             Xncm = numpy.matrix( XEtnnp.getA()*numpy.array(Wm) ).sum(axis=1)
 217             #
 218             if Bounds is not None and self._parameters["ConstrainedBy"] == "EstimateProjection":
 219                 Xncm = numpy.max(numpy.hstack((Xncm,numpy.asmatrix(Bounds)[:,0])),axis=1)
 220                 Xncm = numpy.min(numpy.hstack((Xncm,numpy.asmatrix(Bounds)[:,1])),axis=1)
 221             #
 222             if self._parameters["EstimationOf"] == "State":        Pnm = Q
 223             elif self._parameters["EstimationOf"] == "Parameters": Pnm = 0.
 224             for point in range(nbSpts):
 225                 Pnm += Wc[i] * (XEtnnp[:,point]-Xncm) * (XEtnnp[:,point]-Xncm).T
 226             #
 227             if self._parameters["EstimationOf"] == "Parameters" and Bounds is not None:
 228                 Pnmdemi = self._parameters["Reconditioner"] * numpy.linalg.cholesky(Pnm)
 229             else:
 230                 Pnmdemi = numpy.linalg.cholesky(Pnm)
 231             #
 232             Xnnp = numpy.hstack([Xncm, Xncm+Gamma*Pnmdemi, Xncm-Gamma*Pnmdemi])
 233             #
 234             if Bounds is not None and self._parameters["ConstrainedBy"] == "EstimateProjection":
 235                 for point in range(nbSpts):
 236                     Xnnp[:,point] = numpy.max(numpy.hstack((Xnnp[:,point],numpy.asmatrix(Bounds)[:,0])),axis=1)
 237                     Xnnp[:,point] = numpy.min(numpy.hstack((Xnnp[:,point],numpy.asmatrix(Bounds)[:,1])),axis=1)
 238             #
 239             Ynnp = []
 240             for point in range(nbSpts):
 241                 if self._parameters["EstimationOf"] == "State":
 242                     Ynnpi = numpy.asmatrix(numpy.ravel( H( (Xnnp[:,point], None) ) )).T
 243                 elif self._parameters["EstimationOf"] == "Parameters":
 244                     Ynnpi = numpy.asmatrix(numpy.ravel( H( (Xnnp[:,point], Un) ) )).T
 245                 Ynnp.append( Ynnpi )
 246             Ynnp = numpy.hstack( Ynnp )
 247             #
 248             Yncm = numpy.matrix( Ynnp.getA()*numpy.array(Wm) ).sum(axis=1)
 249             #
 250             Pyyn = R
 251             Pxyn = 0.
 252             for point in range(nbSpts):
 253                 Pyyn += Wc[i] * (Ynnp[:,point]-Yncm) * (Ynnp[:,point]-Yncm).T
 254                 Pxyn += Wc[i] * (Xnnp[:,point]-Xncm) * (Ynnp[:,point]-Yncm).T
 255             #
 256             d  = Ynpu - Yncm
 257             if self._parameters["EstimationOf"] == "Parameters":
 258                 if Cm is not None and Un is not None: # Attention : si Cm est aussi dans H, doublon !
 259                     d = d - Cm * Un
 260             #
 261             Kn = Pxyn * Pyyn.I
 262             Xn = Xncm + Kn * d
 263             Pn = Pnm - Kn * Pyyn * Kn.T
 264             #
 265             if Bounds is not None and self._parameters["ConstrainedBy"] == "EstimateProjection":
 266                 Xn = numpy.max(numpy.hstack((Xn,numpy.asmatrix(Bounds)[:,0])),axis=1)
 267                 Xn = numpy.min(numpy.hstack((Xn,numpy.asmatrix(Bounds)[:,1])),axis=1)
 268             #
 269             self.StoredVariables["Analysis"].store( Xn.A1 )
 270             if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
 271                 self.StoredVariables["APosterioriCovariance"].store( Pn )
 272             if "Innovation" in self._parameters["StoreSupplementaryCalculations"]:
 273                 self.StoredVariables["Innovation"].store( numpy.ravel( d.A1 ) )
 274             if self._parameters["StoreInternalVariables"]:
 275                 Jb  = 0.5 * (Xn - Xb).T * BI * (Xn - Xb)
 276                 Jo  = 0.5 * d.T * RI * d
 277                 J   = float( Jb ) + float( Jo )
 278                 if self._parameters["StoreInternalVariables"] or "CurrentState" in self._parameters["StoreSupplementaryCalculations"]:
 279                     self.StoredVariables["CurrentState"].store( Xn )
 280                 self.StoredVariables["CostFunctionJb"].store( Jb )
 281                 self.StoredVariables["CostFunctionJo"].store( Jo )
 282                 self.StoredVariables["CostFunctionJ" ].store( J )
 283                 if J < previousJMinimum:
 284                     previousJMinimum  = J
 285                     Xa                = Xn
 286                     if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
 287                         covarianceXa  = Pn
 288             else:
 289                 Xa = Xn
 290             #
 291         #
 292         # Stockage supplementaire de l'optimum en estimation de parametres
 293         # ----------------------------------------------------------------
 294         if self._parameters["EstimationOf"] == "Parameters":
 295             self.StoredVariables["Analysis"].store( Xa.A1 )
 296             if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
 297                 self.StoredVariables["APosterioriCovariance"].store( covarianceXa )
 298         #
 299         if "BMA" in self._parameters["StoreSupplementaryCalculations"]:
 300             self.StoredVariables["BMA"].store( numpy.ravel(Xb) - numpy.ravel(Xa) )
 301         #
 302         self._post_run(HO)
 303         return 0
 304
 305 # ==============================================================================
 306 if __name__ == "__main__":
 307     print '\n AUTODIAGNOSTIC \n'