Continuing implementation of KF algorithm