Adding user variable and minor corrections

[modules/adao.git] / src / daComposant / daAlgorithms / ExtendedKalmanFilter.py

diff --git a/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py b/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py
index f9e2c81d74ffe69dcabf3b573375184a2e20d4a8..1d66ef745ebfd7262a58c5b750ac1bc148a79d50 100644 (file)
--- a/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py
+++ b/src/daComposant/daAlgorithms/ExtendedKalmanFilter.py
@@ -112,7 +112,7 @@ class ElementaryAlgorithm(BasicObjects.Algorithm):
         #
         self.StoredVariables["Analysis"].store( Xn.A1 )
         if "APosterioriCovariance" in self._parameters["StoreSupplementaryCalculations"]:
-            self.StoredVariables["APosterioriCovariance"].store( Pn )
+            self.StoredVariables["APosterioriCovariance"].store( Pn.asfullmatrix(Xn.size) )
             covarianceXa = Pn
         Xa               = Xn
         previousJMinimum = numpy.finfo(float).max
@@ -166,8 +166,10 @@ class ElementaryAlgorithm(BasicObjects.Algorithm):
                 if Cm is not None and Un is not None: # Attention : si Cm est aussi dans H, doublon !
                     d = d - Cm * Un
             #
+            _A = R + Ht * Pn_predicted * Ha
+            _u = numpy.linalg.solve( _A , d )
+            Xn = Xn_predicted + Pn_predicted * Ha * _u
             Kn = Pn_predicted * Ha * (R + Ht * Pn_predicted * Ha).I
-            Xn = Xn_predicted + Kn * d
             Pn = Pn_predicted - Kn * Ht * Pn_predicted
             #
             self.StoredVariables["Analysis"].store( Xn.A1 )