First stage of EM estimation implementation

src/kalman_smooth.jl

@@ -133,6 +133,26 @@ function kalman_smooth(y::Array, model::StateSpaceModel; u::Array=zeros(size(y,1
 
 end #smooth
 
+# Bad performance - will be much better with sparse matrices
+function lag1_smooth(y::Array, u::Array, m::StateSpaceModel)
+
+    A_stack(t) = [m.A(t) zeros(m.nx, m.nx); eye(m.nx) zeros(m.nx, m.nx)]
+    B_stack(t) = [m.B(t); zeros(m.nx, m.nu)]
+    V_stack(t) = [m.V(t) zeros(m.nx, m.nx); zeros(m.nx, 2*m.nx)]
+    C_stack(t) = [m.C(t) zeros(m.ny, m.nx)]
+    x1_stack   = [m.x1; zeros(model.x1)]
+    P1_stack   = [m.P1 zeros(m.nx, m.nx); zeros(m.nx, 2*m.nx)]
+    stack_model = StateSpaceModel(A_stack, B_stack, V_stack,
+                                  C_stack, model.D, model.W, x1_stack, V1_stack)
+
+    stack_smoothed = smooth(y, stack_model, u=u)
+    x     = stack_smoothed.x[:, 1:model.nx]
+    V     = stack_smoothed.V[1:model.nx, 1:model.nx, :]
+    Vlag1 = stack_smoothed.V[1:model.nx, (model.nx+1):end]
+    return x, V, Vlag1, stack_smoothed.loglik
+
+end #function
+
 
 # Rauch-Tung-Striebel Smoothing
 function kalman_smooth(filt::KalmanFiltered)