updated python pf to fully-adapted for LGSS model

Former-commit-id: b762a17 [formerly b762a17 [formerly 0dbd426]]
Former-commit-id: b4796563e982ee43a6381ba2282ffb458d56df10
Former-commit-id: d227e5c

Author: compops

python/example1-lgss.py

@@ -88,12 +88,11 @@
 # Using the Kalman filter
 xHatFilteredKalmanFilter = kalmanFilter(y, theta, initialState, 0.01)
 
-
 plt.subplot(3, 1, 3)
-plt.plot(xHatFilteredKalmanFilter - xHatFilteredParticleFilter, color='#7570B3', linewidth=1.5)
+plt.plot(xHatFilteredKalmanFilter[1:T] - xHatFilteredParticleFilter[0:T-1], color='#7570B3', linewidth=1.5)
 plt.xlabel("time")
 plt.ylabel("difference in estimate")
-
+plt.show()
 
 ##############################################################################
 # End of file

python/helpers/stateEstimation.py

@@ -133,7 +133,7 @@ def particleFilter(y, theta, noParticles, initialState):
     #=====================================================================
     # Run main loop
     #=====================================================================
-    for t in range(1, T):
+    for t in range(1, T - 1):
 
         #=============================================================
         # Resample particles
@@ -149,14 +149,19 @@ def particleFilter(y, theta, noParticles, initialState):
         #=============================================================
         # Propagate particles
         #=============================================================
-        particles[:, t] = theta[0] * particles[newAncestors, t - 1] + theta[1] * randn(1, noParticles)
+        part1 = (theta[1]**(-2) + theta[2]**(-2))**(-1)
+        part2 = theta[2]**(-2) * y[t]
+        part2 = part2 + theta[1]**(-2) * theta[0] * particles[newAncestors, t - 1]
+        particles[:, t] = part1 * part2 + np.sqrt(part1) * randn(1, noParticles)
 
         #=================================================================
         # Weight particles
         #=================================================================
 
         # Compute log-weights
-        weights[:, t] = norm.logpdf(y[t - 1], particles[:, t], theta[2])
+        yhatMean = theta[0] * particles[:, t]
+        yhatVariance = np.sqrt(theta[1]**2 + theta[2]**2)
+        weights[:, t] = norm.logpdf(y[t + 1], yhatMean, yhatVariance)
 
         # Rescale log-weights and recover weights
         maxWeight = np.max(weights[:, t])