implemented kalman parameter smoother

src/SystemIdentification.jl

@@ -13,7 +13,7 @@ FitResult,
 IdData,
 # Functions
 ar,arx,getARregressor,getARXregressor,find_na,
-toeplitz, kalman
+toeplitz, kalman, kalman_smoother
 
 ## Fit Methods =================
 :LS
@@ -106,6 +106,10 @@ sse(x) = sum(x.^2)
 fit(y,yh) = 100 * (1-rms(y-yh)./rms(y-mean(y)));
 aic(x,d) = log(sse(x)) + 2d/length(x)
 
+##
+Base.show(fit::FitStatistics) = println("Fit RMS:$(fit.RMS), FIT:$(fit.FIT), AIC:$(fit.AIC)")
+
+
 include("armax.jl")
 include("kalman.jl")
 include("PCA.jl")

src/kalman.jl

 """
-One dimensional Kalman filter
+One dimensional Kalman filter for parameter estimates
 `kalman(R1,R2,theta, y, A, P)`
 """
 function kalman(R1,R2,theta, y, A, P)
@@ -11,3 +11,58 @@ function kalman(R1,R2,theta, y, A, P)
     theta = theta + K*e
     return theta, P, e, yp
 end
+
+
+
+function forward_kalman(y,A,R1,R2, P0)
+    na = size(R1,1)
+    N = length(y)
+    xkk = zeros(na,N);
+    Pkk = zeros(na,na,N)
+    xkk[:,1] = A\y;
+    Pkk[:,:,1] = P0;
+    xk = xkk
+    Pk = Pkk
+    i = 1
+    e = y[i]-Hk*xk[:,i]
+    S = Hk*Pk[:,:,i]*Hk' + R2
+    K = (Pk[:,:,i]*H')/S
+    xkk[:,i] = xk[:,i] + K*e
+    Pkk[:,:,i] = (I - K*Hk)*Pk[:,:,i]
+    for i = 2:N
+        Fk = 1
+        Hk = A[i,:]'
+        xk[:,i] = Fk*xkk[:,i-1]
+        Pk[:,:,i] = Fk*Pkk[:,:,i-1]*Fk' + R1
+        e = y[i]-Hk*xk[:,i]
+        S = Hk*Pk[:,:,i]*Hk' + R2
+        K = (Pk[:,:,i]*H')/S
+        xkk[:,i] = xk[:,i] + K*e
+        Pkk[:,:,i] = (I - K*Hk)*Pk[:,:,i]
+    end
+    return xkk,xk,Pkk,Pk
+end
+
+"""A kalman parameter smoother"""
+function kalman_smoother(y, R1, R2)
+    na = size(R1,1)
+    N = length(y)
+
+    P0 = 100*R1;
+    xkk,xk,Pkk,Pk = forward_kalman(y,A,R1,R2, P0)
+    xkn = zeros(xkk)
+    Pkn = zeros(P)
+    for i = N-1:-1:1
+        Ai = A[i,:]
+        Fk = 1
+        Hk = A[i,:]'
+        C = Pkk[:,:,i]/Pk[:,:,i+1]
+        xkn[:,i] = xkk[:,i] + C*(xkn[:,i+1] - xk[:,i+1])
+        Pkn[:,:,i] = Pkk[:,:,i] + C*(Pkn[:,:,i+1] - Pk[:,:,i+1])*C'
+    end
+
+    newplot(xkk'); title("x_{k|k}")
+    newplot(xkn'); title("x_{k|n}")
+    return xkn
+
+end