kalman.jl 277 Bytes
Newer Older
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
1
2
3
4
"""
One dimensional Kalman filter
`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