fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: optimize KHP computation

Browse Source 
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
This commit is contained in:
bresch 2022-04-29 12:14:33 +02:00 committed by Daniel Agar
parent e69d9ec48f
commit f8ff34f82d
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
1 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/ekf2/EKF/ekf.h
View File

@ -742,24 +742,20 @@ private:
template <size_t ...Idxs>
SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &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);
}
}