From: Jean-Philippe ARGAUD Date: Thu, 7 Feb 2013 20:15:37 +0000 (+0100) Subject: Improving KF and EKF performances for parameter estimation X-Git-Tag: V7_1_0~9 X-Git-Url: http://git.salome-platform.org/gitweb/?a=commitdiff_plain;h=9d5a4fb55fefdb2ba67db26e2ba49b2b9080d85b;p=modules%2Fadao.git Improving KF and EKF performances for parameter estimation --- diff --git a/src/daComposant/daAlgorithms/Blue.py b/src/daComposant/daAlgorithms/Blue.py index 46130cf..99619a2 100644 --- a/src/daComposant/daAlgorithms/Blue.py +++ b/src/daComposant/daAlgorithms/Blue.py @@ -48,8 +48,8 @@ class ElementaryAlgorithm(BasicObjects.Algorithm): # # Opérateur d'observation # ----------------------- - Hm = HO["Tangent"].asMatrix(None) - Ha = HO["Adjoint"].asMatrix(None) + Hm = HO["Tangent"].asMatrix(Xb) + Ha = HO["Adjoint"].asMatrix(Xb) # # Utilisation éventuelle d'un vecteur H(Xb) précalculé # ---------------------------------------------------- diff --git a/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py b/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py index f025924..053a9fb 100644 --- a/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py +++ b/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py @@ -61,7 +61,8 @@ class ElementaryAlgorithm(BasicObjects.Algorithm): # H = HO["Direct"].appliedControledFormTo # - M = EM["Direct"].appliedControledFormTo + if self._parameters["EstimationType"] == "State": + M = EM["Direct"].appliedControledFormTo # # Nombre de pas du Kalman identique au nombre de pas d'observations # ----------------------------------------------------------------- @@ -102,10 +103,11 @@ class ElementaryAlgorithm(BasicObjects.Algorithm): Ha = HO["Adjoint"].asMatrix(ValueForMethodForm = Xn) Ha = Ha.reshape(Xn.size,Ynpu.size) # ADAO & check shape # - Mt = EM["Tangent"].asMatrix(ValueForMethodForm = Xn) - Mt = Mt.reshape(Xn.size,Xn.size) # ADAO & check shape - Ma = EM["Adjoint"].asMatrix(ValueForMethodForm = Xn) - Ma = Ma.reshape(Xn.size,Xn.size) # ADAO & check shape + if self._parameters["EstimationType"] == "State": + Mt = EM["Tangent"].asMatrix(ValueForMethodForm = Xn) + Mt = Mt.reshape(Xn.size,Xn.size) # ADAO & check shape + Ma = EM["Adjoint"].asMatrix(ValueForMethodForm = Xn) + Ma = Ma.reshape(Xn.size,Xn.size) # ADAO & check shape # if U is not None: if hasattr(U,"store") and len(U)>1: @@ -119,14 +121,19 @@ class ElementaryAlgorithm(BasicObjects.Algorithm): # if self._parameters["EstimationType"] == "State": Xn_predicted = numpy.asmatrix(numpy.ravel( M( (Xn, Un) ) )).T - else: - Xn_predicted = numpy.asmatrix(numpy.ravel( M( (Xn, None) ) )).T - Pn_predicted = Mt * Pn * Ma + Q + Pn_predicted = Mt * Pn * Ma + Q + elif self._parameters["EstimationType"] == "Parameters": + # Xn_predicted = numpy.asmatrix(numpy.ravel( M( (Xn, None) ) )).T + # Pn_predicted = Mt * Pn * Ma + Q + # --- > Par principe, M = Id, Q = 0 + Xn_predicted = Xn + Pn_predicted = Pn # if self._parameters["EstimationType"] == "Parameters": d = Ynpu - numpy.asmatrix(numpy.ravel( H( (Xn_predicted, Un) ) )).T - else: + elif self._parameters["EstimationType"] == "State": d = Ynpu - numpy.asmatrix(numpy.ravel( H( (Xn_predicted, None) ) )).T + # K = Pn_predicted * Ha * (Ht * Pn_predicted * Ha + R).I Xn = Xn_predicted + K * d Pn = Pn_predicted - K * Ht * Pn_predicted diff --git a/src/daComposant/daAlgorithms/KalmanFilter.py b/src/daComposant/daAlgorithms/KalmanFilter.py index cd96ea1..118fd9e 100644 --- a/src/daComposant/daAlgorithms/KalmanFilter.py +++ b/src/daComposant/daAlgorithms/KalmanFilter.py @@ -59,14 +59,15 @@ class ElementaryAlgorithm(BasicObjects.Algorithm): if R is None: raise ValueError("Observation error covariance matrix has to be properly defined!") # - Ht = HO["Tangent"].asMatrix(None) - Ha = HO["Adjoint"].asMatrix(None) + Ht = HO["Tangent"].asMatrix(Xb) + Ha = HO["Adjoint"].asMatrix(Xb) # - Mt = EM["Tangent"].asMatrix(None) - Ma = EM["Adjoint"].asMatrix(None) + if self._parameters["EstimationType"] == "State": + Mt = EM["Tangent"].asMatrix(Xb) + Ma = EM["Adjoint"].asMatrix(Xb) # if CM is not None and CM.has_key("Tangent") and U is not None: - Cm = CM["Tangent"].asMatrix(None) + Cm = CM["Tangent"].asMatrix(Xb) else: Cm = None # @@ -113,14 +114,22 @@ class ElementaryAlgorithm(BasicObjects.Algorithm): # if self._parameters["EstimationType"] == "State" and Cm is not None and Un is not None: Xn_predicted = Mt * Xn + Cm * Un - else: + Pn_predicted = Mt * Pn * Ma + Q + elif self._parameters["EstimationType"] == "State" and (Cm is None or Un is None): Xn_predicted = Mt * Xn - Pn_predicted = Mt * Pn * Ma + Q + Pn_predicted = Mt * Pn * Ma + Q + elif self._parameters["EstimationType"] == "Parameters": + # Xn_predicted = Mt * Xn + # Pn_predicted = Mt * Pn * Ma + Q + # --- > Par principe, M = Id, Q = 0 + Xn_predicted = Xn + Pn_predicted = Pn # if self._parameters["EstimationType"] == "Parameters" and Cm is not None and Un is not None: d = Ynpu - Ht * Xn_predicted - Cm * Un else: d = Ynpu - Ht * Xn_predicted + # K = Pn_predicted * Ha * (Ht * Pn_predicted * Ha + R).I Xn = Xn_predicted + K * d Pn = Pn_predicted - K * Ht * Pn_predicted