Adding FilterPy tests.

src/yaflpy/yaflpy.pyx

@@ -2133,7 +2133,7 @@ def _rum(res, u):
 
     nr = res.shape[0]
     nc = res.shape[1]
-    assert _U_sz(nc) == u.shape[0]
+    assert _U_sz(nr) == u.shape[0]
 
     cdef yaflFloat [:, ::1] v_r = res
     cdef yaflFloat [::1]    v_u = u
@@ -2430,6 +2430,36 @@ cdef class yaflKalmanBase:
     def Dr(self, value):
         self._Dr[:] = value
 
+    #--------------------------------------------------------------------------
+    @property
+    def P(self):
+        _,u = _set_u(self._Up)
+        return u.dot(np.diag(self._Dp).dot(u.T))
+
+    @P.setter
+    def P(self, value):
+        raise AttributeError('yaflKalmanBase does not support this!')
+
+    #--------------------------------------------------------------------------
+    @property
+    def Q(self):
+        _,u = _set_u(self._Uq)
+        return u.dot(np.diag(self._Dq).dot(u.T))
+
+    @Q.setter
+    def Q(self, value):
+        raise AttributeError('yaflKalmanBase does not support this!')
+
+    #--------------------------------------------------------------------------
+    @property
+    def R(self):
+        _,u = _set_u(self._Ur)
+        return u.dot(np.diag(self._Dr).dot(u.T))
+
+    @R.setter
+    def R(self, value):
+        raise AttributeError('yaflKalmanBase does not support this!')
+
     #--------------------------------------------------------------------------
     @property
     def rff(self):

tests/src/filterpy/init_tests.py

@@ -1,3 +1,73 @@
+#!/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 sys
 sys.path.append("../yafl")
+
+import numpy as np
+
+from filterpy.kalman import ExtendedKalmanFilter
+from filterpy.kalman import UnscentedKalmanFilter
+from filterpy.kalman import JulierSigmaPoints
+
+from ab_tests import *
+from case1    import *
+from case1    import _fx, _jfx, _hx, _jhx
+
+from yaflpy import _mwgs, _ruv, _rum, _set_u
+
+class SeqEKF(ExtendedKalmanFilter):
+
+    def shx(self, x):
+        return _ruv(self.Ur, _hx(x))[1]
+
+    def sjhx(self, x):
+        sjhx = _jhx(x)
+        _rum(sjhx, self.Ur)
+        return sjhx
+
+    def update(self, z):
+        # Decorrelate noice z
+        zd = _ruv(self.Ur, z)[1]
+        super().update(zd, self.sjhx, self.shx)
+        _, 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))
+
+def new_seq_ekf(std):
+    sz = 4
+
+    ekf = SeqEKF(sz, 2)
+    ekf.x = np.array([0, 0.3, 0, 0])
+    ekf.F = _jfx(ekf.x, 1.0)
+
+    up = _set_u(1e-8 * np.ones(((sz*(sz-1))//2,)))[1]
+    dp = 0.00001 * np.eye(sz)
+    ekf.P = up.dot(dp.dot(up.T))
+
+    uq = up.copy()
+    dq = 1e-6 * np.eye(sz)
+    ekf.Q = uq.dot(dq.dot(uq.T))
+
+    ekf.Ur = np.array([0.5])
+    ekf.Dr = std * std * np.array([0.75, 1.0])
+
+    #ur = _set_u(ekf.Ur)[1]
+    #dr = np.diag(ekf.Dr)
+
+    ekf.R = np.diag(ekf.Dr) #ur.dot(dr.dot(ur.T))
+
+    return ekf

tests/src/filterpy/testFilterpy01.py

@@ -19,15 +19,9 @@ import time
 import matplotlib.pyplot as plt
 import numpy as np
 
-import init_tests
+from init_tests import *
+from init_tests import _fx, _jfx, _hx, _jhx
 
-from ab_tests import *
-from case1    import *
-from case1    import _fx, _jfx, _hx, _jhx
-
-from filterpy.kalman import ExtendedKalmanFilter
-from filterpy.kalman import UnscentedKalmanFilter
-from filterpy.kalman import JulierSigmaPoints
 #------------------------------------------------------------------------------
 N = 10000
 STD = 100.

tests/src/filterpy/testFilterpy11.py

@@ -42,7 +42,7 @@ class A(ExtendedKalmanFilter):
 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.P *= 0.00001
 
 auq = np.array([[1, 1e-8, 1e-8, 1e-8],
                 [0, 1,    1e-8, 1e-8],
@@ -71,7 +71,7 @@ class B(UDExtendedKalmanFilter):
 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.P *= 0.00001
 
 b.Q = auq.dot(adq.dot(auq.T))
 b.R = aur.dot(adr.dot(aur.T))

tests/src/filterpy/testFilterpy12.py

@@ -0,0 +1,70 @@
+#!/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()