matlab version of KF routines