matlab code:
floor estimation added