Skip to content
Snippets Groups Projects
Commit 59d2e3ac authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

implemented kalman parameter smoother

parent f6941403
No related branches found
No related tags found
1 merge request!1Dev
......@@ -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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment