From 5735811e285323a2b3895b7692d462b552022101 Mon Sep 17 00:00:00 2001 From: Freja Nordsiek Date: Tue, 29 Jun 2021 14:24:45 +0200 Subject: [PATCH 1/7] Fixed the names of the different parts of the QR decomposition result in the update step of the square root decomposition with regards to K, S, and the square root of S. --- filterpy/kalman/square_root.py | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/filterpy/kalman/square_root.py b/filterpy/kalman/square_root.py index 6dd445c7..0a11a039 100644 --- a/filterpy/kalman/square_root.py +++ b/filterpy/kalman/square_root.py @@ -110,9 +110,6 @@ class SquareRootKalmanFilter(object): 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. - Examples -------- @@ -151,8 +148,9 @@ def __init__(self, dim_x, dim_z, dim_u=0): self._R = eye(dim_z) # state uncertainty self.z = np.array([[None]*self.dim_z]).T - self.K = 0. - self.S = 0. + self.K = np.zeros((dim_x, dim_z)) # kalman gain + self.S1_2 = np.zeros((dim_z, dim_z)) # sqrt system uncertainty + self.SI1_2 = np.zeros((dim_z, dim_z)) # Inverse sqrt system uncertainty # Residual is computed during the innovation (update) step. We # save it so that in case you want to inspect it for various @@ -207,9 +205,10 @@ def update(self, z, R2=None): M[dim_z:, 0:dim_z] = dot(self.H, self._P1_2).T M[dim_z:, dim_z:] = self._P1_2.T - _, self.S = qr(M) - self.K = self.S[0:dim_z, dim_z:].T - N = self.S[0:dim_z, 0:dim_z].T + _, r_decomp = qr(M) + self.S1_2 = r_decomp[0:dim_z, 0:dim_z].T + self.SI1_2 = pinv(self.S1_2) + self.K = dot(r_decomp[0:dim_z, dim_z:].T, self.SI1_2) # y = z - Hx # error (residual) between measurement and prediction @@ -217,8 +216,8 @@ def update(self, z, R2=None): # x = x + Ky # predict new x with residual scaled by the kalman gain - self.x += dot(self.K, pinv(N)).dot(self.y) - self._P1_2 = self.S[dim_z:, dim_z:].T + self.x += dot(self.K, self.y) + self._P1_2 = r_decomp[dim_z:, dim_z:].T self.z = deepcopy(z) self.x_post = self.x.copy() @@ -330,6 +329,11 @@ def R(self, value): self._R = value self._R1_2 = cholesky(self._R, lower=True) + @property + def S(self): + """ system uncertainty (P projected to measurement space) """ + return dot(self.S1_2, self.S1_2.T) + def __repr__(self): return '\n'.join([ 'SquareRootKalmanFilter object', From 48ba9d89b6bd7f78f1fc4d249cc08ec2b305a9f7 Mon Sep 17 00:00:00 2001 From: Freja Nordsiek Date: Tue, 29 Jun 2021 14:25:48 +0200 Subject: [PATCH 2/7] Made the test for the square root kalman filter check compare all elements of P, not just the diagonal. --- filterpy/kalman/tests/test_sqrtkf.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/filterpy/kalman/tests/test_sqrtkf.py b/filterpy/kalman/tests/test_sqrtkf.py index 967b2c1d..74b9bfb4 100644 --- a/filterpy/kalman/tests/test_sqrtkf.py +++ b/filterpy/kalman/tests/test_sqrtkf.py @@ -82,7 +82,8 @@ def test_noisy_1d(): s.to_array() for i in range(f.P.shape[0]): - assert abs(f.P[i,i] - fsq.P[i,i]) < 0.01 + for j in range(f.P.shape[1]): + assert abs(f.P[i,j] - fsq.P[i,j]) < 0.01 # now do a batch run with the stored z values so we can test that From 4f4c5ec14706d2646189dbd25bb920f88de7e179 Mon Sep 17 00:00:00 2001 From: Freja Nordsiek Date: Tue, 29 Jun 2021 14:27:32 +0200 Subject: [PATCH 3/7] Fixed bug in the dot product argument order in the square root kalman filter to get P, Q, and R from their square roots (was dot(P1_2.T, P1_2) when it should be dot(P1_2, P1_2.T)). --- filterpy/kalman/square_root.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/filterpy/kalman/square_root.py b/filterpy/kalman/square_root.py index 0a11a039..71b9476c 100644 --- a/filterpy/kalman/square_root.py +++ b/filterpy/kalman/square_root.py @@ -274,7 +274,7 @@ def measurement_of_state(self, x): @property def Q(self): """ Process uncertainty""" - return dot(self._Q1_2.T, self._Q1_2) + return dot(self._Q1_2, self._Q1_2.T) @property def Q1_2(self): @@ -290,17 +290,17 @@ def Q(self, value): @property def P(self): """ covariance matrix""" - return dot(self._P1_2.T, self._P1_2) + return dot(self._P1_2, self._P1_2.T) @property def P_prior(self): """ covariance matrix of the prior""" - return dot(self._P1_2_prior.T, self._P1_2_prior) + return dot(self._P1_2_prior, self._P1_2_prior.T) @property def P_post(self): """ covariance matrix of the posterior""" - return dot(self._P1_2_prior.T, self._P1_2_prior) + return dot(self._P1_2_prior, self._P1_2_prior.T) @property def P1_2(self): @@ -316,7 +316,7 @@ def P(self, value): @property def R(self): """ measurement uncertainty""" - return dot(self._R1_2.T, self._R1_2) + return dot(self._R1_2, self._R1_2.T) @property def R1_2(self): From 7bc0a61131c1a10ae1ac70c7cdd024b494f8ba7f Mon Sep 17 00:00:00 2001 From: Freja Nordsiek Date: Tue, 29 Jun 2021 14:27:56 +0200 Subject: [PATCH 4/7] Added SI to SquareRootKalmanFilter. --- filterpy/kalman/square_root.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/filterpy/kalman/square_root.py b/filterpy/kalman/square_root.py index 71b9476c..d776182a 100644 --- a/filterpy/kalman/square_root.py +++ b/filterpy/kalman/square_root.py @@ -334,6 +334,11 @@ def S(self): """ system uncertainty (P projected to measurement space) """ return dot(self.S1_2, self.S1_2.T) + @property + def SI(self): + """ inverse system uncertainty (P projected to measurement space) """ + return dot(self.SI1_2.T, self.SI1_2) + def __repr__(self): return '\n'.join([ 'SquareRootKalmanFilter object', @@ -349,6 +354,7 @@ def __repr__(self): pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('S', self.S), + pretty_str('SI', self.SI), pretty_str('M', self.M), pretty_str('B', self.B), ]) From 1c220021f5a174cee4b2ad02dc186619ab33ff80 Mon Sep 17 00:00:00 2001 From: Freja Nordsiek Date: Tue, 29 Jun 2021 14:28:20 +0200 Subject: [PATCH 5/7] Made the tests for the square root kalman filter compare S and SI at every time step. --- filterpy/kalman/tests/test_sqrtkf.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/filterpy/kalman/tests/test_sqrtkf.py b/filterpy/kalman/tests/test_sqrtkf.py index 74b9bfb4..608205be 100644 --- a/filterpy/kalman/tests/test_sqrtkf.py +++ b/filterpy/kalman/tests/test_sqrtkf.py @@ -75,6 +75,13 @@ def test_noisy_1d(): assert abs(f.x[0,0] - fsq.x[0,0]) < 1.e-12 assert abs(f.x[1,0] - fsq.x[1,0]) < 1.e-12 + S_from_sqrt = fsq.S + SI_from_sqrt = fsq.SI + for i in range(f.S.shape[0]): + for j in range(f.S.shape[1]): + assert abs(f.S[i,j] - S_from_sqrt[i,j]) < 1e-6 + assert abs(f.SI[i,j] - SI_from_sqrt[i,j]) < 1e-6 + # save data results.append (f.x[0,0]) measurements.append(z) From b50f307caae753418b32f01461832200903320ca Mon Sep 17 00:00:00 2001 From: Freja Nordsiek Date: Tue, 29 Jun 2021 14:37:19 +0200 Subject: [PATCH 6/7] Made the square root kalman filter test use a 2D measurement space (position and velocity) to better check that S and SI are computed correctly. --- filterpy/kalman/tests/test_sqrtkf.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/filterpy/kalman/tests/test_sqrtkf.py b/filterpy/kalman/tests/test_sqrtkf.py index 608205be..4f5e8c35 100644 --- a/filterpy/kalman/tests/test_sqrtkf.py +++ b/filterpy/kalman/tests/test_sqrtkf.py @@ -25,7 +25,7 @@ DO_PLOT = False def test_noisy_1d(): - f = KalmanFilter (dim_x=2, dim_z=1) + f = KalmanFilter (dim_x=2, dim_z=2) f.x = np.array([[2.], [0.]]) # initial state (location and velocity) @@ -33,12 +33,13 @@ def test_noisy_1d(): f.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix - f.H = np.array([[1.,0.]]) # Measurement function + f.H = np.array([[1.,0.], + [0.,1.]]) # Measurement function f.P *= 1000. # covariance matrix f.R *= 5 # state uncertainty f.Q *= 0.0001 # process uncertainty - fsq = SquareRootKalmanFilter (dim_x=2, dim_z=1) + fsq = SquareRootKalmanFilter (dim_x=2, dim_z=2) fsq.x = np.array([[2.], [0.]]) # initial state (location and velocity) @@ -46,7 +47,8 @@ def test_noisy_1d(): fsq.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix - fsq.H = np.array([[1.,0.]]) # Measurement function + fsq.H = np.array([[1.,0.], + [0.,1.]]) # Measurement function fsq.P = np.eye(2) * 1000. # covariance matrix fsq.R *= 5 # state uncertainty fsq.Q *= 0.0001 # process uncertainty @@ -61,8 +63,9 @@ def test_noisy_1d(): zs = [] s = Saver(fsq) for t in range (100): - # create measurement = t plus white noise - z = t + random.randn()*20 + # create measurement = t plus white noise for position and 1 + + # white noise for velocity + z = np.array([[t], [1.]]) + random.randn(2, 1)*20 zs.append(z) # perform kalman filtering From 2f8f22303495172312da6fbcc22191b8a246db67 Mon Sep 17 00:00:00 2001 From: Freja Nordsiek Date: Tue, 29 Jun 2021 14:37:56 +0200 Subject: [PATCH 7/7] Made the comparison of P in the test of the square root kalman filter more strict by reducing the tolerance to 1e-6. --- filterpy/kalman/tests/test_sqrtkf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/filterpy/kalman/tests/test_sqrtkf.py b/filterpy/kalman/tests/test_sqrtkf.py index 4f5e8c35..d29d1694 100644 --- a/filterpy/kalman/tests/test_sqrtkf.py +++ b/filterpy/kalman/tests/test_sqrtkf.py @@ -93,7 +93,7 @@ def test_noisy_1d(): for i in range(f.P.shape[0]): for j in range(f.P.shape[1]): - assert abs(f.P[i,j] - fsq.P[i,j]) < 0.01 + assert abs(f.P[i,j] - fsq.P[i,j]) < 1e-6 # now do a batch run with the stored z values so we can test that