Add blockAssign
remove debug
push matlab updates
And slam.m
Push debug fixes
Merge branch 'master' into martin