From: Jean-Philippe ARGAUD <jean-philippe.argaud@edf.fr>
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