cf_math: remove matrixcopy/vectorcopy in favor of memcpy()

* memcpy() should be faster than an elementwise copy * memcpy() is easier to read in the code, as the definition is well known
bitcraze · Oct 12, 2021 · 40e01b7 · 40e01b7
1 parent b4c9693
commit 40e01b7
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 28 deletions.
diff --git a/src/modules/src/kalman_core/kalman_core.c b/src/modules/src/kalman_core/kalman_core.c
@@ -291,7 +291,7 @@ void kalmanCoreUpdateWithPKE(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm
     float Ppo[KC_STATE_DIM][KC_STATE_DIM]={0};
     arm_matrix_instance_f32 Ppom = {KC_STATE_DIM, KC_STATE_DIM, (float *)Ppo};
     mat_mult(&tmpNN1m, P_w_m, &Ppom);          // Pm = (I-KH)*P_w_m
-    matrixcopy(KC_STATE_DIM, KC_STATE_DIM, this->P, Ppo);
+    memcpy(this->P, Ppo, sizeof(this->P));
 
     assertStateNotNaN(this);
 

diff --git a/src/modules/src/kalman_core/mm_distance_robust.c b/src/modules/src/kalman_core/mm_distance_robust.c
@@ -117,10 +117,10 @@ void kalmanCoreRobustUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurem
     static arm_matrix_instance_f32 x_errm = {KC_STATE_DIM, 1, x_err};
     static float X_state[KC_STATE_DIM] = {0.0};
     float P_iter[KC_STATE_DIM][KC_STATE_DIM];
-    matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter,this->P);   // init P_iter as P_prior
+    memcpy(P_iter, this->P, sizeof(P_iter));
 
     float R_iter = d->stdDev * d->stdDev;                     // measurement covariance
-    vectorcopy(KC_STATE_DIM, X_state, this->S);               // copy Xpr to X_State and then update in each iterations
+    memcpy(X_state, this->S, sizeof(X_state));
 
     // ---------------------- Start iteration ----------------------- //
     for (int iter = 0; iter < MAX_ITER; iter++){
@@ -179,7 +179,7 @@ void kalmanCoreRobustUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurem
             P_chol[k][k] = P_chol[k][k] + dummy_value;
         }
         // keep P_chol
-        matrixcopy(KC_STATE_DIM, KC_STATE_DIM, tmp1, P_chol);
+        memcpy(tmp1, P_chol, sizeof(tmp1));
         mat_inv(&tmp1m, &Pc_inv_m);                            // Pc_inv_m = inv(Pc_m) = inv(P_chol)
         mat_mult(&Pc_inv_m, &x_errm, &e_x_m);                  // e_x_m = Pc_inv_m.dot(x_errm) 
 
@@ -221,7 +221,7 @@ void kalmanCoreRobustUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurem
             X_state[i] = this->S[i] + x_err[i];       // convert to nominal state
         }
         // update P_iter matrix and R matrix for next iteration
-        matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter, P_w);
+        memcpy(P_iter, P_w, sizeof(P_iter));
         R_iter = R_w;
     }
 

diff --git a/src/modules/src/kalman_core/mm_tdoa_robust.c b/src/modules/src/kalman_core/mm_tdoa_robust.c
@@ -132,10 +132,10 @@ void kalmanCoreRobustUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *t
         static arm_matrix_instance_f32 x_errm = {KC_STATE_DIM, 1, x_err};
         static float X_state[KC_STATE_DIM] = {0.0};
         float P_iter[KC_STATE_DIM][KC_STATE_DIM];
-        matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter,this->P);        // init P_iter as P_prior
-        
+        memcpy(P_iter, this->P, sizeof(P_iter));                 // init P_iter as P_prior
+
         float R_iter = tdoa->stdDev * tdoa->stdDev;                    // measurement covariance
-        vectorcopy(KC_STATE_DIM, X_state, this->S);                    // copy Xpr to X_State and then update in each iterations
+        memcpy(X_state, this->S, sizeof(X_state));                     // copy Xpr to X_State and then update in each iterations
 
         // ---------------------- Start iteration ----------------------- //
         for (int iter = 0; iter < MAX_ITER; iter++){
@@ -189,7 +189,7 @@ void kalmanCoreRobustUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *t
                     P_chol[k][k] = P_chol[k][k] + dummy_value;
                 }
                 // keep P_chol
-                matrixcopy(KC_STATE_DIM, KC_STATE_DIM, tmp1, P_chol);
+                memcpy(tmp1, P_chol, sizeof(tmp1));
                 mat_inv(&tmp1m, &Pc_inv_m);                            // Pc_inv_m = inv(Pc_m) = inv(P_chol)
                 mat_mult(&Pc_inv_m, &x_errm, &e_x_m);                  // e_x_m = Pc_inv_m.dot(x_errm) 
                 // compute w_x, w_y --> weighting matrix
@@ -226,7 +226,7 @@ void kalmanCoreRobustUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *t
                     X_state[i] = this->S[i] + x_err[i];               // convert to nominal state
                 }
                 // update P_iter matrix and R matrix for next iteration
-                matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter, P_w);
+                memcpy(P_iter, P_w, sizeof(P_iter));
                 R_iter = R_w;
             }
         }

diff --git a/src/utils/interface/cf_math.h b/src/utils/interface/cf_math.h
@@ -117,21 +117,3 @@ static inline void mat_scale(const arm_matrix_instance_f32 * pSrcA, float32_t sc
   arm_status result = arm_mat_scale_f32(pSrcA, scale, pDst);
   ASSERT(ARM_MATH_SUCCESS == result);
 }
-
-// copy float matrix
-static inline void matrixcopy(int ROW, int COLUMN, float destmat[ROW][COLUMN], float srcmat[ROW][COLUMN]){
-    //TODO: check the dimension of the matrices
-    for (int i=0; i<ROW; i++){
-        for(int j=0; j<COLUMN; j++){
-            destmat[i][j] = srcmat[i][j];
-        }
-    }
-}
-
-// copy float vector
-static inline void vectorcopy(int DIM, float destVec[DIM], float srcVec[DIM]){
-    //TODO: check the dimension of the vector
-    for (int i=0; i<DIM; i++){
-        destVec[i] = srcVec[i];
-    }
-}