Fixing square root kalman (K, S, and off-diagonal elements of P)

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,18 +205,19 @@ 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
         self.y = z - dot(self.H, self.x)
 
         # 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),
             ])

filterpy/kalman/tests/test_sqrtkf.py

@@ -25,28 +25,30 @@
 
 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)
 
     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)
 
     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,14 +78,22 @@ 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)
         s.save()
     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