Commit 59d2e3ac authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment