the rts_smoother should use k+1 as the index for Fs and Qs.

pavanbathla · Jun 20, 2016 · bd8e5a7 · bd8e5a7
1 parent c9c488a
commit bd8e5a7
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 4 deletions.
diff --git a/filterpy/changelog.txt b/filterpy/changelog.txt
@@ -1,3 +1,17 @@
+Version 0.1.3
+=============
+
+* Github issue #37. rts_smoother uses the wrong index for F and Q: they should
+use k+1, not k. This caused poor smoothing performance when either F or Q are
+time varying.
+
+
+* Github issue #40. Fixed behavior of multivariate_gaussian to accept list as
+the covariance matrix. 
+
+
+
+
 Version 0.1.2
 =============
 

diff --git a/filterpy/kalman/kalman_filter.py b/filterpy/kalman/kalman_filter.py
@@ -623,11 +623,11 @@ def rts_smoother(self, Xs, Ps, Fs=None, Qs=None):
         x, P = Xs.copy(), Ps.copy()
 
         for k in range(n-2,-1,-1):
-            P_pred = dot3(Fs[k], P[k], Fs[k].T) + Qs[k]
+            P_pred = dot3(Fs[k+1], P[k], Fs[k+1].T) + Qs[k+1]
 
-            K[k]  = dot3(P[k], Fs[k].T, linalg.inv(P_pred))
-            x[k] += dot (K[k], x[k+1] - dot(Fs[k], x[k]))
-            P[k] += dot3 (K[k], P[k+1] - P_pred, K[k].T)
+            K[k]  = dot3(P[k], Fs[k+1].T, linalg.inv(P_pred))
+            x[k] += dot(K[k], x[k+1] - dot(Fs[k+1], x[k]))
+            P[k] += dot3(K[k], P[k+1] - P_pred, K[k].T)
 
         return (x, P, K)