Skip to content

Commit

Permalink
Converted V, W to time-dependent functions
Browse files Browse the repository at this point in the history
  • Loading branch information
Gord Stephen committed Oct 21, 2015
1 parent 77579d9 commit aa8b48f
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 42 deletions.
12 changes: 7 additions & 5 deletions src/kalman_filter.jl
View file
Expand Up @@ -24,11 +24,11 @@ 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},
G_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' + model.W # Innovation covariance
S = G_i * P_pred_i * G_i' + W_i # Innovation covariance
K = P_pred_i * G_i' / S # Kalman gain
x_filt_i = x_pred_i + K * innov
P_filt_i = (I - K * G_i) * P_pred_i
Expand All @@ -55,15 +55,17 @@ function kalman_filter{T}(y::Array{T}, model::StateSpaceModel{T}; u::Array{T}=ze
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.x1, model.P1)
model.G(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
P_pred[:, :, i] = F_i1 * P_filt[:, :, i-1] * F_i1' + model.V(i-1)
x_filt[:, i], P_filt[:,:,i], dll = kalman_recursions(y[:, i], u[:, i],
model.G(i), model.D(i), x_pred[:,i], P_pred[:,:,i])
model.G(i), model.D(i), model.W(i),
x_pred[:,i], P_pred[:,:,i])
log_likelihood += dll
end

