UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: symforce zero more efficiently (#23133)

Browse Source 
- increase symforce CppConfig zero_initialization_sparsity_threshold so
   that a Matrix setZero() call is performed instead of individually zeroing

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2024-05-17 05:20:30 -04:00
committed by GitHub
parent ea39032b45
commit d359f6236e
6 changed files with 24 additions and 278 deletions
Download Patch File Download Diff File Expand all files Collapse all files

255
src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h
View File

@@ -236,57 +236,16 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
// Output terms (1)
matrix::Matrix<Scalar, 23, 23> _res;
_res.setZero();
_res(0, 0) = std::pow(_tmp15, Scalar(2)) * gyro_var + _tmp15 * _tmp27 + _tmp18 * _tmp26 +
std::pow(_tmp23, Scalar(2)) * gyro_var + _tmp23 * _tmp25 + _tmp24 * _tmp6 +
std::pow(_tmp6, Scalar(2)) * gyro_var;
_res(1, 0) = 0;
_res(2, 0) = 0;
_res(3, 0) = 0;
_res(4, 0) = 0;
_res(5, 0) = 0;
_res(6, 0) = 0;
_res(7, 0) = 0;
_res(8, 0) = 0;
_res(9, 0) = 0;
_res(10, 0) = 0;
_res(11, 0) = 0;
_res(12, 0) = 0;
_res(13, 0) = 0;
_res(14, 0) = 0;
_res(15, 0) = 0;
_res(16, 0) = 0;
_res(17, 0) = 0;
_res(18, 0) = 0;
_res(19, 0) = 0;
_res(20, 0) = 0;
_res(21, 0) = 0;
_res(22, 0) = 0;
_res(0, 1) = _tmp15 * _tmp38 + _tmp18 * _tmp35 + _tmp24 * _tmp34 + _tmp25 * _tmp29 +
_tmp27 * _tmp36 + _tmp29 * _tmp37 + _tmp34 * _tmp39;
_res(1, 1) = _tmp18 * _tmp43 + std::pow(_tmp29, Scalar(2)) * gyro_var + _tmp29 * _tmp40 +
std::pow(_tmp34, Scalar(2)) * gyro_var + _tmp34 * _tmp41 +
std::pow(_tmp36, Scalar(2)) * gyro_var + _tmp36 * _tmp42;
_res(2, 1) = 0;
_res(3, 1) = 0;
_res(4, 1) = 0;
_res(5, 1) = 0;
_res(6, 1) = 0;
_res(7, 1) = 0;
_res(8, 1) = 0;
_res(9, 1) = 0;
_res(10, 1) = 0;
_res(11, 1) = 0;
_res(12, 1) = 0;
_res(13, 1) = 0;
_res(14, 1) = 0;
_res(15, 1) = 0;
_res(16, 1) = 0;
_res(17, 1) = 0;
_res(18, 1) = 0;
_res(19, 1) = 0;
_res(20, 1) = 0;
_res(21, 1) = 0;
_res(22, 1) = 0;
_res(0, 2) = _tmp15 * _tmp47 * gyro_var + _tmp18 * _tmp46 + _tmp24 * _tmp44 + _tmp25 * _tmp45 +
_tmp27 * _tmp47 + _tmp37 * _tmp45 + _tmp39 * _tmp44;
_res(1, 2) = _tmp18 * _tmp48 + _tmp29 * _tmp45 * gyro_var + _tmp34 * _tmp44 * gyro_var +
@@ -294,26 +253,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(2, 2) = _tmp18 * _tmp51 + std::pow(_tmp44, Scalar(2)) * gyro_var + _tmp44 * _tmp50 +
std::pow(_tmp45, Scalar(2)) * gyro_var + _tmp45 * _tmp49 +
std::pow(_tmp47, Scalar(2)) * gyro_var + _tmp47 * _tmp52;
_res(3, 2) = 0;
_res(4, 2) = 0;
_res(5, 2) = 0;
_res(6, 2) = 0;
_res(7, 2) = 0;
_res(8, 2) = 0;
_res(9, 2) = 0;
_res(10, 2) = 0;
_res(11, 2) = 0;
_res(12, 2) = 0;
_res(13, 2) = 0;
_res(14, 2) = 0;
_res(15, 2) = 0;
_res(16, 2) = 0;
_res(17, 2) = 0;
_res(18, 2) = 0;
_res(19, 2) = 0;
_res(20, 2) = 0;
_res(21, 2) = 0;
_res(22, 2) = 0;
_res(0, 3) = _tmp35 * _tmp75 + _tmp46 * _tmp79 - _tmp53 * _tmp57 - _tmp58 * _tmp61 -
_tmp62 * _tmp64 + _tmp80;
_res(1, 3) = _tmp43 * _tmp75 + _tmp48 * _tmp79 - _tmp57 * _tmp81 - _tmp61 * _tmp82 -
@@ -324,25 +263,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
std::pow(_tmp60, Scalar(2)) * _tmp97 - _tmp61 * _tmp93 +
std::pow(_tmp63, Scalar(2)) * _tmp98 - _tmp64 * _tmp94 + _tmp75 * _tmp91 +
_tmp79 * _tmp92 + _tmp99;
_res(4, 3) = 0;
_res(5, 3) = 0;
_res(6, 3) = 0;
_res(7, 3) = 0;
_res(8, 3) = 0;
_res(9, 3) = 0;
_res(10, 3) = 0;
_res(11, 3) = 0;
_res(12, 3) = 0;
_res(13, 3) = 0;
_res(14, 3) = 0;
_res(15, 3) = 0;
_res(16, 3) = 0;
_res(17, 3) = 0;
_res(18, 3) = 0;
_res(19, 3) = 0;
_res(20, 3) = 0;
_res(21, 3) = 0;
_res(22, 3) = 0;
_res(0, 4) = -_tmp102 * _tmp58 + _tmp105 * _tmp46 - _tmp108 * _tmp62 - _tmp110 * _tmp53 +
_tmp113 * _tmp114 + _tmp115;
_res(1, 4) = -_tmp102 * _tmp82 + _tmp105 * _tmp48 - _tmp108 * _tmp83 - _tmp110 * _tmp81 +
@@ -358,24 +278,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp128 +
std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp129 + _tmp117 * _tmp126 +
_tmp130;
_res(5, 4) = 0;
_res(6, 4) = 0;
_res(7, 4) = 0;
_res(8, 4) = 0;
_res(9, 4) = 0;
_res(10, 4) = 0;
_res(11, 4) = 0;
_res(12, 4) = 0;
_res(13, 4) = 0;
_res(14, 4) = 0;
_res(15, 4) = 0;
_res(16, 4) = 0;
_res(17, 4) = 0;
_res(18, 4) = 0;
_res(19, 4) = 0;
_res(20, 4) = 0;
_res(21, 4) = 0;
_res(22, 4) = 0;
_res(0, 5) = _tmp114 * _tmp136 - _tmp132 * _tmp62 + _tmp133 * _tmp35 - _tmp134 * _tmp58 -
_tmp135 * _tmp53 + _tmp137;
_res(1, 5) = _tmp116 * _tmp138 - _tmp132 * _tmp83 + _tmp133 * _tmp43 - _tmp134 * _tmp82 -
@@ -397,23 +299,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
P(13, 0) * _tmp134 - P(14, 0) * _tmp132 + P(5, 0)) +
_tmp146 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
std::pow(_tmp73, Scalar(2)) * _tmp97;
_res(6, 5) = 0;
_res(7, 5) = 0;
_res(8, 5) = 0;
_res(9, 5) = 0;
_res(10, 5) = 0;
_res(11, 5) = 0;
_res(12, 5) = 0;
_res(13, 5) = 0;
_res(14, 5) = 0;
_res(15, 5) = 0;
_res(16, 5) = 0;
_res(17, 5) = 0;
_res(18, 5) = 0;
_res(19, 5) = 0;
_res(20, 5) = 0;
_res(21, 5) = 0;
_res(22, 5) = 0;
_res(0, 6) =
P(0, 6) * _tmp18 + P(10, 6) * _tmp23 + P(11, 6) * _tmp6 + P(9, 6) * _tmp15 + _tmp80 * dt;
_res(1, 6) =
@@ -431,22 +316,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
dt * (P(0, 3) * _tmp138 + P(1, 3) * _tmp133 - P(12, 3) * _tmp135 -
P(13, 3) * _tmp134 - P(14, 3) * _tmp132 + P(5, 3));
_res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
_res(7, 6) = 0;
_res(8, 6) = 0;
_res(9, 6) = 0;
_res(10, 6) = 0;
_res(11, 6) = 0;
_res(12, 6) = 0;
_res(13, 6) = 0;
_res(14, 6) = 0;
_res(15, 6) = 0;
_res(16, 6) = 0;
_res(17, 6) = 0;
_res(18, 6) = 0;
_res(19, 6) = 0;
_res(20, 6) = 0;
_res(21, 6) = 0;
_res(22, 6) = 0;
_res(0, 7) =
P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp115 * dt;
_res(1, 7) =
@@ -463,21 +332,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
P(13, 4) * _tmp134 - P(14, 4) * _tmp132 + P(5, 4));
_res(6, 7) = P(3, 7) * dt + P(6, 7) + dt * (P(3, 4) * dt + P(6, 4));
_res(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4));
_res(8, 7) = 0;
_res(9, 7) = 0;
_res(10, 7) = 0;
_res(11, 7) = 0;
_res(12, 7) = 0;
_res(13, 7) = 0;
_res(14, 7) = 0;
_res(15, 7) = 0;
_res(16, 7) = 0;
_res(17, 7) = 0;
_res(18, 7) = 0;
_res(19, 7) = 0;
_res(20, 7) = 0;
_res(21, 7) = 0;
_res(22, 7) = 0;
_res(0, 8) =
P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp137 * dt;
_res(1, 8) =
@@ -493,20 +347,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(6, 8) = P(3, 8) * dt + P(6, 8) + dt * (P(3, 5) * dt + P(6, 5));
_res(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5));
_res(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5));
_res(9, 8) = 0;
_res(10, 8) = 0;
_res(11, 8) = 0;
_res(12, 8) = 0;
_res(13, 8) = 0;
_res(14, 8) = 0;
_res(15, 8) = 0;
_res(16, 8) = 0;
_res(17, 8) = 0;
_res(18, 8) = 0;
_res(19, 8) = 0;
_res(20, 8) = 0;
_res(21, 8) = 0;
_res(22, 8) = 0;
_res(0, 9) = _tmp27;
_res(1, 9) = _tmp42;
_res(2, 9) = _tmp52;
@@ -520,19 +360,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(7, 9) = P(4, 9) * dt + P(7, 9);
_res(8, 9) = P(5, 9) * dt + P(8, 9);
_res(9, 9) = P(9, 9);
_res(10, 9) = 0;
_res(11, 9) = 0;
_res(12, 9) = 0;
_res(13, 9) = 0;
_res(14, 9) = 0;
_res(15, 9) = 0;
_res(16, 9) = 0;
_res(17, 9) = 0;
_res(18, 9) = 0;
_res(19, 9) = 0;
_res(20, 9) = 0;
_res(21, 9) = 0;
_res(22, 9) = 0;
_res(0, 10) = _tmp25;
_res(1, 10) = _tmp40;
_res(2, 10) = _tmp49;
@@ -547,18 +374,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(8, 10) = P(5, 10) * dt + P(8, 10);
_res(9, 10) = P(9, 10);
_res(10, 10) = P(10, 10);
_res(11, 10) = 0;
_res(12, 10) = 0;
_res(13, 10) = 0;
_res(14, 10) = 0;
_res(15, 10) = 0;
_res(16, 10) = 0;
_res(17, 10) = 0;
_res(18, 10) = 0;
_res(19, 10) = 0;
_res(20, 10) = 0;
_res(21, 10) = 0;
_res(22, 10) = 0;
_res(0, 11) = _tmp24;
_res(1, 11) = _tmp41;
_res(2, 11) = _tmp50;
@@ -574,17 +389,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(9, 11) = P(9, 11);
_res(10, 11) = P(10, 11);
_res(11, 11) = P(11, 11);
_res(12, 11) = 0;
_res(13, 11) = 0;
_res(14, 11) = 0;
_res(15, 11) = 0;
_res(16, 11) = 0;
_res(17, 11) = 0;
_res(18, 11) = 0;
_res(19, 11) = 0;
_res(20, 11) = 0;
_res(21, 11) = 0;
_res(22, 11) = 0;
_res(0, 12) = _tmp53;
_res(1, 12) = _tmp81;
_res(2, 12) = _tmp85;
@@ -598,16 +402,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(10, 12) = P(10, 12);
_res(11, 12) = P(11, 12);
_res(12, 12) = P(12, 12);
_res(13, 12) = 0;
_res(14, 12) = 0;
_res(15, 12) = 0;
_res(16, 12) = 0;
_res(17, 12) = 0;
_res(18, 12) = 0;
_res(19, 12) = 0;
_res(20, 12) = 0;
_res(21, 12) = 0;
_res(22, 12) = 0;
_res(0, 13) = _tmp58;
_res(1, 13) = _tmp82;
_res(2, 13) = _tmp87;
@@ -622,15 +416,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(11, 13) = P(11, 13);
_res(12, 13) = P(12, 13);
_res(13, 13) = P(13, 13);
_res(14, 13) = 0;
_res(15, 13) = 0;
_res(16, 13) = 0;
_res(17, 13) = 0;
_res(18, 13) = 0;
_res(19, 13) = 0;
_res(20, 13) = 0;
_res(21, 13) = 0;
_res(22, 13) = 0;
_res(0, 14) = _tmp62;
_res(1, 14) = _tmp83;
_res(2, 14) = _tmp88;
@@ -646,14 +431,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(12, 14) = P(12, 14);
_res(13, 14) = P(13, 14);
_res(14, 14) = P(14, 14);
_res(15, 14) = 0;
_res(16, 14) = 0;
_res(17, 14) = 0;
_res(18, 14) = 0;
_res(19, 14) = 0;
_res(20, 14) = 0;
_res(21, 14) = 0;
_res(22, 14) = 0;
_res(0, 15) = P(0, 15) * _tmp18 + P(10, 15) * _tmp23 + P(11, 15) * _tmp6 + P(9, 15) * _tmp15;
_res(1, 15) = P(1, 15) * _tmp18 + P(10, 15) * _tmp29 + P(11, 15) * _tmp34 + P(9, 15) * _tmp36;
_res(2, 15) = P(10, 15) * _tmp45 + P(11, 15) * _tmp44 + P(2, 15) * _tmp18 + P(9, 15) * _tmp47;
@@ -673,13 +450,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(13, 15) = P(13, 15);
_res(14, 15) = P(14, 15);
_res(15, 15) = P(15, 15);
_res(16, 15) = 0;
_res(17, 15) = 0;
_res(18, 15) = 0;
_res(19, 15) = 0;
_res(20, 15) = 0;
_res(21, 15) = 0;
_res(22, 15) = 0;
_res(0, 16) = P(0, 16) * _tmp18 + P(10, 16) * _tmp23 + P(11, 16) * _tmp6 + P(9, 16) * _tmp15;
_res(1, 16) = P(1, 16) * _tmp18 + P(10, 16) * _tmp29 + P(11, 16) * _tmp34 + P(9, 16) * _tmp36;
_res(2, 16) = P(10, 16) * _tmp45 + P(11, 16) * _tmp44 + P(2, 16) * _tmp18 + P(9, 16) * _tmp47;
@@ -700,12 +470,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_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(0, 17) = P(0, 17) * _tmp18 + P(10, 17) * _tmp23 + P(11, 17) * _tmp6 + P(9, 17) * _tmp15;
_res(1, 17) = P(1, 17) * _tmp18 + P(10, 17) * _tmp29 + P(11, 17) * _tmp34 + P(9, 17) * _tmp36;
_res(2, 17) = P(10, 17) * _tmp45 + P(11, 17) * _tmp44 + P(2, 17) * _tmp18 + P(9, 17) * _tmp47;
@@ -727,11 +491,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_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(0, 18) = P(0, 18) * _tmp18 + P(10, 18) * _tmp23 + P(11, 18) * _tmp6 + P(9, 18) * _tmp15;
_res(1, 18) = P(1, 18) * _tmp18 + P(10, 18) * _tmp29 + P(11, 18) * _tmp34 + P(9, 18) * _tmp36;
_res(2, 18) = P(10, 18) * _tmp45 + P(11, 18) * _tmp44 + P(2, 18) * _tmp18 + P(9, 18) * _tmp47;
@@ -754,10 +513,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_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(0, 19) = P(0, 19) * _tmp18 + P(10, 19) * _tmp23 + P(11, 19) * _tmp6 + P(9, 19) * _tmp15;
_res(1, 19) = P(1, 19) * _tmp18 + P(10, 19) * _tmp29 + P(11, 19) * _tmp34 + P(9, 19) * _tmp36;
_res(2, 19) = P(10, 19) * _tmp45 + P(11, 19) * _tmp44 + P(2, 19) * _tmp18 + P(9, 19) * _tmp47;
@@ -781,9 +536,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_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(0, 20) = P(0, 20) * _tmp18 + P(10, 20) * _tmp23 + P(11, 20) * _tmp6 + P(9, 20) * _tmp15;
_res(1, 20) = P(1, 20) * _tmp18 + P(10, 20) * _tmp29 + P(11, 20) * _tmp34 + P(9, 20) * _tmp36;
_res(2, 20) = P(10, 20) * _tmp45 + P(11, 20) * _tmp44 + P(2, 20) * _tmp18 + P(9, 20) * _tmp47;
@@ -808,8 +560,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_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(0, 21) = P(0, 21) * _tmp18 + P(10, 21) * _tmp23 + P(11, 21) * _tmp6 + P(9, 21) * _tmp15;
_res(1, 21) = P(1, 21) * _tmp18 + P(10, 21) * _tmp29 + P(11, 21) * _tmp34 + P(9, 21) * _tmp36;
_res(2, 21) = P(10, 21) * _tmp45 + P(11, 21) * _tmp44 + P(2, 21) * _tmp18 + P(9, 21) * _tmp47;
@@ -835,7 +585,6 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
_res(19, 21) = P(19, 21);
_res(20, 21) = P(20, 21);
_res(21, 21) = P(21, 21);
_res(22, 21) = 0;
_res(0, 22) = P(0, 22) * _tmp18 + P(10, 22) * _tmp23 + P(11, 22) * _tmp6 + P(9, 22) * _tmp15;
_res(1, 22) = P(1, 22) * _tmp18 + P(10, 22) * _tmp29 + P(11, 22) * _tmp34 + P(9, 22) * _tmp36;
_res(2, 22) = P(10, 22) * _tmp45 + P(11, 22) * _tmp44 + P(2, 22) * _tmp18 + P(9, 22) * _tmp47;

