R/kalman.R

In vsartor/bnsa: Bayesian Non-Structured Analysis

#' Kalman Filtering and Smoothing
#'
#' Implements a basic Kalman filter and smoother for dynamic linear models,
#' considering vague priors.
#'
#' @param Y A T by m matrix containing T observations of an m-variate series.
#' @param Ft The observational matrix.
#' @param Gt The evolutional matrix.
#' @param Vt The observational covariance matrix.
#' @param Wt The evolutional covariance matrix.
#'
#' @return A list with posterior means `s` and covariances `S`.
#'
#' @export
#'
kalmanfs = function(Y, Ft, Gt, Vt, Wt) {
    # Problem dimensions
    N = nrow(Y)
    p = nrow(Gt)

    # Hyper-parameters
    m0 = rep(0, p)
    C0 = diag(1e4, p)

    # Initialize variables
    a = m = s = array(dim = c(p, N))
    R = C = S = array(dim = c(p, p, N))

    # First-step filtering
    a[ ,1]   = Gt %*% m0
    R[ , ,1] = Gt %*% C0 %*% t(Gt) + Wt
    f        = Ft %*% a[ ,1]
    Q        = Ft %*% R[ , ,1] %*% t(Ft) + Vt
    A        = R[ , ,1] %*% t(Ft) %*% chol2inv(chol(Q))
    m[ ,1]   = a[ ,1] + A %*% (Y[1, ] - f)
    C[ , ,1] = R[ , ,1] - A %*% Q %*% t(A)

    # Other filtering steps
    for (i in 2:N) {
        a[ ,i]   = Gt %*% m[ ,i-1]
        R[ , ,i] = Gt %*% C[ , ,i-1] %*% t(Gt) + Wt
        f        = Ft %*% a[ ,i]
        Q        = Ft %*% R[ , ,i] %*% t(Ft) + Vt
        A        = R[ , ,i] %*% t(Ft) %*% chol2inv(chol(Q))
        m[ ,i]   = a[ ,i] + A %*% (Y[i, ] - f)
        C[ , ,i] = R[ , ,i] - A %*% Q %*% t(A)
    }

    # First-step smoothing
    s[ ,N]   = m[ ,N]
    S[ , ,N] = C[ , ,N]

    # Other smoothing steps
    for (i in (N-1):1) {
        B        = C[ , ,i] %*% t(Gt) %*% chol2inv(chol(R[ , ,i+1]))
        s[ ,i]   = m[ ,i] + B %*% (s[ ,i+1] - a[ ,i+1])
        S[ , ,i] = C[ , ,i] - B %*% (R[ , ,i+1] - S[ , ,i+1]) %*% t(B)
    }

    # Return smoothed chains
    list(s = s, S = S)
}