Commit 59d2e3ac by Fredrik Bagge Carlson

### implemented kalman parameter smoother

parent f6941403
 ... ... @@ -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") ... ...
 """ 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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!