2
src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py
View File

@@ -56,7 +56,7 @@ def generate_px4_function(function_name, output_names):
codegen = Codegen.function(
function_name,
output_names=output_names,
config=CppConfig())
config=CppConfig(zero_initialization_sparsity_threshold=1))
metadata = codegen.generate_function(
output_dir="generated",
skip_directory_nesting=True)

3
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
View File

@@ -301,8 +301,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
// Use fixed values for delta angle process noise variances
const float d_ang_var = sq(_gyro_noise * delta_ang_dt);
sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P,
Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var, &_ekf_gsf[model_index].P);
_ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var);
// covariance matrix is symmetrical, so copy upper half to lower half
_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);

2
src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py
View File

@@ -171,5 +171,5 @@ def yaw_est_compute_measurement_update(
return (S_inv, S_det_inv, K, P_new)
print("Derive yaw estimator equations...")
derivation_utils.generate_px4_function(yaw_est_predict_covariance, output_names=["P_new"])
derivation_utils.generate_px4_function(yaw_est_predict_covariance, output_names=None)
derivation_utils.generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"])

5
src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_compute_measurement_update.h
View File

@@ -86,12 +86,11 @@ void YawEstComputeMeasurementUpdate(const matrix::Matrix<Scalar, 3, 3>& P, const
if (P_new != nullptr) {
matrix::Matrix<Scalar, 3, 3>& _p_new = (*P_new);
_p_new.setZero();
_p_new(0, 0) = -P(0, 0) * _tmp9 + P(0, 0) - P(1, 0) * _tmp12;
_p_new(1, 0) = 0;
_p_new(2, 0) = 0;
_p_new(0, 1) = -P(0, 1) * _tmp9 + P(0, 1) - P(1, 1) * _tmp12;
_p_new(1, 1) = -P(0, 1) * _tmp10 - P(1, 1) * _tmp13 + P(1, 1);
_p_new(2, 1) = 0;
_p_new(0, 2) = -P(0, 2) * _tmp9 + P(0, 2) - P(1, 2) * _tmp12;
_p_new(1, 2) = -P(0, 2) * _tmp10 - P(1, 2) * _tmp13 + P(1, 2);
_p_new(2, 2) = -P(0, 2) * _tmp11 - P(1, 2) * _tmp14 + P(2, 2);

35
src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_predict_covariance.h
View File

@@ -24,14 +24,14 @@ namespace sym {
* d_ang_var: Scalar
*
* Outputs:
* P_new: Matrix33
* res: Matrix33
*/
template <typename Scalar>
void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
const matrix::Matrix<Scalar, 3, 3>& P,
const matrix::Matrix<Scalar, 2, 1>& d_vel, const Scalar d_vel_var,
const Scalar d_ang, const Scalar d_ang_var,
matrix::Matrix<Scalar, 3, 3>* const P_new = nullptr) {
matrix::Matrix<Scalar, 3, 3> YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
const matrix::Matrix<Scalar, 3, 3>& P,
const matrix::Matrix<Scalar, 2, 1>& d_vel,
const Scalar d_vel_var, const Scalar d_ang,
const Scalar d_ang_var) {
// Total ops: 39
// Input arrays
@@ -48,19 +48,18 @@ void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
const Scalar _tmp7 = std::pow(d_ang, Scalar(2)) + 1;
// Output terms (1)
if (P_new != nullptr) {
matrix::Matrix<Scalar, 3, 3>& _p_new = (*P_new);
matrix::Matrix<Scalar, 3, 3> _res;
_p_new(0, 0) = P(0, 0) + P(2, 0) * _tmp2 + _tmp2 * _tmp3 + _tmp4;
_p_new(1, 0) = 0;
_p_new(2, 0) = 0;
_p_new(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5;
_p_new(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6;
_p_new(2, 1) = 0;
_p_new(0, 2) = _tmp3 * _tmp7;
_p_new(1, 2) = _tmp6 * _tmp7;
_p_new(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var;
}
_res.setZero();
_res(0, 0) = P(0, 0) + P(2, 0) * _tmp2 + _tmp2 * _tmp3 + _tmp4;
_res(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5;
_res(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6;
_res(0, 2) = _tmp3 * _tmp7;
_res(1, 2) = _tmp6 * _tmp7;
_res(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var;
return _res;
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)