Matlab Script for Multidimensional Kalman Filter