Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ekf2: predict covariance avoid explicit temporary nextP #22205

Merged
merged 1 commit into from
Oct 14, 2023
Merged

ekf2: predict covariance avoid explicit temporary nextP #22205

merged 1 commit into from
Oct 14, 2023

Conversation

dagar
Copy link
Member

@dagar dagar commented Oct 11, 2023

This saves about 700 bytes of flash.

bresch
bresch reviewed Oct 11, 2023
View reviewed changes
src/modules/ekf2/EKF/covariance.cpp
}

} else {
// keep previous covariance
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How do we handle that now?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't it already propagated (upper triangular) in P_new?

PX4-Autopilot/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h

Lines 926 to 929 in 0eea925

_res(16, 19) = P(16, 19);
_res(17, 19) = P(17, 19);
_res(18, 19) = P(18, 19);
_res(19, 19) = P(19, 19);

Copy link
Member

@bresch bresch Oct 12, 2023 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, but with that code we're also stopping the propagation of the covariance between the mag and all the other states:

PX4-Autopilot/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h

Lines 802 to 985 in 0eea925

_res(0, 16) = P(0, 16) + P(1, 16) * _tmp9 + P(10, 16) * _tmp10 + P(11, 16) * _tmp12 +
P(12, 16) * _tmp11 + P(2, 16) * _tmp3 + P(3, 16) * _tmp6;
_res(1, 16) = P(0, 16) * _tmp31 + P(1, 16) - P(10, 16) * _tmp29 + P(11, 16) * _tmp11 -
P(12, 16) * _tmp12 + P(2, 16) * _tmp32 + P(3, 16) * _tmp3;
_res(2, 16) = P(0, 16) * _tmp43 + P(1, 16) * _tmp6 - P(10, 16) * _tmp11 - P(11, 16) * _tmp29 +
P(12, 16) * _tmp10 + P(2, 16) + P(3, 16) * _tmp31;
_res(3, 16) = P(0, 16) * _tmp32 + P(1, 16) * _tmp43 + P(10, 16) * _tmp12 - P(11, 16) * _tmp10 -
P(12, 16) * _tmp29 + P(2, 16) * _tmp9 + P(3, 16);
_res(4, 16) = P(0, 16) * _tmp64 + P(1, 16) * _tmp85 - P(13, 16) * _tmp69 - P(14, 16) * _tmp81 -
P(15, 16) * _tmp75 + P(2, 16) * _tmp90 + P(3, 16) * _tmp93 + P(4, 16);
_res(5, 16) = P(0, 16) * _tmp131 + P(1, 16) * _tmp127 - P(13, 16) * _tmp124 -
P(14, 16) * _tmp122 - P(15, 16) * _tmp119 + P(2, 16) * _tmp133 +
P(3, 16) * _tmp129 + P(5, 16);
_res(6, 16) = P(0, 16) * _tmp158 + P(1, 16) * _tmp156 - P(13, 16) * _tmp154 -
P(14, 16) * _tmp150 - P(15, 16) * _tmp152 + P(2, 16) * _tmp157 +
P(3, 16) * _tmp159 + P(6, 16);
_res(7, 16) = P(4, 16) * d_vel_dt + P(7, 16);
_res(8, 16) = P(5, 16) * d_vel_dt + P(8, 16);
_res(9, 16) = P(6, 16) * d_vel_dt + P(9, 16);
_res(10, 16) = P(10, 16);
_res(11, 16) = P(11, 16);
_res(12, 16) = P(12, 16);
_res(13, 16) = P(13, 16);
_res(14, 16) = P(14, 16);
_res(15, 16) = P(15, 16);
_res(16, 16) = P(16, 16);
_res(17, 16) = 0;
_res(18, 16) = 0;
_res(19, 16) = 0;
_res(20, 16) = 0;
_res(21, 16) = 0;
_res(22, 16) = 0;
_res(23, 16) = 0;
_res(0, 17) = P(0, 17) + P(1, 17) * _tmp9 + P(10, 17) * _tmp10 + P(11, 17) * _tmp12 +
P(12, 17) * _tmp11 + P(2, 17) * _tmp3 + P(3, 17) * _tmp6;
_res(1, 17) = P(0, 17) * _tmp31 + P(1, 17) - P(10, 17) * _tmp29 + P(11, 17) * _tmp11 -
P(12, 17) * _tmp12 + P(2, 17) * _tmp32 + P(3, 17) * _tmp3;
_res(2, 17) = P(0, 17) * _tmp43 + P(1, 17) * _tmp6 - P(10, 17) * _tmp11 - P(11, 17) * _tmp29 +
P(12, 17) * _tmp10 + P(2, 17) + P(3, 17) * _tmp31;
_res(3, 17) = P(0, 17) * _tmp32 + P(1, 17) * _tmp43 + P(10, 17) * _tmp12 - P(11, 17) * _tmp10 -
P(12, 17) * _tmp29 + P(2, 17) * _tmp9 + P(3, 17);
_res(4, 17) = P(0, 17) * _tmp64 + P(1, 17) * _tmp85 - P(13, 17) * _tmp69 - P(14, 17) * _tmp81 -
P(15, 17) * _tmp75 + P(2, 17) * _tmp90 + P(3, 17) * _tmp93 + P(4, 17);
_res(5, 17) = P(0, 17) * _tmp131 + P(1, 17) * _tmp127 - P(13, 17) * _tmp124 -
P(14, 17) * _tmp122 - P(15, 17) * _tmp119 + P(2, 17) * _tmp133 +
P(3, 17) * _tmp129 + P(5, 17);
_res(6, 17) = P(0, 17) * _tmp158 + P(1, 17) * _tmp156 - P(13, 17) * _tmp154 -
P(14, 17) * _tmp150 - P(15, 17) * _tmp152 + P(2, 17) * _tmp157 +
P(3, 17) * _tmp159 + P(6, 17);
_res(7, 17) = P(4, 17) * d_vel_dt + P(7, 17);
_res(8, 17) = P(5, 17) * d_vel_dt + P(8, 17);
_res(9, 17) = P(6, 17) * d_vel_dt + P(9, 17);
_res(10, 17) = P(10, 17);
_res(11, 17) = P(11, 17);
_res(12, 17) = P(12, 17);
_res(13, 17) = P(13, 17);
_res(14, 17) = P(14, 17);
_res(15, 17) = P(15, 17);
_res(16, 17) = P(16, 17);
_res(17, 17) = P(17, 17);
_res(18, 17) = 0;
_res(19, 17) = 0;
_res(20, 17) = 0;
_res(21, 17) = 0;
_res(22, 17) = 0;
_res(23, 17) = 0;
_res(0, 18) = P(0, 18) + P(1, 18) * _tmp9 + P(10, 18) * _tmp10 + P(11, 18) * _tmp12 +
P(12, 18) * _tmp11 + P(2, 18) * _tmp3 + P(3, 18) * _tmp6;
_res(1, 18) = P(0, 18) * _tmp31 + P(1, 18) - P(10, 18) * _tmp29 + P(11, 18) * _tmp11 -
P(12, 18) * _tmp12 + P(2, 18) * _tmp32 + P(3, 18) * _tmp3;
_res(2, 18) = P(0, 18) * _tmp43 + P(1, 18) * _tmp6 - P(10, 18) * _tmp11 - P(11, 18) * _tmp29 +
P(12, 18) * _tmp10 + P(2, 18) + P(3, 18) * _tmp31;
_res(3, 18) = P(0, 18) * _tmp32 + P(1, 18) * _tmp43 + P(10, 18) * _tmp12 - P(11, 18) * _tmp10 -
P(12, 18) * _tmp29 + P(2, 18) * _tmp9 + P(3, 18);
_res(4, 18) = P(0, 18) * _tmp64 + P(1, 18) * _tmp85 - P(13, 18) * _tmp69 - P(14, 18) * _tmp81 -
P(15, 18) * _tmp75 + P(2, 18) * _tmp90 + P(3, 18) * _tmp93 + P(4, 18);
_res(5, 18) = P(0, 18) * _tmp131 + P(1, 18) * _tmp127 - P(13, 18) * _tmp124 -
P(14, 18) * _tmp122 - P(15, 18) * _tmp119 + P(2, 18) * _tmp133 +
P(3, 18) * _tmp129 + P(5, 18);
_res(6, 18) = P(0, 18) * _tmp158 + P(1, 18) * _tmp156 - P(13, 18) * _tmp154 -
P(14, 18) * _tmp150 - P(15, 18) * _tmp152 + P(2, 18) * _tmp157 +
P(3, 18) * _tmp159 + P(6, 18);
_res(7, 18) = P(4, 18) * d_vel_dt + P(7, 18);
_res(8, 18) = P(5, 18) * d_vel_dt + P(8, 18);
_res(9, 18) = P(6, 18) * d_vel_dt + P(9, 18);
_res(10, 18) = P(10, 18);
_res(11, 18) = P(11, 18);
_res(12, 18) = P(12, 18);
_res(13, 18) = P(13, 18);
_res(14, 18) = P(14, 18);
_res(15, 18) = P(15, 18);
_res(16, 18) = P(16, 18);
_res(17, 18) = P(17, 18);
_res(18, 18) = P(18, 18);
_res(19, 18) = 0;
_res(20, 18) = 0;
_res(21, 18) = 0;
_res(22, 18) = 0;
_res(23, 18) = 0;
_res(0, 19) = P(0, 19) + P(1, 19) * _tmp9 + P(10, 19) * _tmp10 + P(11, 19) * _tmp12 +
P(12, 19) * _tmp11 + P(2, 19) * _tmp3 + P(3, 19) * _tmp6;
_res(1, 19) = P(0, 19) * _tmp31 + P(1, 19) - P(10, 19) * _tmp29 + P(11, 19) * _tmp11 -
P(12, 19) * _tmp12 + P(2, 19) * _tmp32 + P(3, 19) * _tmp3;
_res(2, 19) = P(0, 19) * _tmp43 + P(1, 19) * _tmp6 - P(10, 19) * _tmp11 - P(11, 19) * _tmp29 +
P(12, 19) * _tmp10 + P(2, 19) + P(3, 19) * _tmp31;
_res(3, 19) = P(0, 19) * _tmp32 + P(1, 19) * _tmp43 + P(10, 19) * _tmp12 - P(11, 19) * _tmp10 -
P(12, 19) * _tmp29 + P(2, 19) * _tmp9 + P(3, 19);
_res(4, 19) = P(0, 19) * _tmp64 + P(1, 19) * _tmp85 - P(13, 19) * _tmp69 - P(14, 19) * _tmp81 -
P(15, 19) * _tmp75 + P(2, 19) * _tmp90 + P(3, 19) * _tmp93 + P(4, 19);
_res(5, 19) = P(0, 19) * _tmp131 + P(1, 19) * _tmp127 - P(13, 19) * _tmp124 -
P(14, 19) * _tmp122 - P(15, 19) * _tmp119 + P(2, 19) * _tmp133 +
P(3, 19) * _tmp129 + P(5, 19);
_res(6, 19) = P(0, 19) * _tmp158 + P(1, 19) * _tmp156 - P(13, 19) * _tmp154 -
P(14, 19) * _tmp150 - P(15, 19) * _tmp152 + P(2, 19) * _tmp157 +
P(3, 19) * _tmp159 + P(6, 19);
_res(7, 19) = P(4, 19) * d_vel_dt + P(7, 19);
_res(8, 19) = P(5, 19) * d_vel_dt + P(8, 19);
_res(9, 19) = P(6, 19) * d_vel_dt + P(9, 19);
_res(10, 19) = P(10, 19);
_res(11, 19) = P(11, 19);
_res(12, 19) = P(12, 19);
_res(13, 19) = P(13, 19);
_res(14, 19) = P(14, 19);
_res(15, 19) = P(15, 19);
_res(16, 19) = P(16, 19);
_res(17, 19) = P(17, 19);
_res(18, 19) = P(18, 19);
_res(19, 19) = P(19, 19);
_res(20, 19) = 0;
_res(21, 19) = 0;
_res(22, 19) = 0;
_res(23, 19) = 0;
_res(0, 20) = P(0, 20) + P(1, 20) * _tmp9 + P(10, 20) * _tmp10 + P(11, 20) * _tmp12 +
P(12, 20) * _tmp11 + P(2, 20) * _tmp3 + P(3, 20) * _tmp6;
_res(1, 20) = P(0, 20) * _tmp31 + P(1, 20) - P(10, 20) * _tmp29 + P(11, 20) * _tmp11 -
P(12, 20) * _tmp12 + P(2, 20) * _tmp32 + P(3, 20) * _tmp3;
_res(2, 20) = P(0, 20) * _tmp43 + P(1, 20) * _tmp6 - P(10, 20) * _tmp11 - P(11, 20) * _tmp29 +
P(12, 20) * _tmp10 + P(2, 20) + P(3, 20) * _tmp31;
_res(3, 20) = P(0, 20) * _tmp32 + P(1, 20) * _tmp43 + P(10, 20) * _tmp12 - P(11, 20) * _tmp10 -
P(12, 20) * _tmp29 + P(2, 20) * _tmp9 + P(3, 20);
_res(4, 20) = P(0, 20) * _tmp64 + P(1, 20) * _tmp85 - P(13, 20) * _tmp69 - P(14, 20) * _tmp81 -
P(15, 20) * _tmp75 + P(2, 20) * _tmp90 + P(3, 20) * _tmp93 + P(4, 20);
_res(5, 20) = P(0, 20) * _tmp131 + P(1, 20) * _tmp127 - P(13, 20) * _tmp124 -
P(14, 20) * _tmp122 - P(15, 20) * _tmp119 + P(2, 20) * _tmp133 +
P(3, 20) * _tmp129 + P(5, 20);
_res(6, 20) = P(0, 20) * _tmp158 + P(1, 20) * _tmp156 - P(13, 20) * _tmp154 -
P(14, 20) * _tmp150 - P(15, 20) * _tmp152 + P(2, 20) * _tmp157 +
P(3, 20) * _tmp159 + P(6, 20);
_res(7, 20) = P(4, 20) * d_vel_dt + P(7, 20);
_res(8, 20) = P(5, 20) * d_vel_dt + P(8, 20);
_res(9, 20) = P(6, 20) * d_vel_dt + P(9, 20);
_res(10, 20) = P(10, 20);
_res(11, 20) = P(11, 20);
_res(12, 20) = P(12, 20);
_res(13, 20) = P(13, 20);
_res(14, 20) = P(14, 20);
_res(15, 20) = P(15, 20);
_res(16, 20) = P(16, 20);
_res(17, 20) = P(17, 20);
_res(18, 20) = P(18, 20);
_res(19, 20) = P(19, 20);
_res(20, 20) = P(20, 20);
_res(21, 20) = 0;
_res(22, 20) = 0;
_res(23, 20) = 0;
_res(0, 21) = P(0, 21) + P(1, 21) * _tmp9 + P(10, 21) * _tmp10 + P(11, 21) * _tmp12 +
P(12, 21) * _tmp11 + P(2, 21) * _tmp3 + P(3, 21) * _tmp6;
_res(1, 21) = P(0, 21) * _tmp31 + P(1, 21) - P(10, 21) * _tmp29 + P(11, 21) * _tmp11 -
P(12, 21) * _tmp12 + P(2, 21) * _tmp32 + P(3, 21) * _tmp3;
_res(2, 21) = P(0, 21) * _tmp43 + P(1, 21) * _tmp6 - P(10, 21) * _tmp11 - P(11, 21) * _tmp29 +
P(12, 21) * _tmp10 + P(2, 21) + P(3, 21) * _tmp31;
_res(3, 21) = P(0, 21) * _tmp32 + P(1, 21) * _tmp43 + P(10, 21) * _tmp12 - P(11, 21) * _tmp10 -
P(12, 21) * _tmp29 + P(2, 21) * _tmp9 + P(3, 21);
_res(4, 21) = P(0, 21) * _tmp64 + P(1, 21) * _tmp85 - P(13, 21) * _tmp69 - P(14, 21) * _tmp81 -
P(15, 21) * _tmp75 + P(2, 21) * _tmp90 + P(3, 21) * _tmp93 + P(4, 21);
_res(5, 21) = P(0, 21) * _tmp131 + P(1, 21) * _tmp127 - P(13, 21) * _tmp124 -
P(14, 21) * _tmp122 - P(15, 21) * _tmp119 + P(2, 21) * _tmp133 +
P(3, 21) * _tmp129 + P(5, 21);
_res(6, 21) = P(0, 21) * _tmp158 + P(1, 21) * _tmp156 - P(13, 21) * _tmp154 -
P(14, 21) * _tmp150 - P(15, 21) * _tmp152 + P(2, 21) * _tmp157 +
P(3, 21) * _tmp159 + P(6, 21);
_res(7, 21) = P(4, 21) * d_vel_dt + P(7, 21);
_res(8, 21) = P(5, 21) * d_vel_dt + P(8, 21);
_res(9, 21) = P(6, 21) * d_vel_dt + P(9, 21);

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

However I'm wondering if this makes sense. Even if we only consider those states when the fusion is stopped, the covariance between that one and the other states should still evolve through the covariance propagation step.

@bresch bresch self-requested a review October 12, 2023 08:27
@dagar dagar merged commit 9676af2 into main Oct 14, 2023
87 checks passed
@dagar dagar deleted the pr-ekf2_predict_covariance_skip_nextP branch October 14, 2023 00:57
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@bresch bresch Awaiting requested review from bresch

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

None yet
2 participants
@dagar @bresch