Skip to content

Commit

Permalink
Merge ab55ec8 into fb88c7d
Browse files Browse the repository at this point in the history
  • Loading branch information
Gord Stephen committed Oct 29, 2015
2 parents fb88c7d + ab55ec8 commit 856ecc2
Show file tree
Hide file tree
Showing 5 changed files with 599 additions and 70 deletions.
24 changes: 12 additions & 12 deletions src/kalman_filter.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ type KalmanFiltered{T}
loglik::T
end

function show{T}(io::IO, filt::KalmanFiltered{T})
function Base.show{T}(io::IO, filt::KalmanFiltered{T})
n = size(filt.y, 1)
dx, dy = filt.model.nx, filt.model.ny
println("KalmanFiltered{$T}")
Expand All @@ -24,14 +24,14 @@ function kalman_filter{T}(y::Array{T}, model::StateSpaceModel{T}; u::Array{T}=ze
@assert size(u,2) == model.nu

function kalman_recursions(y_i::Vector{T}, u_i::Vector{T},
G_i::Matrix{T}, D_i::Matrix{T}, W_i::Matrix{T},
C_i::Matrix{T}, D_i::Matrix{T}, W_i::Matrix{T},
x_pred_i::Vector{T}, P_pred_i::Matrix{T})
if !any(isnan(y_i))
innov = y_i - G_i * x_pred_i - D_i * u_i
S = G_i * P_pred_i * G_i' + W_i # Innovation covariance
K = P_pred_i * G_i' / S # Kalman gain
innov = y_i - C_i * x_pred_i - D_i * u_i
S = C_i * P_pred_i * C_i' + W_i # Innovation covariance
K = P_pred_i * C_i' / S # Kalman gain
x_filt_i = x_pred_i + K * innov
P_filt_i = (I - K * G_i) * P_pred_i
P_filt_i = (I - K * C_i) * P_pred_i
dll = (dot(innov,S\innov) + logdet(S))/2
else
x_filt_i = x_pred_i
Expand All @@ -51,20 +51,20 @@ function kalman_filter{T}(y::Array{T}, model::StateSpaceModel{T}; u::Array{T}=ze
log_likelihood = n*model.ny*log(2pi)/2

# first iteration
F_1 = model.F(1)
A_1 = model.A(1)
x_pred[:, 1] = model.x1
P_pred[:, :, 1] = model.P1
x_filt[:, 1], P_filt[:,:,1], dll = kalman_recursions(y[:, 1], u[:, 1],
model.G(1), model.D(1), model.W(1),
model.C(1), model.D(1), model.W(1),
model.x1, model.P1)
log_likelihood += dll

for i=2:n
F_i1 = model.F(i)
x_pred[:, i] = F_i1 * x_filt[:, i-1] + model.B(i-1) * u[:, i-1]
P_pred[:, :, i] = F_i1 * P_filt[:, :, i-1] * F_i1' + model.V(i-1)
A_i1 = model.A(i-1)
x_pred[:, i] = A_i1 * x_filt[:, i-1] + model.B(i-1) * u[:, i-1]
P_pred[:, :, i] = A_i1 * P_filt[:, :, i-1] * A_i1' + model.V(i-1)
x_filt[:, i], P_filt[:,:,i], dll = kalman_recursions(y[:, i], u[:, i],
model.G(i), model.D(i), model.W(i),
model.C(i), model.D(i), model.W(i),
x_pred[:,i], P_pred[:,:,i])
log_likelihood += dll
end
Expand Down
48 changes: 34 additions & 14 deletions src/kalman_smooth.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ function kalman_smooth(y::Array, model::StateSpaceModel; u::Array=zeros(size(y,1
innov_cov_inv_t = copy(innov_cov_t)
Kt = zeros(model.nx, model.ny)

Ft, Gt, Dt, Wt, ut = model.F(1), model.G(1), model.D(1), model.W(1), zeros(model.nu)
At, Ct, Dt, Wt, ut = model.A(1), model.C(1), model.D(1), model.W(1), zeros(model.nu)

log_likelihood = n*model.ny*log(2pi)/2
marginal_likelihood(innov::Vector, innov_cov::Matrix, innov_cov_inv::Matrix) =
Expand All @@ -74,16 +74,16 @@ function kalman_smooth(y::Array, model::StateSpaceModel; u::Array=zeros(size(y,1

# Predict using last iteration's values
if t > 1
x_pred_t = Ft * x_pred_t + model.B(t-1) * ut + Kt * innov_t
V_pred_t = Ft * V_pred_t * (Ft - Kt * Gt)' + model.V(t)
x_pred_t = At * x_pred_t + model.B(t-1) * ut + Kt * innov_t
V_pred_t = At * V_pred_t * (At - Kt * Ct)' + model.V(t)
V_pred_t = (V_pred_t + V_pred_t')/2
end #if
x_pred[:, t] = x_pred_t
V_pred[:, :, t] = V_pred_t

# Assign new values
Ft = model.F(t)
Gt = model.G(t)
At = model.A(t)
Ct = model.C(t)
Dt = model.D(t)
Wt = model.W(t)
ut = u[:, t]
Expand All @@ -93,15 +93,15 @@ function kalman_smooth(y::Array, model::StateSpaceModel; u::Array=zeros(size(y,1
if missing_obs
ynnt = y_notnan[:, t]
I1, I2 = diagm(ynnt), diagm(!ynnt)
Gt, Dt = I1 * Gt, I1 * Dt
Ct, Dt = I1 * Ct, I1 * Dt
Wt = I1 * Wt * I1 + I2 * Wt * I2
end #if

# Compute nessecary filtering quantities
innov_t = y[:, t] - Gt * x_pred_t - Dt * ut
innov_cov_t = Gt * V_pred_t * Gt' + Wt
innov_t = y[:, t] - Ct * x_pred_t - Dt * ut
innov_cov_t = Ct * V_pred_t * Ct' + Wt
innov_cov_inv_t = inv(innov_cov_t)
Kt = Ft * V_pred_t * Gt' * innov_cov_inv_t
Kt = At * V_pred_t * Ct' * innov_cov_inv_t

innov[:, t] = innov_t
innov_cov_inv[:, :, t] = innov_cov_inv_t
Expand All @@ -115,13 +115,13 @@ function kalman_smooth(y::Array, model::StateSpaceModel; u::Array=zeros(size(y,1

for t = n:-1:1

Gt = model.G(t)
Ct = model.C(t)
innov_cov_inv_t = innov_cov_inv[:, :, t]
V_pred_t = V_pred[:, :, t]
L = model.F(t) - K[:, :, t] * Gt
L = model.A(t) - K[:, :, t] * Ct

r = Gt' * innov_cov_inv_t * innov[:, t] + L' * r
N = Gt' * innov_cov_inv_t * Gt + L' * N * L
r = Ct' * innov_cov_inv_t * innov[:, t] + L' * r
N = Ct' * innov_cov_inv_t * Ct + L' * N * L

x_smooth[:, t] = x_pred[:, t] + V_pred_t * r
V_smooth_t = V_pred_t - V_pred_t * N * V_pred_t
Expand All @@ -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)
Expand All @@ -149,7 +169,7 @@ function kalman_smooth(filt::KalmanFiltered)
x_smooth[:, n] = x_filt[:, n]
P_smoov[:, :, n] = P_filt[:, :, n]
for i = (n-1):-1:1
J = P_filt[:, :, i] * model.F(i)' * inv(P_pred[:,:,i+1])
J = P_filt[:, :, i] * model.A(i)' * inv(P_pred[:,:,i+1])
x_smooth[:, i] = x_filt[:, i] + J * (x_smooth[:, i+1] - x_pred[:, i+1])
P_smoov[:, :, i] = P_filt[:, :, i] + J * (P_smoov[:, :, i+1] - P_pred[:,:,i+1]) * J'
end
Expand Down
Loading

0 comments on commit 856ecc2

Please sign in to comment.