--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pylearn/algorithms/sandbox/kalman.py	Thu Nov 20 12:17:56 2008 -0500
@@ -0,0 +1,57 @@
+
+"""
+Modules and misc. code related to the Kalman Filter.
+
+
+Kalman filter algorithm as presented in "Probabilistic Robotics"
+
+x_t is the state
+
+u_t is a control vector
+
+z_t is the observation vector
+
+\epsilon_t is a random noise term with zero mean and covariance R_t.
+
+\delta_t is a random noise term with zero mean and covariance Q_t.
+
+state (x_t) evolves according to 
+
+    x_t = A_t x_{t-1} + B_t u_t + \epsilon_t
+
+Observation z_t is made according to
+    
+    z_t = C_t x_t + \delta_t
+
+Assume that the distribution over initial states is a Gaussian.
+
+With these linear/Gaussian assumptions, the belief about the state all times t is Gaussian, so
+we can represent it compactly by the mean (mu) and the covariance (sigma).
+
+"""
+
+class KalmanModule(Module):
+    """
+    """
+    def __init__(self):
+
+        self.mu = Member()
+        self.sigma = Member()
+
+        u, z = vector(), vector()
+
+        # the formulas here work for A, B, R, C matrix or sparse matrix.
+        # ... anything that supports dot, +, -, dotinv, and transpose.
+
+        A, B, C= matrix(), matrix(), matrix()
+        R, Q = matrix(), matrix()
+
+        #algo from Probabilistic Robotics pg. 42
+        mu_bar = dot(A, self.mu) + dot(B, u)
+        sigma_bar = dot(A, self.sigma, A.T) + R
+        K = dot(sigma_bar, C.T, dotinv(dot(C, sigma_bar, C.T) + Q))
+        mu_t = mu_bar + dot(K, z - dot(C,mu_bar))
+        sigma_t = dot(ident - dot(K,C), sigma_bar)
+
+        self.update = Method([u, z, A, B, C, R, Q], [], updates = {self.mu:mu_t, self.sigma:sigma_t})
+