Forward-backward IIR filter that uses Gustafsson's method.

filtfilt_gustafsson.jl

# Forward-backward IIR filter that uses Gustafsson's method.
#
# Apply the IIR filter defined by `(b,a)` to `x` twice, first forward
# then backward, using Gustafsson's initial conditions [1]_.
#
# Let `y_fb` be the result of filtering first forward and then backward,
# and let `y_bf` be the result of filtering first backward then forward.
# Gustafsson's method is to compute initial conditions for the forward
# pass and the backward pass such that `y_fb == y_bf`.
#
# Based upon [1] and the proposed SciPy implemenation [2]. Not thoroughly
# tested or verified. Requires an implementation of `filt` that allows
# setting initial conditions [3].
# 
# References
# ----------
# .. [1] F. Gustaffson. Determining the initial states in forward-backward
#        filtering. Transactions on Signal Processing, 46(4):988-992, 1996.
# .. [2] https://github.com/scipy/scipy/pull/3442
# .. [3] https://github.com/JuliaLang/julia/pull/7513

function filtfilt{T}(b::Vector{T}, a::Vector{T}, x::Vector{T}, irlen=div(size(x,1),2))

    order = max(length(b), length(a)) - 1
    if order == 0
        # The filter is just scalar multiplication, with no state.
        scale = (b[1] / a[1])^2
        return scale * x
    end

    # n is the number of samples in the data to be filtered.
    n = size(x,1)

    # m is the number of samples in which to consider edge effects
    n < 2*irlen && throw(ArgumentError("irlen too long"))
    m = irlen

    # Create 𝒪, the observability matrix
    # This matrix can be interpreted as the operator that propagates
    # an arbitrary initial state to the output, assuming the input is
    # zero.
    # In Gustafsson's paper, the forward and backward filters are not
    # necessarily the same, so he has both 𝒪_f and 𝒪_b.  We use the same
    # filter in both directions, so we only need 𝒪. The same comment
    # applies to S below.
    𝒪 = zeros(T, (m, order))
    zi = zeros(T, order)
    zi[1] = 1
    𝒪[:, 1] = filt(b, a, zeros(T, m), zi)
    for k in 2:order
        𝒪[k:end, k] = 𝒪[1:end+1-k, 1]
    end

    # 𝒪ᴿ is Gustafsson's notation for row-reversed 𝒪
    𝒪ᴿ = flipud(𝒪)

    # Create S.  S is the matrix that applies the filter to the reversed
    # propagated initial conditions.  That is,
    #     out = S.dot(zi)
    # is the same as
    #     tmp, _ = lfilter(b, a, zeros(), zi=zi)  # Propagate ICs.
    #     out = lfilter(b, a, tmp[::-1])          # Reverse and filter.

    # Equations (5) & (6) of [1]
    S = hcat([filt(b, a, 𝒪ᴿ[:,c]) for c = 1:size(𝒪ᴿ,2)]...)
    Sᴿ = flipud(S)

    # M is [(Sᴿ - O), (Oᴿ - S)]
    if m == n
        M = hcat(Sᴿ - 𝒪, 𝒪ᴿ - S)
    else
        # Matrix described in section IV of [1].
        # TODO: use blkdiag once implemented for dense matrices
        M = zeros(T, 2*m, 2*order)
        M[1:m, 1:order] = Sᴿ - 𝒪
        M[m+1:end, order+1:end] = 𝒪ᴿ - S
    end
    # Naive forward-backward and backward-forward filters.
    # These have large transients because the filters use zero initial
    # conditions.
    y_f = filt(b, a, x)
    y_fb = reverse!(filt(b, a, reverse(y_f)))

    y_b = reverse!(filt(b, a, reverse(x)))
    y_bf = filt(b, a, y_b)

    delta_y_bf_fb = y_bf - y_fb
    if m == n
        delta = delta_y_bf_fb
    else
        start_m = delta_y_bf_fb[1:m,:]
        end_m = delta_y_bf_fb[end-m+1:end,:]
        delta = vcat(start_m, end_m)
    end
    # ic_opt holds the "optimal" initial conditions.
    # The following code computes the result shown in the formula
    # of the paper between equations (6) and (7).
    ic_opt = M \ delta

    # Now compute the filtered signal using equation (7) of [1].
    # First, form [Sᴿ, 𝒪ᴿ] and call it W.
    if m == n
        W = hcat(Sᴿ, 𝒪ᴿ)
    else
        W = zeros(T, 2*m, 2*order)
        W[1:m, 1:order] = Sᴿ
        W[m+1:end, order+1:end] = 𝒪ᴿ
    end
    # Equation (7) of [1] says
    #     Y_fb^opt = Y_fb^0 + W * [x_0^opt; x_{N-1}^opt]
    # `wic` is (almost) the product on the right.
    if m == n
        y_opt = y_fb + W * ic_opt
    else
        wic = W * ic_opt
        y_opt = y_fb
        y_opt[1:m,:] += wic[1:m,:]
        y_opt[end-m+1:end] += wic[end-m+1:end]
    end

    return y_opt
end