From f8ff34f82da8f16fe121b1c87a2889ed897c8af2 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 29 Apr 2022 12:14:33 +0200 Subject: [PATCH] ekf2: optimize KHP computation Calculating K(HP) takes less operations than (KH)P because K and H are vectors. Without considering the sparsity optimization: - KH (24*24 operations) is then a 24x24 matrix an it takes 24^3 operations to multiply it with P. Total: 14400 op - HP (24*(24+24-1) operations) is a row vector and it takes 24 operations to left-multiply it by K. Total:1152 op --- src/modules/ekf2/EKF/ekf.h | 28 ++++++++++++---------------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b59488117d..6eeb52fb1a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -742,24 +742,20 @@ private: template SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f &H) const { - SquareMatrix24f KHP; - constexpr size_t non_zeros = sizeof...(Idxs); - float KH[non_zeros]; - - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned i = 0; i < H.non_zeros(); i++) { - KH[i] = K(row) * H.atCompressedIndex(i); + // K(HP) and (KH)P are equivalent (matrix multiplication is associative) + // but K(HP) is computationally much less expensive + Vector24f HP; + for (unsigned i = 0; i < H.non_zeros(); i++) { + const size_t row = H.index(i); + for (unsigned col = 0; col < _k_num_states; col++) { + HP(col) = HP(col) + H.atCompressedIndex(i) * P(row, col); } + } - for (unsigned column = 0; column < _k_num_states; column++) { - float tmp = 0.f; - - for (unsigned i = 0; i < H.non_zeros(); i++) { - const size_t index = H.index(i); - tmp += KH[i] * P(index, column); - } - - KHP(row, column) = tmp; + SquareMatrix24f KHP; + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned col = 0; col < _k_num_states; col++) { + KHP(row, col) = K(row) * HP(col); } }