Keep active set between iterations for SdqpLmpcConstant

olejodd10 · Apr 11, 2024 · 9f8dfbd · 9f8dfbd
1 parent 20276a7
commit 9f8dfbd
Showing 1 changed file with 8 additions and 2 deletions.
diff --git a/src/SdqpLmpcConstant.c b/src/SdqpLmpcConstant.c
@@ -46,6 +46,7 @@ static real_t *m_y;
 static real_t *m_m5;
 static real_t *m_temp1; // Use x instead? Has length n_M and x is unused in initialize_y
 static real_t *m_temp2;
+static real_t *m_temp3;
 
 // in-place version, v = (I-Ahat)^-1 * v
 static void multiply_inv_eye_sub_Ahat_inplace(size_t n_x, size_t N, const real_t A[n_x][n_x], real_t v[N][n_x]) {
@@ -435,6 +436,7 @@ void sdqp_lmpc_constant_init(
     m_m5 = (real_t*)malloc(sizeof(real_t)*m_n_H);
     m_temp1 = (real_t*)malloc(sizeof(real_t)*m_n_M);
     m_temp2 = (real_t*)malloc(sizeof(real_t)*m_n_M);
+    m_temp3 = (real_t*)malloc(sizeof(real_t)*m_n_H);
 
     ramp_init(m_n_H, get_column_M4);
     ramp_enable_infeasibility_error(1e-12, 1e12);
@@ -456,18 +458,22 @@ void sdqp_lmpc_constant_cleanup(void) {
 	free(m_m5);
 	free(m_temp1);
 	free(m_temp2);
+	free(m_temp3);
 
     ramp_cleanup();
 }
 
 int sdqp_lmpc_constant_solve(size_t n_x, size_t n_u, size_t N, const real_t x0[n_x], real_t x[N][n_x], real_t u[N][n_u]) {
     // initialize y
-    iterable_set_clear(&m_a_set);
-    indexed_vectors_clear(&m_invq);
     initialize_y(n_x, n_u, m_n_y, m_n_t, N, m_n_H, 
 		CAST_CONST_2D_VLA(m_Q, n_x), CAST_CONST_2D_VLA(m_S, n_x), CAST_CONST_2D_VLA(m_R, n_u),
 		CAST_CONST_2D_VLA(m_A, n_x), CAST_CONST_2D_VLA(m_B, n_u), CAST_CONST_2D_VLA(m_C, n_x),
 		CAST_CONST_2D_VLA(m_Lt, n_x), x0, m_y);
+    memcpy(m_temp3, m_y, sizeof(real_t)*m_n_H);
+    for (size_t i = iterable_set_first(&m_a_set); i != iterable_set_end(&m_a_set); i = iterable_set_next(&m_a_set, i)) {
+        linalg_vector_add_scaled(m_n_H, m_y, indexed_vectors_get(&m_invq, i), m_temp3[i], m_y);
+        m_y[i] -= m_temp3[i];
+    }
     int err = ramp_solve(m_n_H, m_n_a, &m_a_set, &m_invq, m_y);
     if (err) {
         return err;