kalman.jl 1.44 KB
Newer Older
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
1
"""
2
One dimensional Kalman filter for parameter estimates
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
3
4
`kalman(R1,R2,theta, y, A, P)`
"""
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
5
function kalman(R1,R2,theta, y, A, P)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
6
7
8
9
10
11
12
    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
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
13
end
14
15
16
17
18
19
20
21
22
23
24
25
26



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
27
    Hk = A[i,:]
28
29
    e = y[i]-Hk*xk[:,i]
    S = Hk*Pk[:,:,i]*Hk' + R2
30
    K = (Pk[:,:,i]*Hk')/S
31
32
33
34
    xkk[:,i] = xk[:,i] + K*e
    Pkk[:,:,i] = (I - K*Hk)*Pk[:,:,i]
    for i = 2:N
        Fk = 1
35
        Hk = A[i,:]
36
37
38
39
        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
40
        K = (Pk[:,:,i]*Hk')/S
41
42
43
44
45
46
        xkk[:,i] = xk[:,i] + K*e
        Pkk[:,:,i] = (I - K*Hk)*Pk[:,:,i]
    end
    return xkk,xk,Pkk,Pk
end

47
48
49
using Debug

# """A kalman parameter smoother"""
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
50
function kalman_smoother(y, A, R1, R2, P0)
51
52
53
54
55
    na = size(R1,1)
    N = length(y)

    xkk,xk,Pkk,Pk = forward_kalman(y,A,R1,R2, P0)
    xkn = zeros(xkk)
56
    Pkn = zeros(Pkk)
57
58
59
60
61
62
63
64
    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

65
    return xkn
66
end