diff --git a/sm/csm/icp/icp.c b/sm/csm/icp/icp.c
index a96838ca90973ae0e9e7c1d2abaef55f3d71d653..4472ea2792afd6bd7c0b7d71f7d807c3ef864737 100644
--- a/sm/csm/icp/icp.c
+++ b/sm/csm/icp/icp.c
@@ -175,10 +175,11 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 		res->iterations = iterations;
 		res->nvalid = nvalid;
 
-		gsl_vector_free(x_new);
-		gsl_vector_free(x_old);
 		gsl_vector_free(best_x);
 	}
+	gsl_vector_free(x_new);
+	gsl_vector_free(x_old);
+
 
 	egsl_pop_named("sm_icp");
 
diff --git a/sm/lib/egsl/egsl.c b/sm/lib/egsl/egsl.c
index bfecfdc55fddf8fa73bd30b708b6443290856ccc..c4943b49cd2896528c27d45d8bd6d5c921420d16 100644
--- a/sm/lib/egsl/egsl.c
+++ b/sm/lib/egsl/egsl.c
@@ -190,12 +190,33 @@ val egsl_alloc(size_t rows, size_t columns) {
 }
 
 val egsl_alloc_in_context(int context, size_t rows, size_t columns) {
-	egsl_total_allocations++;
-	struct egsl_context *c = egsl_contexts+context;
+	struct egsl_context*c = egsl_contexts+context;
+	
+	if(c->nvars>=MAX_VALS) {
+		fprintf(stderr,"Limit reached, in context %d, nvars is %d\n",context,c->nvars);
+		egsl_error();
+	}
 	int index = c->nvars;
-	c->vars[index].gsl_m = gsl_matrix_alloc((size_t)rows,(size_t)columns);
-	c->nvars++;
-	return assemble_val(context,index,c->vars[index].gsl_m);
+	if(index<c->nallocated) {
+		gsl_matrix*m = c->vars[index].gsl_m;
+		if(m->size1 == rows && m->size2 == columns) {
+			egsl_cache_hits++;
+			c->nvars++;
+			return assemble_val(context,index,c->vars[index].gsl_m);
+		} else {
+			gsl_matrix_free(m);
+			egsl_total_allocations++;			
+			c->vars[index].gsl_m = gsl_matrix_alloc((size_t)rows,(size_t)columns);
+			c->nvars++;
+			return assemble_val(context,index,c->vars[index].gsl_m);
+		}
+	} else {
+		egsl_total_allocations++;
+		c->vars[index].gsl_m = gsl_matrix_alloc((size_t)rows,(size_t)columns);
+		c->nvars++;
+		c->nallocated++;
+		return assemble_val(context,index,c->vars[index].gsl_m);
+	}
 }
 
 /** Creates a copy of v in the previous context. */
@@ -278,5 +299,13 @@ double egsl_atm(val v1, size_t i, size_t j){
 	return *egsl_atmp(v1, i, j);
 }
 
-
+void egsl_free(void){
+	int c;
+	for(c=0;c<=max_cid;c++) {
+		for(int i=egsl_contexts[c].nvars; i<egsl_contexts[c].nallocated; i++){
+			gsl_matrix_free(egsl_contexts[c].vars[i].gsl_m);
+		}
+	egsl_contexts[c].nallocated = egsl_contexts[c].nvars;
+	} 
+}
 
diff --git a/sm/lib/egsl/egsl_conversions.c b/sm/lib/egsl/egsl_conversions.c
index 580a89ab0b3b0e5fa2bce68482ad4879b41d71a6..2d81c09df2f0fc2f1c9c7a0e9049747945150351 100644
--- a/sm/lib/egsl/egsl_conversions.c
+++ b/sm/lib/egsl/egsl_conversions.c
@@ -60,5 +60,5 @@ gsl_matrix* egsl_v2gslm(val v){
 	gsl_matrix * m = egsl_gslm(v); 
 	gsl_matrix * m2 = gsl_matrix_alloc(m->size1,m->size2);
 	gsl_matrix_memcpy(m2,m);
-	return m;
+	return m2;
 }