kalman.jl 1.5 KB
 Fredrik Bagge Carlson committed Sep 08, 2015 1 ``````""" `````` Fredrik Bagge Carlson committed Sep 09, 2015 2 ``````One dimensional Kalman filter for parameter estimates `````` Fredrik Bagge Carlson committed Sep 08, 2015 3 4 ```````kalman(R1,R2,theta, y, A, P)` """ `````` Fredrik Bagge Carlson committed Sep 04, 2015 5 ``````function kalman(R1,R2,theta, y, A, P) `````` Fredrik Bagge Carlson committed Sep 08, 2015 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 committed Sep 04, 2015 13 ``````end `````` Fredrik Bagge Carlson committed Sep 09, 2015 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 `````` 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 e = y[i]-Hk*xk[:,i] S = Hk*Pk[:,:,i]*Hk' + R2 K = (Pk[:,:,i]*H')/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]*H')/S xkk[:,i] = xk[:,i] + K*e Pkk[:,:,i] = (I - K*Hk)*Pk[:,:,i] end return xkk,xk,Pkk,Pk end """A kalman parameter smoother""" function kalman_smoother(y, R1, R2) na = size(R1,1) N = length(y) P0 = 100*R1; xkk,xk,Pkk,Pk = forward_kalman(y,A,R1,R2, P0) xkn = zeros(xkk) Pkn = zeros(P) for i = N-1:-1:1 Ai = A[i,:] 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 newplot(xkk'); title("x_{k|k}") newplot(xkn'); title("x_{k|n}") return xkn end``````