Initialize variables: ,
for to do
Predict
   Compute Jacobian
   Predict state
   Predict covariance
Update
   Compute Jacobian
   Residual
   Residual covariance
   Kalman gain
   Update state
   Update covariance
end for

Algorithm 1: Extended Kalman filter.