toy routine for KF in matlab