From d312aa15c97ea7a0c9785e487d500e10930f5202 Mon Sep 17 00:00:00 2001 From: Jean-Philippe ARGAUD Date: Fri, 26 Apr 2024 18:55:40 +0200 Subject: [PATCH] Minor documentation and code review corrections (44) --- .../ref_algorithm_UnscentedKalmanFilter.rst | 7 +++--- .../ref_algorithm_UnscentedKalmanFilter.rst | 8 +++---- src/daComposant/daCore/NumericObjects.py | 24 ++++++++++--------- 3 files changed, 21 insertions(+), 18 deletions(-) diff --git a/doc/en/ref_algorithm_UnscentedKalmanFilter.rst b/doc/en/ref_algorithm_UnscentedKalmanFilter.rst index bdbf155..650ca8a 100644 --- a/doc/en/ref_algorithm_UnscentedKalmanFilter.rst +++ b/doc/en/ref_algorithm_UnscentedKalmanFilter.rst @@ -79,11 +79,12 @@ robust formulations are proposed here: The following are a few practical suggestions for the effective use of these algorithms: -- The recommended variant of this algorithm is the "S3F" even if the canonical +- The recommended variant of this algorithm is the "S3F", even if the canonical "UKF" algorithm remains by default the more robust one. - When there are no defined bounds, the constraint-aware versions of the - algorithms are identical to the unconstrained versions. This is not the case - if constraints are defined, even if the bounds are very wide. + algorithms ("CUKF" et "CS3F") are identical to the unconstrained versions + ("UKF" et "S3F"). This is not the case if constraints are defined, even if + the bounds are very wide. - An essential difference between the algorithms is the number of sampling "sigma" points used, depending on the :math:`n` dimension of the state space. The canonical "UKF" algorithm uses :math:`2n+1`, the "S3F" algorithm uses diff --git a/doc/fr/ref_algorithm_UnscentedKalmanFilter.rst b/doc/fr/ref_algorithm_UnscentedKalmanFilter.rst index 95954f9..63f6f25 100644 --- a/doc/fr/ref_algorithm_UnscentedKalmanFilter.rst +++ b/doc/fr/ref_algorithm_UnscentedKalmanFilter.rst @@ -80,12 +80,12 @@ stables et robustes suivantes : Voici quelques suggestions pratiques pour une utilisation efficace de ces algorithmes : -- La variante recommandée de cet algorithme est le "S3F" même si l'algorithme +- La variante recommandée de cet algorithme est le "S3F", même si l'algorithme canonique "UKF" reste par défaut le plus robuste. - Lorsqu'il n'y a aucune borne de définie, les versions avec prise en compte - des contraintes des algorithmes sont identiques aux versions sans - contraintes. Ce n'est pas le cas s'il a des contraintes définies mêmes si les - bornes sont très larges. + des contraintes des algorithmes ("CUKF" et "CS3F") sont identiques aux + versions sans contraintes ("UKF" et "S3F"). Ce n'est pas le cas s'il a des + contraintes définies, mêmes si les bornes choisies sont très larges. - Une différence essentielle entre les algorithmes est le nombre de "sigma" points d'échantillonnage utilisés en fonction de la dimension :math:`n` de l'espace des états. L'algorithme canonique "UKF" en utilise :math:`2n+1`, diff --git a/src/daComposant/daCore/NumericObjects.py b/src/daComposant/daCore/NumericObjects.py index 87d95da..cafe15d 100644 --- a/src/daComposant/daCore/NumericObjects.py +++ b/src/daComposant/daCore/NumericObjects.py @@ -1130,7 +1130,7 @@ class GenerateWeightsAndSigmaPoints(object): self.Alpha = numpy.longdouble( Alpha ) self.Beta = numpy.longdouble( Beta ) if abs(Kappa) < 2 * mpr: - if EO == "Parameters": + if EO == "Parameters" and VariantM == "UKF": self.Kappa = 3 - self.Nn else: # EO == "State": self.Kappa = 0 @@ -1156,7 +1156,7 @@ class GenerateWeightsAndSigmaPoints(object): def __UKF2000(self): "Standard Set, Julier et al. 2000 (aka Canonical UKF)" # Rq.: W^{(m)}_{i=/=0} = 1. / (2.*(n + Lambda)) - Winn = 1. / (2. * self.Alpha**2 * ( self.Nn + self.Kappa )) + Winn = 1. / (2. * ( self.Nn + self.Kappa ) * self.Alpha**2) Ww = [] Ww.append( 0. ) for point in range(2 * self.Nn): @@ -1167,6 +1167,7 @@ class GenerateWeightsAndSigmaPoints(object): Wm[0] = LsLpL Wc = numpy.array( Ww ) Wc[0] = LsLpL + (1. - self.Alpha**2 + self.Beta) + # OK: assert abs(Wm.sum()-1.) < self.Nn*mpr, "UKF ill-conditioned %s >= %s"%(abs(Wm.sum()-1.), self.Nn*mpr) # SC = numpy.zeros((self.Nn, len(Ww))) for ligne in range(self.Nn): @@ -1179,7 +1180,7 @@ class GenerateWeightsAndSigmaPoints(object): def __S3F2022(self): "Scaled Spherical Simplex Set, Papakonstantinou et al. 2022" # Rq.: W^{(m)}_{i=/=0} = (n + Kappa) / ((n + Lambda) * (n + 1 + Kappa)) - Winn = 1. / (self.Alpha**2 * (self.Nn + 1. + self.Kappa)) + Winn = 1. / ((self.Nn + 1. + self.Kappa) * self.Alpha**2) Ww = [] Ww.append( 0. ) for point in range(self.Nn + 1): @@ -1190,7 +1191,7 @@ class GenerateWeightsAndSigmaPoints(object): Wm[0] = LsLpL Wc = numpy.array( Ww ) Wc[0] = LsLpL + (1. - self.Alpha**2 + self.Beta) - # assert abs(Wm.sum()-1.) < self.Nn*mpr, "S3F ill-conditioned" + # OK: assert abs(Wm.sum()-1.) < self.Nn*mpr, "S3F ill-conditioned %s >= %s"%(abs(Wm.sum()-1.), self.Nn*mpr) # SC = numpy.zeros((self.Nn, len(Ww))) for ligne in range(self.Nn): @@ -1206,8 +1207,8 @@ class GenerateWeightsAndSigmaPoints(object): rho2 = (1 - self.Alpha) / self.Nn Cc = numpy.real(scipy.linalg.sqrtm( numpy.identity(self.Nn) - rho2 )) Ww = self.Alpha * rho2 * scipy.linalg.inv(Cc) @ numpy.ones(self.Nn) @ scipy.linalg.inv(Cc.T) - # Wm = Wc = numpy.concatenate((Ww, [self.Alpha])) + # OK: assert abs(Wm.sum()-1.) < self.Nn*mpr, "MSS ill-conditioned %s >= %s"%(abs(Wm.sum()-1.), self.Nn*mpr) # # inv(sqrt(W)) = diag(inv(sqrt(W))) SC1an = Cc @ numpy.diag(1. / numpy.sqrt( Ww )) @@ -1225,9 +1226,10 @@ class GenerateWeightsAndSigmaPoints(object): Ww.append( 1. / 36. ) Ww.append( (self.Nn**2 - 7 * self.Nn) / 18. + 1.) Wm = Wc = numpy.array( Ww ) + # OK: assert abs(Wm.sum()-1.) < self.Nn*mpr, "5OS ill-conditioned %s >= %s"%(abs(Wm.sum()-1.), self.Nn*mpr) # - xi1n = numpy.diag( 3. * numpy.ones( self.Nn ) ) - xi2n = numpy.diag( -3. * numpy.ones( self.Nn ) ) + xi1n = numpy.diag( math.sqrt(3) * numpy.ones( self.Nn ) ) + xi2n = numpy.diag( -math.sqrt(3) * numpy.ones( self.Nn ) ) # xi3n1 = numpy.zeros((int((self.Nn - 1) * self.Nn / 2), self.Nn), dtype=float) xi3n2 = numpy.zeros((int((self.Nn - 1) * self.Nn / 2), self.Nn), dtype=float) @@ -1236,11 +1238,11 @@ class GenerateWeightsAndSigmaPoints(object): ia = 0 for i1 in range(self.Nn - 1): for i2 in range(i1 + 1, self.Nn): - xi3n1[ia, i1] = xi3n2[ia, i2] = 3 - xi3n2[ia, i1] = xi3n1[ia, i2] = -3 + xi3n1[ia, i1] = xi3n2[ia, i2] = math.sqrt(3) + xi3n2[ia, i1] = xi3n1[ia, i2] = -math.sqrt(3) # -------------------------------- - xi4n1[ia, i1] = xi4n1[ia, i2] = 3 - xi4n2[ia, i1] = xi4n2[ia, i2] = -3 + xi4n1[ia, i1] = xi4n1[ia, i2] = math.sqrt(3) + xi4n2[ia, i1] = xi4n2[ia, i2] = -math.sqrt(3) ia += 1 SC = numpy.concatenate((xi1n, xi2n, xi3n1, xi3n2, xi4n1, xi4n2, numpy.zeros((1, self.Nn)))).T # -- 2.39.2