diff --git a/filterpy/kalman/square_root.py b/filterpy/kalman/square_root.py index 6dd445c7..d776182a 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() @@ -275,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): @@ -291,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): @@ -317,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): @@ -330,6 +329,16 @@ 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) + + @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', @@ -345,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), ]) diff --git a/filterpy/kalman/tests/test_sqrtkf.py b/filterpy/kalman/tests/test_sqrtkf.py index 967b2c1d..d29d1694 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 @@ -75,6 +78,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) @@ -82,7 +92,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]) < 1e-6 # now do a batch run with the stored z values so we can test that