Matlab test code for EKF thermalling