kalman.jl

"""
One dimensional Kalman filter for parameter estimates
`kalman(R1,R2,theta, y, A, P)`
"""
function kalman(R1,R2,theta, y, A, P)
    ATP = A'P
    K = (P*A)/(R2+ATP*A)
    P = P - (P*A*ATP)./(R2 + ATP*A) + R1
    yp = (A'theta)[1]
    e = y-yp
    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
    Hk = A[i,:]
    e = y[i]-Hk*xk[:,i]
    S = Hk*Pk[:,:,i]*Hk' + R2
    K = (Pk[:,:,i]*Hk')/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]*Hk')/S
        xkk[:,i] = xk[:,i] + K*e
        Pkk[:,:,i] = (I - K*Hk)*Pk[:,:,i]
    end
    return xkk,xk,Pkk,Pk
end

using Debug

# """A kalman parameter smoother"""
function kalman_smoother(y, A, R1, R2, P0)
    na = size(R1,1)
    N = length(y)

    xkk,xk,Pkk,Pk = forward_kalman(y,A,R1,R2, P0)
    xkn = zeros(xkk)
    Pkn = zeros(Pkk)
    for i = N-1:-1:1
        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

    return xkn
end