Done AB-tests between the current implementation and 2019 prototypes.

tests/src/filterpy/UDEKF.py

@@ -1,6 +1,7 @@
 # -*- coding: utf-8 -*-
 # pylint: disable=invalid-name,too-many-instance-attributes, too-many-arguments
 """
+Copyright 2022 Paul A Beltyukov
 Copyright 2019 Paul A Beltyukov
 Copyright 2015 Roger R Labbe Jr.
 

tests/src/filterpy/UDEKHFPrior2.py

@@ -0,0 +1,161 @@
+# -*- coding: utf-8 -*-
+# pylint: disable=invalid-name,too-many-instance-attributes, too-many-arguments
+"""
+Copyright 2022 Paul A Beltyukov
+Copyright 2019 Paul A Beltyukov
+Copyright 2015 Roger R Labbe Jr.
+
+FilterPy library.
+http://github.com/rlabbe/filterpy
+
+Documentation at:
+https://filterpy.readthedocs.org
+
+Supporting book at:
+https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
+
+This is licensed under an MIT license. See the readme.MD file
+for more information.
+"""
+import numpy as np
+from numpy import dot, outer
+from scipy.stats import chi2
+from UDEKF import mwgs, UDExtendedKalmanFilter
+
+
+class UDExtendedKalmanHinfFilterPrior2(UDExtendedKalmanFilter):
+
+    """
+    Implements an UD modification of extended Kalman/Hinfinity filter (EKHF)
+    with prior resuduals used for Hinfinity correction. You are responsible
+    for setting the various state variables to reasonable values;
+    the defaults will  not give you a functional filter.
+
+    You will have to set the following attributes after constructing this
+    object for the filter to perform properly. Please note that there are
+    various checks in place to ensure that you have made everything the
+    'correct' size. However, it is possible to provide incorrectly sized
+    arrays such that the linear algebra can not perform an operation.
+    It can also fail silently - you can end up with matrices of a size that
+    allows the linear algebra to work, but are the wrong shape for the problem
+    you are trying to solve.
+
+    Parameters
+    ----------
+
+    dim_x : int
+        Number of state variables for the Kalman filter. For example, if
+        you are tracking the position and velocity of an object in two
+        dimensions, dim_x would be 4.
+
+        This is used to set the default size of P, Q, and u
+
+    dim_z : int
+        Number of of measurement inputs. For example, if the sensor
+        provides you with position in (x,y), dim_z would be 2.
+
+    Attributes
+    ----------
+    x : numpy.array(dim_x, 1)
+        State estimate vector
+
+    P : numpy.array(dim_x, dim_x)
+        Covariance matrix
+
+    x_prior : numpy.array(dim_x, 1)
+        Prior (predicted) state estimate. The *_prior and *_post attributes
+        are for convienence; they store the  prior and posterior of the
+        current epoch. Read Only.
+
+    P_prior : numpy.array(dim_x, dim_x)
+        Prior (predicted) state covariance matrix. Read Only.
+
+    x_post : numpy.array(dim_x, 1)
+        Posterior (updated) state estimate. Read Only.
+
+    P_post : numpy.array(dim_x, dim_x)
+        Posterior (updated) state covariance matrix. Read Only.
+
+    R : numpy.array(dim_z, dim_z)
+        Measurement noise matrix
+
+    Q : numpy.array(dim_x, dim_x)
+        Process noise matrix
+
+    F : numpy.array()
+        State Transition matrix
+
+    H : numpy.array(dim_x, dim_x)
+        Measurement function
+
+    y : numpy.array
+        Residual of the update step. Read only.
+
+    K : numpy.array(dim_x, dim_z)
+        Kalman gain of the update step. Read only.
+
+    S :  numpy.array
+        Systen uncertaintly projected to measurement space. Read only.
+
+    z : ndarray
+        Last measurement used in update(). Read only.
+
+    log_likelihood : float
+        log-likelihood of the last measurement. Read only.
+
+    likelihood : float
+        likelihood of last measurment. Read only.
+
+        Computed from the log-likelihood. The log-likelihood can be very
+        small,  meaning a large negative value such as -28000. Taking the
+        exp() of that results in 0.0, which can break typical algorithms
+        which multiply by this value, so by default we always return a
+        number >= sys.float_info.min.
+
+    mahalanobis : float
+        mahalanobis distance of the innovation. E.g. 3 means measurement
+        was 3 standard deviations away from the predicted value.
+
+        Read only.
+    """
+
+    def __init__(self, dim_x, dim_z, dim_u=0, alpha = 0.001):
+        UDExtendedKalmanFilter.__init__(self, dim_x, dim_z, dim_u)
+        self.beta_1 = chi2.ppf(1.0 - alpha, 1)
+
+    def _scalar_update(self, nu, h, r):
+        """Joseph/Hinfinity scalar update using prior errors
+
+        Parameters
+        ----------
+
+        axis_residual : function which returns current axis residual
+                        returns scalar, float.
+
+        axis_hjacobian : function which returns current axis HJacobian row
+                         returns np.array, float.
+
+        r : scalar, float, current axis state disp
+        """
+        u, d, n = self.U, self.D, self.dim_x
+
+        f = h.dot(u)
+        v = d * f
+        c = f.dot(v)
+        s = c + r
+
+        a = (nu * nu) / self.beta_1 - s
+        if a > 0.0:
+            #Divergence detected, H-infinity correction needed
+            k = a / c + 1.0
+            d *= k
+            v *= k
+            s = k * c + r
+
+        K = u.dot(v / s).reshape((n, 1))
+
+        WW = np.concatenate((outer(K, f) - u, K), axis = 1)
+        DD = np.concatenate((d, np.array([r])))
+
+        self.U, self.D = mwgs(WW, DD)
+        self.x += (K*nu).reshape(self.x.shape)

tests/src/filterpy/testFilterpy01.py

@@ -19,9 +19,15 @@ import time
 import matplotlib.pyplot as plt
 import numpy as np
 
-from init_tests import *
-from init_tests import _fx, _jfx, _hx, _jhx
+import init_tests
 
+from ab_tests import *
+from case1    import *
+from case1    import _fx, _jfx, _hx, _jhx
+
+from UDEKF import UDExtendedKalmanFilter
+
+from yaflpy import Bierman as B
 #------------------------------------------------------------------------------
 N = 10000
 STD = 100.
@@ -30,26 +36,42 @@ STD = 100.
 clean, noisy, t = case_data(N, STD)
 
 #------------------------------------------------------------------------------
-class A(ExtendedKalmanFilter):
+class A(UDExtendedKalmanFilter):
     def update(self, z):
         super().update(z, self.jhx, self.hx)
+        self.y = np.dot(self.Dm, self.y)
 
 a = A(4,2)
 a.x = np.array([0, 0.3, 0, 0])
 a.F = _jfx(a.x, 1.0)
-a.P *= 0.0001
-a.Q *= 1e-6
-a.R *= STD * STD
+
+aup = np.array([[1, 1e-8, 1e-8, 1e-8],
+                [0, 1,    1e-8, 1e-8],
+                [0, 0,    1,    1e-8],
+                [0, 0,    0,    1]])
+adp = 0.00001 * np.eye(4)
+a.P = aup.dot(adp.dot(aup.T))
+
+auq = np.array([[1, 1e-8, 1e-8, 1e-8],
+                [0, 1,    1e-8, 1e-8],
+                [0, 0,    1,    1e-8],
+                [0, 0,    0,    1]])
+adq = 1e-6 * np.eye(4)
+a.Q = auq.dot(adq.dot(auq.T))
+
+
+aur = np.array([[1, 0.5],
+                [0, 1   ]])
+
+adr = STD * STD * np.eye(2)
+adr[0,0] *= 0.75
+a.R = aur.dot(adr.dot(aur.T))
+
 a.hx  = _hx
 a.jhx = _jhx
 
 #------------------------------------------------------------------------------
-sp = JulierSigmaPoints(4, 0.0)
-b  = UnscentedKalmanFilter(4, 2, 1.0, _hx, _fx, sp)
-b.x = np.array([0, 0.3, 0, 0])
-b.P *= 0.0001
-b.Q *= 1e-6
-b.R *= STD * STD
+b = case_ekf(B, STD)
 
 #------------------------------------------------------------------------------
 start = time.time()

tests/src/filterpy/testFilterpy02.py

@@ -25,10 +25,9 @@ from ab_tests import *
 from case1    import *
 from case1    import _fx, _jfx, _hx, _jhx
 
-from yaflpy import _mwgs, _ruv
+from UDEKF import UDExtendedKalmanFilter
 
-from filterpy.kalman import ExtendedKalmanFilter
-from yaflpy          import Bierman as B
+from yaflpy import Bierman as B
 #------------------------------------------------------------------------------
 N = 10000
 STD = 100.
@@ -37,13 +36,21 @@ STD = 100.
 clean, noisy, t = case_data(N, STD)
 
 #------------------------------------------------------------------------------
-class A(ExtendedKalmanFilter):
+class A(UDExtendedKalmanFilter):
     def update(self, z):
+
+        if self.rff > 0:
+            self.R *= 1.0 - self.rff
+            self.R += self.rff * np.outer(self.y, self.y)
+            h = self.jhx(self.x)
+            self.R += self.rff * h.dot(self.P.dot(h.T))
+
         super().update(z, self.jhx, self.hx)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self.dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self.dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self.dim_z))
-        _, self.y = _ruv(self.Ur, self.y)
+
+        if self.rff > 0:
+            self.y = z - self.hx(self.x)
+        else:
+            self.y = np.dot(self.Dm, self.y)
 
 a = A(4,2)
 a.x = np.array([0, 0.3, 0, 0])
@@ -74,35 +81,32 @@ a.R = aur.dot(adr.dot(aur.T))
 a.hx  = _hx
 a.jhx = _jhx
 
+a.rff = 0.001
+
 #------------------------------------------------------------------------------
 b = case_ekf(B, STD)
-
+b.rff = 0.001
 #------------------------------------------------------------------------------
 start = time.time()
 
-rpa,rua,xa, rpb,rub,xb, nup,ndp, nuq,ndq, nur,ndr, nx, ny = yafl_ab_test(a, b, noisy)
+xa,xb, nP,nQ,nR, nx,ny = filterpy_ab_test(a, b, noisy)
 
 end = time.time()
 print(end - start)
 
 #------------------------------------------------------------------------------
-plt.plot(nup)
-plt.show()
-plt.plot(ndp)
+plt.plot(nP)
 plt.show()
 
-plt.plot(nuq)
-plt.show()
-plt.plot(ndq)
+plt.plot(nQ)
 plt.show()
 
-plt.plot(nur)
-plt.show()
-plt.plot(ndr)
+plt.plot(nR)
 plt.show()
 
 plt.plot(nx)
 plt.show()
+
 plt.plot(ny)
 plt.show()
 

tests/src/filterpy/testFilterpy03.py

@@ -25,12 +25,9 @@ from ab_tests import *
 from case1    import *
 from case1    import _fx, _jfx, _hx, _jhx
 
-from yaflpy import _mwgs, _ruv
+from UDEKF import UDExtendedKalmanFilter
 
-from filterpy.kalman import UnscentedKalmanFilter
-from filterpy.kalman import JulierSigmaPoints
-
-from yaflpy          import Joseph as B
+from yaflpy import Bierman as B
 #------------------------------------------------------------------------------
 N = 10000
 STD = 100.
@@ -39,18 +36,40 @@ STD = 100.
 clean, noisy, t = case_data(N, STD)
 
 #------------------------------------------------------------------------------
-class A(UnscentedKalmanFilter):
+class A(UDExtendedKalmanFilter):
+
+    def _scalar_update(self, nu, h, r):
+
+        x_prior = self.x.copy()
+
+        super()._scalar_update(nu, h, r)
+
+        if self.qff > 0:
+            knu = self.x - x_prior
+            self.Q += self.qff * np.outer(knu, knu)
+
+
     def update(self, z):
-        super().update(z)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self._dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self._dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self._dim_z))
-        _, self.y = _ruv(self.Ur, self.y)
 
-sp = JulierSigmaPoints(4, 0.0)
-a = A(4, 2, 1.0, _hx, _fx, sp)
+        if self.rff > 0:
+            self.R *= 1.0 - self.rff
+            self.R += self.rff * np.outer(self.y, self.y)
+            h = self.jhx(self.x)
+            self.R += self.rff * h.dot(self.P.dot(h.T))
+
+        if self.qff > 0:
+            self.Q *= 1.0 - self.qff
+
+        super().update(z, self.jhx, self.hx)
 
+        if self.rff > 0:
+            self.y = z - self.hx(self.x)
+        else:
+            self.y = np.dot(self.Dm, self.y)
+
+a = A(4,2)
 a.x = np.array([0, 0.3, 0, 0])
+a.F = _jfx(a.x, 1.0)
 
 aup = np.array([[1, 1e-8, 1e-8, 1e-8],
                 [0, 1,    1e-8, 1e-8],
@@ -74,35 +93,37 @@ adr = STD * STD * np.eye(2)
 adr[0,0] *= 0.75
 a.R = aur.dot(adr.dot(aur.T))
 
+a.hx  = _hx
+a.jhx = _jhx
+
+a.rff = 0.001
+a.qff = 0.0001
+
 #------------------------------------------------------------------------------
 b = case_ekf(B, STD)
-
+b.rff = 0.001
+b.qff = 0.0001
 #------------------------------------------------------------------------------
 start = time.time()
 
-rpa,rua,xa, rpb,rub,xb, nup,ndp, nuq,ndq, nur,ndr, nx, ny = yafl_ab_test(a, b, noisy)
+xa,xb, nP,nQ,nR, nx,ny = filterpy_ab_test(a, b, noisy)
 
 end = time.time()
 print(end - start)
 
 #------------------------------------------------------------------------------
-plt.plot(nup)
-plt.show()
-plt.plot(ndp)
+plt.plot(nP)
 plt.show()
 
-plt.plot(nuq)
-plt.show()
-plt.plot(ndq)
+plt.plot(nQ)
 plt.show()
 
-plt.plot(nur)
-plt.show()
-plt.plot(ndr)
+plt.plot(nR)
 plt.show()
 
 plt.plot(nx)
 plt.show()
+
 plt.plot(ny)
 plt.show()
 

tests/src/filterpy/testFilterpy04.py

@@ -25,12 +25,9 @@ from ab_tests import *
 from case1    import *
 from case1    import _fx, _jfx, _hx, _jhx
 
-from yaflpy import _mwgs, _ruv
+from UDEKHFPrior2 import UDExtendedKalmanHinfFilterPrior2
 
-from filterpy.kalman import UnscentedKalmanFilter
-from filterpy.kalman import JulierSigmaPoints
-
-from yaflpy          import Unscented as B
+from yaflpy import AdaptiveBierman as B
 #------------------------------------------------------------------------------
 N = 10000
 STD = 100.
@@ -39,19 +36,14 @@ STD = 100.
 clean, noisy, t = case_data(N, STD)
 
 #------------------------------------------------------------------------------
-class A(UnscentedKalmanFilter):
+class A(UDExtendedKalmanHinfFilterPrior2):
     def update(self, z):
-        super().update(z)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self._dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self._dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self._dim_z))
-        _, us, ds = _mwgs(np.linalg.cholesky(self.S), np.ones(self._dim_z))
-        _, self.y = _ruv(us, self.y)
-
-sp = JulierSigmaPoints(4, 0.0)
-a = A(4, 2, 1.0, _hx, _fx, sp)
+        super().update(z, self.jhx, self.hx)
+        self.y = np.dot(self.Dm, self.y)
 
+a = A(4,2)
 a.x = np.array([0, 0.3, 0, 0])
+a.F = _jfx(a.x, 1.0)
 
 aup = np.array([[1, 1e-8, 1e-8, 1e-8],
                 [0, 1,    1e-8, 1e-8],
@@ -75,35 +67,36 @@ adr = STD * STD * np.eye(2)
 adr[0,0] *= 0.75
 a.R = aur.dot(adr.dot(aur.T))
 
+a.hx  = _hx
+a.jhx = _jhx
+
+a.beta_1 = 10.827566170662733
+
 #------------------------------------------------------------------------------
-b = case_ukf(B, STD)
+b = case_ekf(B, STD)
+b.chi2 = 10.827566170662733
 
 #------------------------------------------------------------------------------
 start = time.time()
 
-rpa,rua,xa, rpb,rub,xb, nup,ndp, nuq,ndq, nur,ndr, nx, ny = yafl_ab_test(a, b, noisy)
+xa,xb, nP,nQ,nR, nx,ny = filterpy_ab_test(a, b, noisy)
 
 end = time.time()
 print(end - start)
 
 #------------------------------------------------------------------------------
-plt.plot(nup)
-plt.show()
-plt.plot(ndp)
+plt.plot(nP)
 plt.show()
 
-plt.plot(nuq)
-plt.show()
-plt.plot(ndq)
+plt.plot(nQ)
 plt.show()
 
-plt.plot(nur)
-plt.show()
-plt.plot(ndr)
+plt.plot(nR)
 plt.show()
 
 plt.plot(nx)
 plt.show()
+
 plt.plot(ny)
 plt.show()
 

tests/src/filterpy/testFilterpy05.py

@@ -1,112 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-import init_tests
-
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from yaflpy import _mwgs, _ruv
-
-from filterpy.kalman import UnscentedKalmanFilter
-from filterpy.kalman import JulierSigmaPoints
-
-from yaflpy          import UnscentedBierman as B
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-class A(UnscentedKalmanFilter):
-    def update(self, z):
-        super().update(z)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self._dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self._dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self._dim_z))
-        _, self.y = _ruv(self.Ur, self.y)
-
-sp = JulierSigmaPoints(4, 0.0)
-a = A(4, 2, 1.0, _hx, _fx, sp)
-
-a.x = np.array([0, 0.3, 0, 0])
-
-aup = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adp = 0.00001 * np.eye(4)
-a.P = aup.dot(adp.dot(aup.T))
-
-auq = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adq = 1e-6 * np.eye(4)
-a.Q = auq.dot(adq.dot(auq.T))
-
-
-aur = np.array([[1, 0.5],
-                [0, 1   ]])
-
-adr = STD * STD * np.eye(2)
-adr[0,0] *= 0.75
-a.R = aur.dot(adr.dot(aur.T))
-
-#------------------------------------------------------------------------------
-b = case_ukf(B, STD)
-
-#------------------------------------------------------------------------------
-start = time.time()
-
-rpa,rua,xa, rpb,rub,xb, nup,ndp, nuq,ndq, nur,ndr, nx, ny = yafl_ab_test(a, b, noisy)
-
-end = time.time()
-print(end - start)
-
-#------------------------------------------------------------------------------
-plt.plot(nup)
-plt.show()
-plt.plot(ndp)
-plt.show()
-
-plt.plot(nuq)
-plt.show()
-plt.plot(ndq)
-plt.show()
-
-plt.plot(nur)
-plt.show()
-plt.plot(ndr)
-plt.show()
-
-plt.plot(nx)
-plt.show()
-plt.plot(ny)
-plt.show()
-
-plt.plot(noisy[:,0], noisy[:,1], xa[:,0], xa[:,2])
-plt.show()
-plt.plot(clean[:,0], clean[:,1], xa[:,0], xa[:,2])
-plt.show()

tests/src/filterpy/testFilterpy06.py

@@ -1,154 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-import init_tests
-
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from yaflpy import _mwgs, _ruv
-
-from filterpy.kalman import UnscentedKalmanFilter
-from filterpy.kalman import JulierSigmaPoints
-
-from yaflpy          import Unscented as B
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-class A(UnscentedKalmanFilter):
-    def update(self, z):
-
-        if (self.rff > 0):
-            self.R *= 1.0 - self.rff
-            self.R += self.rff * np.outer(self.y, self.y)
-
-            sigmas = []
-            for s in self.sigmas_f:
-                sigmas.append(self.hx(s))
-            sigmas = np.array(sigmas)
-
-            zp = np.dot(self.Wm, sigmas)
-            y = sigmas - zp[np.newaxis, :]
-            self.R += np.dot(y.T, np.dot(np.diag(self.Wc * self.rff), y))
-            self.R = (self.R + self.R.T) / 2.
-
-        super().update(z)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self._dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self._dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self._dim_z))
-
-        if self.rff > 0:
-            self.compute_process_sigmas(self._dt, self.fx)
-
-            sigmas = []
-            for s in self.sigmas_f:
-                sigmas.append(self.hx(s))
-            sigmas = np.array(sigmas)
-
-            self.y = self.residual_z(z, np.dot(self.Wm, sigmas))
-        else:
-            _, us, ds = _mwgs(np.linalg.cholesky(self.S), np.ones(self._dim_z))
-            _, self.y = _ruv(us, self.y)
-
-
-
-sp = JulierSigmaPoints(4, 0.0)
-a = A(4, 2, 1.0, _hx, _fx, sp)
-a._res = a.y
-#a.dim_x = 4
-#a.dim_z = 2
-
-a.x = np.array([0, 0.3, 0, 0])
-
-aup = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adp = 0.00001 * np.eye(4)
-a.P = aup.dot(adp.dot(aup.T))
-
-auq = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adq = 1e-6 * np.eye(4)
-a.Q = auq.dot(adq.dot(auq.T))
-
-
-aur = np.array([[1, 0.5],
-                [0, 1   ]])
-
-adr = STD * STD * np.eye(2)
-adr[0,0] *= 0.75
-a.R = aur.dot(adr.dot(aur.T))
-a.rff = 1e-3
-
-#------------------------------------------------------------------------------
-b = case_ukf(B, STD)
-b.rff = 1e-3
-
-#------------------------------------------------------------------------------
-start = time.time()
-
-rpa,rua,xa, rpb,rub,xb, nup,ndp, nuq,ndq, nur,ndr, nx, ny = yafl_ab_test(a, b, noisy)
-
-end = time.time()
-print(end - start)
-
-#------------------------------------------------------------------------------
-plt.plot(nup)
-plt.show()
-plt.plot(ndp)
-plt.show()
-
-plt.plot(nuq)
-plt.show()
-plt.plot(ndq)
-plt.show()
-
-plt.plot(nur)
-plt.show()
-plt.plot(ndr)
-plt.show()
-
-plt.plot(nx)
-plt.show()
-plt.plot(ny)
-plt.show()
-
-plt.plot(noisy[:,0], noisy[:,1], xa[:,0], xa[:,2])
-plt.show()
-plt.plot(clean[:,0], clean[:,1], xa[:,0], xa[:,2])
-plt.show()
-
-plt.plot(noisy[:,0], noisy[:,1], xb[:,0], xb[:,2])
-plt.show()
-plt.plot(clean[:,0], clean[:,1], xb[:,0], xb[:,2])
-plt.show()
-
-plt.plot(xa[:,[0,2]]-xb[:,[0,2]])
-plt.show()

tests/src/filterpy/testFilterpy07.py

@@ -1,128 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-import init_tests
-
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from yaflpy import _mwgs, _ruv
-
-from filterpy.kalman import ExtendedKalmanFilter
-from yaflpy          import Joseph as B
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-class A(ExtendedKalmanFilter):
-    def update(self, z):
-        #R update
-        if self.rff > 0:
-            self.R *= 1.0 - self.rff
-            self.R += self.rff * np.outer(self.y, self.y)
-            h = self.jhx(self.x)
-            self.R += self.rff * h.dot(self.P.dot(h.T))
-
-        super().update(z, self.jhx, self.hx)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self.dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self.dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self.dim_z))
-        if self.rff > 0:
-            self.y = z - self.hx(self.x)
-        else:
-            _, self.y = _ruv(self.Ur, self.y)
-
-a = A(4,2)
-a.x = np.array([0, 0.3, 0, 0])
-a.F = _jfx(a.x, 1.0)
-
-aup = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adp = 0.00001 * np.eye(4)
-a.P = aup.dot(adp.dot(aup.T))
-
-auq = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adq = 1e-6 * np.eye(4)
-a.Q = auq.dot(adq.dot(auq.T))
-
-
-aur = np.array([[1, 0.5],
-                [0, 1   ]])
-
-adr = STD * STD * np.eye(2)
-adr[0,0] *= 0.75
-a.R = aur.dot(adr.dot(aur.T))
-
-a.hx  = _hx
-a.jhx = _jhx
-
-a.rff = 0.#001
-
-#------------------------------------------------------------------------------
-b = case_ekf(B, STD)
-b.rff = 0.#001
-
-#------------------------------------------------------------------------------
-start = time.time()
-
-rpa,rua,xa, rpb,rub,xb, nup,ndp, nuq,ndq, nur,ndr, nx, ny = yafl_ab_test(a, b, noisy)
-
-end = time.time()
-print(end - start)
-
-#------------------------------------------------------------------------------
-plt.plot(nup)
-plt.show()
-plt.plot(ndp)
-plt.show()
-
-plt.plot(nuq)
-plt.show()
-plt.plot(ndq)
-plt.show()
-
-plt.plot(nur)
-plt.show()
-plt.plot(ndr)
-plt.show()
-
-plt.plot(nx)
-plt.show()
-plt.plot(ny)
-plt.show()
-
-plt.plot(noisy[:,0], noisy[:,1], xa[:,0], xa[:,2])
-plt.show()
-plt.plot(clean[:,0], clean[:,1], xa[:,0], xa[:,2])
-plt.show()
-
-plt.plot(xa[:,[0,2]]-xb[:,[0,2]])
-plt.show()

tests/src/filterpy/testFilterpy08.py

@@ -1,93 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-import init_tests
-
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from filterpy.kalman import ExtendedKalmanFilter
-from UDEKF import UDExtendedKalmanFilter
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-class A(ExtendedKalmanFilter):
-    def update(self, z):
-        super().update(z, self.jhx, self.hx)
-
-a = A(4,2)
-a.x = np.array([0, 0.3, 0, 0])
-a.F = _jfx(a.x, 1.0)
-a.P *= 0.0001
-a.Q *= 1e-6
-a.R *= STD * STD
-a.hx  = _hx
-a.jhx = _jhx
-
-#------------------------------------------------------------------------------
-class B(UDExtendedKalmanFilter):
-    def update(self, z):
-        super().update(z, self.jhx, self.hx)
-
-
-b = B(4,2)
-b.x = np.array([0, 0.3, 0, 0])
-b.F = _jfx(a.x, 1.0)
-b.P *= 0.0001
-b.Q *= 1e-6
-b.R *= STD * STD
-b.hx  = _hx
-b.jhx = _jhx
-
-#------------------------------------------------------------------------------
-start = time.time()
-
-xa,xb, nP,nQ,nR, nx,ny = filterpy_ab_test(a, b, noisy)
-
-end = time.time()
-print(end - start)
-
-#------------------------------------------------------------------------------
-plt.plot(nP)
-plt.show()
-
-plt.plot(nQ)
-plt.show()
-
-plt.plot(nR)
-plt.show()
-
-plt.plot(nx)
-plt.show()
-
-plt.plot(ny)
-plt.show()
-
-plt.plot(noisy[:,0], noisy[:,1], xa[:,0], xa[:,2])
-plt.show()
-plt.plot(clean[:,0], clean[:,1], xa[:,0], xa[:,2])
-plt.show()

tests/src/filterpy/testFilterpy09.py

@@ -1,146 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-import init_tests
-
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from yaflpy import _mwgs, _ruv, _rum, _set_u
-
-from filterpy.kalman import ExtendedKalmanFilter
-from yaflpy          import Joseph as B
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-class A(ExtendedKalmanFilter):
-    def update(self, z):
-        super().update(z, self.jhx, self.hx)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self.dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self.dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self.dim_z))
-        _, self.y = _ruv(self.Ur, self.y)
-
-a = A(4,2)
-a.x = np.array([0, 0.3, 0, 0])
-a.F = _jfx(a.x, 1.0)
-
-aup = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adp = 0.00001 * np.eye(4)
-a.P = aup.dot(adp.dot(aup.T))
-
-auq = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adq = 1e-6 * np.eye(4)
-a.Q = auq.dot(adq.dot(auq.T))
-
-
-aur = np.array([[1, 0.5],
-                [0, 1   ]])
-
-adr = STD * STD * np.eye(2)
-adr[0,0] *= 0.75
-a.R = aur.dot(adr.dot(aur.T))
-
-#a.R = STD * STD * np.eye(2)
-
-a.hx  = _hx
-a.jhx = _jhx
-
-#------------------------------------------------------------------------------
-b = case_ekf(B, STD)
-
-#b.Ur *= 0.
-#b.Dr[0] *= 4./3.
-#------------------------------------------------------------------------------
-start = time.time()
-
-dxp = []
-dxu = []
-dy = []
-duu = []
-ddu = []
-dup = []
-ddp = []
-
-for z in noisy:
-    b.x = a.x
-
-    #Не влияет на расхождение
-    #_, b.Up, b.Dp = _mwgs(np.linalg.cholesky(a.P), np.ones(a.dim_x))
-
-    #Тут всё 1 в 1
-    a.predict()
-    b.predict()
-    dxp.append(2. * norm(a.x - b.x) / norm(a.x + b.x))
-
-    _, a.Up, a.Dp = _mwgs(np.linalg.cholesky(a.P), np.ones(a.dim_x))
-    dup.append(2. * norm(a.Up - b.Up) / norm(a.Up + b.Up))
-    ddp.append(2. * norm(a.Dp - b.Dp) / norm(a.Dp + b.Dp))
-
-    #Расхождение тут!
-    a.update(z)
-    b.update(z)
-    dxu.append(2. * norm(a.x - b.x) / norm(a.x + b.x))
-    dy.append(2. * norm(a.y - b.y) / norm(a.y + b.y))
-
-    duu.append(2. * norm(a.Up - b.Up) / norm(a.Up + b.Up))
-    ddu.append(2. * norm(a.Dp - b.Dp) / norm(a.Dp + b.Dp))
-
-
-
-end = time.time()
-print(end - start)
-
-plt.plot(dxp)
-plt.show()
-
-plt.plot(dxu)
-plt.show()
-
-plt.plot(dy)
-plt.show()
-
-plt.plot(duu)
-plt.show()
-
-plt.plot(ddu)
-plt.show()
-
-plt.plot(dup)
-plt.show()
-
-plt.plot(ddp)
-plt.show()
-
-#------------------------------------------------------------------------------
-

tests/src/filterpy/testFilterpy10.py

@@ -1,121 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-import init_tests
-
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from yaflpy import _mwgs, _ruv, _rum, _set_u
-
-from filterpy.kalman import ExtendedKalmanFilter
-from yaflpy          import Joseph as B
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-class A(ExtendedKalmanFilter):
-    def update(self, z):
-        super().update(z, self.jhx, self.hx)
-        _, self.Up, self.Dp = _mwgs(np.linalg.cholesky(self.P), np.ones(self.dim_x))
-        _, self.Uq, self.Dq = _mwgs(np.linalg.cholesky(self.Q), np.ones(self.dim_x))
-        _, self.Ur, self.Dr = _mwgs(np.linalg.cholesky(self.R), np.ones(self.dim_z))
-        _, self.y = _ruv(self.Ur, self.y)
-
-a = A(4,2)
-a.x = np.array([0, 0.3, 0, 0])
-a.F = _jfx(a.x, 1.0)
-
-aup = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adp = 0.00001 * np.eye(4)
-a.P = aup.dot(adp.dot(aup.T))
-
-auq = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adq = 1e-6 * np.eye(4)
-a.Q = auq.dot(adq.dot(auq.T))
-
-
-#aur = np.array([[1, 0.5],
-#                [0, 1   ]])
-
-#adr = STD * STD * np.eye(2)
-#adr[0,0] *= 0.75
-#a.R = aur.dot(adr.dot(aur.T))
-
-a.R = STD * STD * np.eye(2)
-
-a.hx  = _hx
-a.jhx = _jhx
-
-#------------------------------------------------------------------------------
-b = case_ekf(B, STD)
-
-b.Ur *= 0.
-b.Dr[0] *= 4./3.
-#------------------------------------------------------------------------------
-start = time.time()
-
-dxp = []
-dxu = []
-dy = []
-
-for z in noisy:
-    #b.x = a.x
-
-    #Не влияет на расхождение
-    #_, b.Up, b.Dp = _mwgs(np.linalg.cholesky(a.P), np.ones(a.dim_x))
-
-    #Тут всё 1 в 1
-    a.predict()
-    b.predict()
-    dxp.append(2. * norm(a.x - b.x) / norm(a.x + b.x))
-
-    #Расхождение тут!
-    a.update(z)
-    b.update(z)
-    dxu.append(2. * norm(a.x - b.x) / norm(a.x + b.x))
-    dy.append(2. * norm(a.y - b.y) / norm(a.y + b.y))
-
-end = time.time()
-print(end - start)
-
-plt.plot(dxp)
-plt.show()
-
-plt.plot(dxu)
-plt.show()
-
-plt.plot(dy)
-plt.show()
-
-#------------------------------------------------------------------------------
-

tests/src/filterpy/testFilterpy11.py

@@ -1,109 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-import init_tests
-
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from filterpy.kalman import ExtendedKalmanFilter
-from UDEKF import UDExtendedKalmanFilter
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-class A(ExtendedKalmanFilter):
-    def update(self, z):
-        super().update(z, self.jhx, self.hx)
-
-a = A(4,2)
-a.x = np.array([0, 0.3, 0, 0])
-a.F = _jfx(a.x, 1.0)
-a.P *= 0.00001
-
-auq = np.array([[1, 1e-8, 1e-8, 1e-8],
-                [0, 1,    1e-8, 1e-8],
-                [0, 0,    1,    1e-8],
-                [0, 0,    0,    1]])
-adq = 1e-6 * np.eye(4)
-a.Q = auq.dot(adq.dot(auq.T))
-
-
-aur = np.array([[1, 0.5],
-                [0, 1   ]])
-
-adr = STD * STD * np.eye(2)
-adr[0,0] *= 0.75
-a.R = aur.dot(adr.dot(aur.T))
-
-a.hx  = _hx
-a.jhx = _jhx
-
-#------------------------------------------------------------------------------
-class B(UDExtendedKalmanFilter):
-    def update(self, z):
-        super().update(z, self.jhx, self.hx)
-
-
-b = B(4,2)
-b.x = np.array([0, 0.3, 0, 0])
-b.F = _jfx(a.x, 1.0)
-b.P *= 0.00001
-
-b.Q = auq.dot(adq.dot(auq.T))
-b.R = aur.dot(adr.dot(aur.T))
-
-b.hx  = _hx
-b.jhx = _jhx
-
-#------------------------------------------------------------------------------
-start = time.time()
-
-xa,xb, nP,nQ,nR, nx,ny = filterpy_ab_test(a, b, noisy)
-
-end = time.time()
-print(end - start)
-
-#------------------------------------------------------------------------------
-plt.plot(nP)
-plt.show()
-
-plt.plot(nQ)
-plt.show()
-
-plt.plot(nR)
-plt.show()
-
-plt.plot(nx)
-plt.show()
-
-plt.plot(ny)
-plt.show()
-
-plt.plot(noisy[:,0], noisy[:,1], xa[:,0], xa[:,2])
-plt.show()
-plt.plot(clean[:,0], clean[:,1], xa[:,0], xa[:,2])
-plt.show()

tests/src/filterpy/testFilterpy12.py

@@ -1,70 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright 2022 anonimous <shkolnick-kun@gmail.com> and contributors.
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-    http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing,
-    software distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
-    See the License for the specific language governing permissions
-"""
-import time
-import matplotlib.pyplot as plt
-import numpy as np
-
-from init_tests import *
-
-from yaflpy          import Bierman as B
-#------------------------------------------------------------------------------
-N = 10000
-STD = 100.
-
-#------------------------------------------------------------------------------
-clean, noisy, t = case_data(N, STD)
-
-#------------------------------------------------------------------------------
-a = new_seq_ekf(STD)
-
-#------------------------------------------------------------------------------
-b = case_ekf(B, STD)
-
-#------------------------------------------------------------------------------
-start = time.time()
-
-rpa,rua,xa, rpb,rub,xb, nup,ndp, nuq,ndq, nur,ndr, nx, ny = yafl_ab_test(a, b, noisy)
-
-end = time.time()
-print(end - start)
-
-#------------------------------------------------------------------------------
-plt.plot(nup)
-plt.show()
-plt.plot(ndp)
-plt.show()
-
-plt.plot(nuq)
-plt.show()
-plt.plot(ndq)
-plt.show()
-
-plt.plot(nur)
-plt.show()
-plt.plot(ndr)
-plt.show()
-
-plt.plot(nx)
-plt.show()
-plt.plot(ny)
-plt.show()
-
-plt.plot(noisy[:,0], noisy[:,1], xa[:,0], xa[:,2])
-plt.show()
-plt.plot(clean[:,0], clean[:,1], xa[:,0], xa[:,2])
-plt.show()