Expand Down
25 changes: 11 additions & 14 deletions src/kalman_smooth.jl
View file
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, model.W, zeros(model.nu)
Ft, Gt, Dt, Wt, ut = model.F(1), model.G(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 @@ -75,28 +75,28 @@ 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
V_pred_t = Ft * V_pred_t * (Ft - Kt * Gt)' + 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)
Dt = model.D(t)
Wt = model.W(t)
ut = u[:, t]

# Check for and handle missing observation values
missing_obs = !all(y_notnan[:, t])
if missing_obs
ynnt = y_notnan[:, t]
I1, I2 = diagm(ynnt), diagm(!ynnt)
Gt = I1 * model.G(t)
Dt = I1 * model.D(t)
Wt = I1 * model.W * I1 + I2 * model.W * I2
else
Gt = model.G(t)
Dt = model.D(t)
Gt, Dt = I1 * Gt, I1 * Dt
Wt = I1 * Wt * I1 + I2 * Wt * I2
end #if

Ft = model.F(t)
ut = u[:, t]

# Compute nessecary filtering quantities
innov_t = y[:, t] - Gt * x_pred_t - Dt * ut
innov_cov_t = Gt * V_pred_t * Gt' + Wt
Expand All @@ -108,9 +108,6 @@ function kalman_smooth(y::Array, model::StateSpaceModel; u::Array=zeros(size(y,1
K[:, :, t] = Kt
log_likelihood += marginal_likelihood(innov_t, innov_cov_t, innov_cov_inv_t)

# Reset Wt if nessecary
missing_obs && (Wt = model.W)

end #for

# Smoothing
Expand Down
40 changes: 20 additions & 20 deletions src/statespacemodel.jl
View file
Expand Up @@ -3,12 +3,12 @@ immutable StateSpaceModel{T <: Real}
# Process transition matrix, control matrix, and noise covariance
F::Function
B::Function
V::Matrix{T}
V::Function

# Observation matrix, feed-forward matrix, and noise covariance
G::Function
D::Function
W::Matrix{T}
W::Function

# Inital state and error covariance conditions
x1::Vector{T}
Expand All @@ -19,47 +19,47 @@ immutable StateSpaceModel{T <: Real}
ny::Int
nu::Int

function StateSpaceModel(F::Function, B::Function, V::Matrix{T},
G::Function, D::Function, W::Matrix{T}, x1::Vector{T}, P1::Matrix{T})
function StateSpaceModel(F::Function, B::Function, V::Function,
G::Function, D::Function, W::Function, x1::Vector{T}, P1::Matrix{T})

ispossemidef(x::Matrix) = issym(x) && (eigmin(x) >= 0)
@assert ispossemidef(V)
@assert ispossemidef(W)
@assert ispossemidef(V(1))
@assert ispossemidef(W(1))
@assert ispossemidef(P1)

nx, ny, nu = confirm_matrix_sizes(F(1), B(1), V, G(1), D(1), W, x1, P1)
nx, ny, nu = confirm_matrix_sizes(F(1), B(1), V(1), G(1), D(1), W(1), x1, P1)
new(F, B, V, G, D, W, x1, P1, nx, ny, nu)
end
end

# Time-dependent definitions
function StateSpaceModel{T}(F::Function, B::Function, V::Matrix{T},
G::Function, D::Function, W::Matrix{T},
function StateSpaceModel{T}(F::Function, B::Function, V::Function,
G::Function, D::Function, W::Function,
x1::Vector{T}, P1::Matrix{T})
StateSpaceModel{T}(F, B, V, G, D, W, x1, P1)
end

function StateSpaceModel{T}(F::Function, V::Matrix{T},
G::Function, W::Matrix{T},
function StateSpaceModel{T}(F::Function, V::Function,
G::Function, W::Function,
x1::Vector{T}, P1::Matrix{T})
B(_) = zeros(size(V, 1), 1)
D(_) = zeros(size(W, 1), 1)
B(_) = zeros(size(V(1), 1), 1)
D(_) = zeros(size(W(1), 1), 1)
StateSpaceModel{T}(F, B, V, G, D, W, x1, P1)
end

# Time-independent definitions
function StateSpaceModel{T}(F::Matrix{T}, B::Matrix{T}, V::Matrix{T},
G::Matrix{T}, D::Matrix{T}, W::Matrix{T},
x1::Vector{T}, P1::Matrix{T})
StateSpaceModel{T}(_->F, _->B, V, _->G, _->D, W, x1, P1)
StateSpaceModel{T}(_->F, _->B, _->V, _->G, _->D, _->W, x1, P1)
end

function StateSpaceModel{T}(F::Matrix{T}, V::Matrix{T},
G::Matrix{T}, W::Matrix{T},
x1::Vector{T}, P1::Matrix{T})
B(_) = zeros(size(V, 1), 1)
D(_) = zeros(size(W, 1), 1)
StateSpaceModel{T}(_->F, B, V, _->G, D, W, x1, P1)
StateSpaceModel{T}(_->F, B, _->V, _->G, D, _->W, x1, P1)
end

function show{T}(io::IO, mod::StateSpaceModel{T})
Expand Down Expand Up @@ -121,15 +121,15 @@ function simulate(model::StateSpaceModel, n::Int; u::Array=zeros(n, model.nu))

# Cholesky decompositions of the covariance matrices, for generating
# random noise
V_chol = chol(model.V, Val{:L})
W_chol = chol(model.W, Val{:L})
V_chol(t) = chol(model.V(t), Val{:L})
W_chol(t) = chol(model.W(t), Val{:L})

# Generate the series
x[:, 1] = model.x1
y[:, 1] = model.G(1) * model.x1 + model.D(1) * u[:, 1] + W_chol * randn(model.ny)
y[:, 1] = model.G(1) * model.x1 + model.D(1) * u[:, 1] + W_chol(1) * randn(model.ny)
for i=2:n
x[:, i] = model.F(i-1) * x[:, i-1] + model.B(i-1) * u[:, i-1] + V_chol * randn(model.nx)
y[:, i] = model.G(i) * x[:, i] + model.D(i) * u[:, i] + W_chol * randn(model.ny)
x[:, i] = model.F(i-1) * x[:, i-1] + model.B(i-1) * u[:, i-1] + V_chol(i-1) * randn(model.nx)
y[:, i] = model.G(i) * x[:, i] + model.D(i) * u[:, i] + W_chol(1) * randn(model.ny)
end

return x', y'
Expand Down
6 changes: 3 additions & 3 deletions test/kalman_filter.jl
View file
Expand Up @@ -25,11 +25,11 @@ end
# Time varying model
function sinusoid_model(omega::Real; fs::Int=256, x0=[0.5, -0.5], W::AbstractFloat=0.1)
F(n) = [1.0 0; 0 1.0]
V = diagm([1e-10, 1e-10])
V(n) = diagm([1e-10, 1e-10])
G(n) = [cos(2*pi*omega*(1/fs)*n) -sin(2*pi*omega*(1/fs)*n)]
W = diagm([W])
w(n) = diagm([W])
P0 = diagm([1e-1, 1e-1])
StateSpaceModel(F, V, G, W, x0, P0)
StateSpaceModel(F, V, G, w, x0, P0)
end


Expand Down

0 comments on commit aa8b48f

Please sign in to comment.