""" 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