diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 5d0db4d245..8ed363b918 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -116,14 +116,14 @@ void Ekf::fuseAirspeed() _fault_status.flags.bad_airspeed = false; // Observation Jacobians - SparseVector24f<4,5,6,22,23> Hfusion; + SparseVector25f<4,5,6,22,23> Hfusion; Hfusion.at<4>() = HK4; Hfusion.at<5>() = HK5; Hfusion.at<6>() = HK3*vd; Hfusion.at<22>() = -HK4; Hfusion.at<23>() = -HK5; - Vector24f Kfusion; // Kalman gain vector + Vector25f Kfusion; // Kalman gain vector if (!update_wind_only) { // we have no other source of aiding, so use airspeed measurements to correct states diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index a045e8180e..5202f9c86e 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -386,14 +386,15 @@ struct parameters { }; struct stateSample { - Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame - Vector3f vel; ///< NED velocity in earth frame in m/s - Vector3f pos; ///< NED position in earth frame in m + Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame + Vector3f vel; ///< NED velocity in earth frame in m/s + Vector3f pos; ///< NED position in earth frame in m Vector3f delta_ang_bias; ///< delta angle bias estimate in rad Vector3f delta_vel_bias; ///< delta velocity bias estimate in m/s - Vector3f mag_I; ///< NED earth magnetic field in gauss - Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss - Vector2f wind_vel; ///< horizontal wind velocity in earth frame in m/s + Vector3f mag_I; ///< NED earth magnetic field in gauss + Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss + Vector2f wind_vel; ///< horizontal wind velocity in earth frame in m/s + float posd_terrain; ///< vertical position of the terrain surface in m }; union fault_status_u { diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f1b7478bd9..f6487c472f 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -202,7 +202,7 @@ void Ekf::predictCovariance() } // compute noise variance for stationary processes - Vector24f process_noise; + Vector25f process_noise; // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances @@ -217,6 +217,9 @@ void Ekf::predictCovariance() process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig); // wind velocity states process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig); + // terrain vertical position state + process_noise.slice<1, 1>(24, 0) = sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise) + sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) + * (sq(_state.vel(0)) + sq(_state.vel(1))); // assign IMU noise variances // inputs to the system are 3 delta angles and 3 delta velocities @@ -482,7 +485,7 @@ void Ekf::predictCovariance() // covariance update - SquareMatrix24f nextP; + SquareMatrix25f nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states @@ -862,6 +865,37 @@ void Ekf::predictCovariance() } + if (_terrain_initialised) { + nextP(0,24) = P(0,24) - P(1,24)*PS11 + P(10,24)*PS6 + P(11,24)*PS7 + P(12,24)*PS9 - P(2,24)*PS12 - P(3,24)*PS13; + nextP(1,24) = P(0,24)*PS11 + P(1,24) - P(10,24)*PS34 + P(11,24)*PS9 - P(12,24)*PS7 + P(2,24)*PS13 - P(3,24)*PS12; + nextP(2,24) = P(0,24)*PS12 - P(1,24)*PS13 - P(10,24)*PS9 - P(11,24)*PS34 + P(12,24)*PS6 + P(2,24) + P(3,24)*PS11; + nextP(3,24) = P(0,24)*PS13 + P(1,24)*PS12 + P(10,24)*PS7 - P(11,24)*PS6 - P(12,24)*PS34 - P(2,24)*PS11 + P(3,24); + nextP(4,24) = P(0,24)*PS174 + P(1,24)*PS173 + P(13,24)*PS43 + P(14,24)*PS172 - P(15,24)*PS171 + P(2,24)*PS175 - P(3,24)*PS176 + P(4,24); + nextP(5,24) = -P(0,24)*PS202 - P(1,24)*PS204 - P(13,24)*PS193 + P(14,24)*PS75 + P(15,24)*PS190 + P(2,24)*PS201 + P(3,24)*PS203 + P(5,24); + nextP(6,24) = P(0,24)*PS216 + P(1,24)*PS217 + P(13,24)*PS199 - P(14,24)*PS197 + P(15,24)*PS87 - P(2,24)*PS214 + P(3,24)*PS215 + P(6,24); + nextP(7,24) = P(4,24)*dt + P(7,24); + nextP(8,24) = P(5,24)*dt + P(8,24); + nextP(9,24) = P(6,24)*dt + P(9,24); + nextP(10,24) = P(10,24); + nextP(11,24) = P(11,24); + nextP(12,24) = P(12,24); + nextP(13,24) = P(13,24); + nextP(14,24) = P(14,24); + nextP(15,24) = P(15,24); + nextP(16,24) = P(16,24); + nextP(17,24) = P(17,24); + nextP(18,24) = P(18,24); + nextP(19,24) = P(19,24); + nextP(20,24) = P(20,24); + nextP(21,24) = P(21,24); + nextP(22,24) = P(22,24); + nextP(23,24) = P(23,24); + nextP(24,24) = P(24,24); + + // add process noise that is not from the IMU + nextP(24, 24) += process_noise(24); + } + // stop position covariance growth if our total position variance reaches 100m // this can happen if we lose gps for some time if ((P(7, 7) + P(8, 8)) > 1e4f) { @@ -898,7 +932,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // and set corresponding entries in Q to zero when states exceed 50% of the limit // Covariance diagonal limits. Use same values for states which // belong to the same group (e.g. vel_x, vel_y, vel_z) - float P_lim[8] = {}; + float P_lim[9] = {}; P_lim[0] = 1.0f; // quaternion max var P_lim[1] = 1e6f; // velocity max var P_lim[2] = 1e6f; // positiion max var @@ -907,6 +941,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) P_lim[5] = 1.0f; // earth mag field max var P_lim[6] = 1.0f; // body mag field max var P_lim[7] = 1e6f; // wind max var + P_lim[8] = 1E4f; // terrain vertical position max variance for (int i = 0; i <= 3; i++) { // quaternion states @@ -1056,11 +1091,25 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) P.makeRowColSymmetric<2>(22); } } + + // terrain vertical position + if (!_terrain_initialised) { + P.uncorrelateCovarianceSetVariance<1>(24, 0.0f); + + } else { + // constrain variances + P(24, 24) = math::constrain(P(24, 24), 0.0f, P_lim[8]); + + // force symmetry + if (force_symmetry) { + P.makeRowColSymmetric<1>(24); + } + } } // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected -bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) +bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix25f &KHP) { bool healthy = true; diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 47cb2d7bbb..5b9f0af1aa 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -46,8 +46,8 @@ void Ekf::fuseDrag(const dragSample &drag_sample) { - SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians - Vector24f Kfusion; // Kalman gain vector + SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians + Vector25f Kfusion; // Kalman gain vector const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2 const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 718c62b14f..8d22606eef 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -51,15 +51,15 @@ class Ekf final : public EstimatorInterface { public: - static constexpr uint8_t _k_num_states{24}; ///< number of EKF states + static constexpr uint8_t _k_num_states{25}; ///< number of EKF states - typedef matrix::Vector Vector24f; - typedef matrix::SquareMatrix SquareMatrix24f; + typedef matrix::Vector Vector25f; + typedef matrix::SquareMatrix SquareMatrix25f; typedef matrix::SquareMatrix Matrix2f; typedef matrix::Vector Vector4f; template - using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; + using SparseVector25f = matrix::SparseVectorf<25, Idxs...>; Ekf() = default; virtual ~Ekf() = default; @@ -125,7 +125,7 @@ public: void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } // get the state vector at the delayed time horizon - matrix::Vector getStateAtFusionHorizonAsVector() const; + matrix::Vector getStateAtFusionHorizonAsVector() const; // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; @@ -137,10 +137,10 @@ public: float getTrueAirspeed() const; // get the full covariance matrix - const matrix::SquareMatrix &covariances() const { return P; } + const matrix::SquareMatrix &covariances() const { return P; } // get the diagonal elements of the covariance matrix - matrix::Vector covariances_diagonal() const { return P.diag(); } + matrix::Vector covariances_diagonal() const { return P.diag(); } // get the orientation (quaterion) covariances matrix::SquareMatrix orientation_covariances() const { return P.slice<4, 4>(0, 0); } @@ -418,7 +418,7 @@ private: bool _non_mag_yaw_aiding_running_prev{false}; ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS). bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited - SquareMatrix24f P{}; ///< state covariance matrix + SquareMatrix25f P{}; ///< state covariance matrix Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance @@ -739,9 +739,9 @@ private: // matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24> // that is optimized by exploring the sparsity in H template - SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f &H) const + SquareMatrix25f computeKHP(const Vector25f &K, const SparseVector25f &H) const { - SquareMatrix24f KHP; + SquareMatrix25f KHP; constexpr size_t non_zeros = sizeof...(Idxs); float KH[non_zeros]; @@ -768,7 +768,7 @@ private: // measurement update with a single measurement // returns true if fusion is performed template - bool measurementUpdate(Vector24f &K, const SparseVector24f &H, float innovation) + bool measurementUpdate(Vector25f &K, const SparseVector25f &H, float innovation) { for (unsigned i = 0; i < 3; i++) { if (_accel_bias_inhibit[i]) { @@ -779,7 +779,7 @@ private: // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(K, H); + const SquareMatrix25f KHP = computeKHP(K, H); const bool is_healthy = checkAndFixCovarianceUpdate(KHP); @@ -798,7 +798,7 @@ private: // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected - bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP); + bool checkAndFixCovarianceUpdate(const SquareMatrix25f &KHP); // limit the diagonal of the covariance matrix // force symmetry when the argument is true @@ -809,7 +809,7 @@ private: // generic function which will perform a fusion step given a kalman gain K // and a scalar innovation value - void fuse(const Vector24f &K, float innovation); + void fuse(const Vector25f &K, float innovation); float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index b59c47a06c..1fdb901675 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -673,9 +673,9 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const } // get the state vector at the delayed time horizon -matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const +matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const { - matrix::Vector state; + matrix::Vector state; state.slice<4, 1>(0, 0) = _state.quat_nominal; state.slice<3, 1>(4, 0) = _state.vel; state.slice<3, 1>(7, 0) = _state.pos; @@ -684,6 +684,7 @@ matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const state.slice<3, 1>(16, 0) = _state.mag_I; state.slice<3, 1>(19, 0) = _state.mag_B; state.slice<2, 1>(22, 0) = _state.wind_vel; + state.slice<1, 1>(24, 0) = _state.posd_terrain; return state; } @@ -986,7 +987,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const *status = soln_status.value; } -void Ekf::fuse(const Vector24f &K, float innovation) +void Ekf::fuse(const Vector25f &K, float innovation) { _state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation; _state.quat_nominal.normalize(); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 317744da17..c852f209bd 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -170,7 +170,7 @@ void Ekf::fuseGpsYaw() // calculate observation jacobian // Observation jacobian and Kalman gain vectors - SparseVector24f<0,1,2,3> Hfusion; + SparseVector25f<0,1,2,3> Hfusion; Hfusion.at<0>() = -HK16*HK18; Hfusion.at<1>() = -HK18*HK24; Hfusion.at<2>() = -HK18*HK25; @@ -178,7 +178,7 @@ void Ekf::fuseGpsYaw() // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion; + Vector25f Kfusion; Kfusion(0) = HK27*HK32; Kfusion(1) = HK28*HK32; Kfusion(2) = HK30*HK32; diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index f967952858..4fbfb9e820 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -196,8 +196,8 @@ void Ekf::fuseMag(const Vector3f &mag) const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); // Observation jacobian and Kalman gain vectors - SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion; - Vector24f Kfusion; + SparseVector25f<0,1,2,3,16,17,18,19,20,21> Hfusion; + Vector25f Kfusion; // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { @@ -623,7 +623,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion; + Vector25f Kfusion; for (uint8_t row = 0; row <= 15; row++) { for (uint8_t col = 0; col <= 3; col++) { @@ -679,7 +679,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP - SquareMatrix24f KHP; + SquareMatrix25f KHP; float KH[4]; for (unsigned row = 0; row < _k_num_states; row++) { @@ -809,12 +809,12 @@ void Ekf::fuseDeclination(float decl_sigma) // Calculate the observation Jacobian // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost // Note Hfusion indices do not match state indices - SparseVector24f<16,17> Hfusion; + SparseVector25f<16,17> Hfusion; Hfusion.at<16>() = -HK0*HK2*magE; Hfusion.at<17>() = HK4; // Calculate the Kalman gains - Vector24f Kfusion; + Vector25f Kfusion; for (unsigned row = 0; row <= 15; row++) { Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17)); diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index d158f90523..b6e82a3bb3 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -260,8 +260,8 @@ void Ekf::fuseOptFlow() } // fuse observation axes sequentially - SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians - Vector24f Kfusion; // Optical flow Kalman gains + SparseVector25f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians + Vector25f Kfusion; // Optical flow Kalman gains for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp index a6fa3d2484..8563dc2993 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp @@ -3,8 +3,8 @@ #include #include "../../../../../matrix/matrix/math.hpp" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; float sq(float in) { return in * in; @@ -15,8 +15,8 @@ int main() // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations float Hfusion[24]; - Vector24f H_MAG; - Vector24f Kfusion; + Vector25f H_MAG; + Vector25f Kfusion; float mag_innov_var; // quaternion inputs must be normalised @@ -38,7 +38,7 @@ int main() const bool update_all_states = true; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { @@ -132,7 +132,7 @@ int main() // save output and repeat calculation using legacy matlab generated code float Hfusion_sympy[24]; - Vector24f Kfusion_sympy; + Vector25f Kfusion_sympy; for (int row=0; row<24; row++) { Hfusion_sympy[row] = Hfusion[row]; Kfusion_sympy(row) = Kfusion(row); @@ -308,7 +308,7 @@ int main() // save output and repeat calculation using legacy matlab generated code float Hfusion_sympy[24]; - Vector24f Kfusion_sympy; + Vector25f Kfusion_sympy; for (int row=0; row<24; row++) { Hfusion_sympy[row] = Hfusion[row]; Kfusion_sympy(row) = Kfusion(row); @@ -488,7 +488,7 @@ int main() // save output and repeat calculation using legacy matlab generated code float Hfusion_sympy[24]; - Vector24f Kfusion_sympy; + Vector25f Kfusion_sympy; for (int row=0; row<24; row++) { Hfusion_sympy[row] = Hfusion[row]; Kfusion_sympy(row) = Kfusion(row); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp index 523bd90654..0b00de27de 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp @@ -4,25 +4,25 @@ #include "../../../../../matrix/matrix/math.hpp" #include "util.h" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; template -using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; +using SparseVector25f = matrix::SparseVectorf<24, Idxs...>; int main() { // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations - SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; - Vector24f H_ACC; - Vector24f Kfusion; + SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion; + Vector25f H_ACC; + Vector25f Kfusion; float drag_innov_var; - Vector24f Hfusion_sympy; - Vector24f Kfusion_sympy; + Vector25f Hfusion_sympy; + Vector25f Kfusion_sympy; - Vector24f Hfusion_matlab; - Vector24f Kfusion_matlab; + Vector25f Hfusion_matlab; + Vector25f Kfusion_matlab; float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector @@ -56,7 +56,7 @@ int main() const float rho = 1.225f; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp index 50940ba03b..aa14dcda6c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp @@ -4,19 +4,19 @@ #include "../../../../../matrix/matrix/math.hpp" #include "util.h" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; template -using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; +using SparseVector25f = matrix::SparseVectorf<24, Idxs...>; int main() { // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations - Vector24f Hfusion_sympy; - Vector24f Kfusion_sympy; + Vector25f Hfusion_sympy; + Vector25f Kfusion_sympy; - Vector24f Hfusion_matlab; - Vector24f Kfusion_matlab; + Vector25f Hfusion_matlab; + Vector25f Kfusion_matlab; float _beta_innov_var; @@ -43,7 +43,7 @@ int main() const float vwe = 5.0f * 2.0f * ((float)rand() - 0.5f); // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { @@ -157,7 +157,7 @@ int main() // } // Observation Jacobians - SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; + SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion; Hfusion.at<0>() = HK21*HK22; Hfusion.at<1>() = HK22*HK25; Hfusion.at<2>() = HK22*HK26; @@ -169,7 +169,7 @@ int main() Hfusion.at<23>() = HK16*HK32; // Calculate Kalman gains - Vector24f Kfusion; + Vector25f Kfusion; if (!update_wind_only) { Kfusion(0) = HK38*HK52; @@ -224,7 +224,7 @@ int main() SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd; SH_BETA[12] = 2.0f*q0*q3; - Vector24f H_BETA; + Vector25f H_BETA; H_BETA(0) = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; H_BETA(1) = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; H_BETA(2) = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; @@ -248,7 +248,7 @@ int main() SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; - Vector24f Kfusion; + Vector25f Kfusion; Kfusion(0) = SK_BETA[0]*(P(0,0)*SK_BETA[5] + P(0,1)*SK_BETA[4] - P(0,4)*SK_BETA[1] + P(0,5)*SK_BETA[2] + P(0,2)*SK_BETA[6] + P(0,6)*SK_BETA[3] - P(0,3)*SK_BETA[7] + P(0,22)*SK_BETA[1] - P(0,23)*SK_BETA[2]); Kfusion(1) = SK_BETA[0]*(P(1,0)*SK_BETA[5] + P(1,1)*SK_BETA[4] - P(1,4)*SK_BETA[1] + P(1,5)*SK_BETA[2] + P(1,2)*SK_BETA[6] + P(1,6)*SK_BETA[3] - P(1,3)*SK_BETA[7] + P(1,22)*SK_BETA[1] - P(1,23)*SK_BETA[2]); Kfusion(2) = SK_BETA[0]*(P(2,0)*SK_BETA[5] + P(2,1)*SK_BETA[4] - P(2,4)*SK_BETA[1] + P(2,5)*SK_BETA[2] + P(2,2)*SK_BETA[6] + P(2,6)*SK_BETA[3] - P(2,3)*SK_BETA[7] + P(2,22)*SK_BETA[1] - P(2,23)*SK_BETA[2]); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp index 102e25921c..8e5d391d9f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp @@ -1,9 +1,9 @@ // Equations for covariance matrix prediction, without process noise! -const float PS0 = powf(q1, 2); +const float PS0 = ecl::powf(q1, 2); const float PS1 = 0.25F*daxVar; -const float PS2 = powf(q2, 2); +const float PS2 = ecl::powf(q2, 2); const float PS3 = 0.25F*dayVar; -const float PS4 = powf(q3, 2); +const float PS4 = ecl::powf(q3, 2); const float PS5 = 0.25F*dazVar; const float PS6 = 0.5F*q1; const float PS7 = 0.5F*q2; @@ -94,7 +94,7 @@ const float PS91 = PS59 + PS82; const float PS92 = PS69 + PS79; const float PS93 = PS49 - 2*PS51 + PS53; const float PS94 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9; -const float PS95 = powf(q0, 2); +const float PS95 = ecl::powf(q0, 2); const float PS96 = -P(10,11)*PS34; const float PS97 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS96; const float PS98 = P(0,2)*PS13; @@ -238,20 +238,20 @@ nextP(0,4) = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS nextP(1,4) = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122; nextP(2,4) = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147; nextP(3,4) = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167; -nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*powf(PS56, 2) + PS185*powf(PS45, 2) + PS186 + powf(PS43, 2)*dvxVar; +nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*ecl::powf(PS56, 2) + PS185*ecl::powf(PS45, 2) + PS186 + ecl::powf(PS43, 2)*dvxVar; nextP(0,5) = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86; nextP(1,5) = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124; nextP(2,5) = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149; nextP(3,5) = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169; nextP(4,5) = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196; -nextP(5,5) = PS185*powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*powf(PS76, 2) + PS213 + powf(PS75, 2)*dvyVar; +nextP(5,5) = PS185*ecl::powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*ecl::powf(PS76, 2) + PS213 + ecl::powf(PS75, 2)*dvyVar; nextP(0,6) = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94; nextP(1,6) = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125; nextP(2,6) = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150; nextP(3,6) = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170; nextP(4,6) = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200; nextP(5,6) = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218; -nextP(6,6) = PS184*powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + powf(PS87, 2)*dvzVar; +nextP(6,6) = PS184*ecl::powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*ecl::powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + ecl::powf(PS87, 2)*dvzVar; nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS73*dt; nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS122*dt; nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS147*dt; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated_compare.cpp index 6d43e457e3..3ec761bd9a 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated_compare.cpp @@ -3,7 +3,7 @@ #include #include "../../../../../matrix/matrix/math.hpp" -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::SquareMatrix SquareMatrix25f; inline float sq(float in) { return in * in; @@ -73,7 +73,7 @@ int main() const float dvzVar = dvxVar; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { @@ -273,7 +273,7 @@ int main() const float PS185 = P(0,13)*PS60 + P(1,13)*PS74 + P(13,13)*PS166 - P(13,14)*PS165 - P(13,15)*PS70 - P(2,13)*PS72 + P(3,13)*PS62 + P(6,13); const float PS186 = P(0,6)*PS60 + P(1,6)*PS74 - P(2,6)*PS72 + P(3,6)*PS62 + P(6,13)*PS166 - P(6,14)*PS165 - P(6,15)*PS70 + P(6,6); - SquareMatrix24f nextP; + SquareMatrix25f nextP; nextP(0,0) = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5; nextP(0,1) = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5; @@ -577,7 +577,7 @@ int main() nextP(23,23) = P(23,23); // save output and repeat calculation using legacy matlab generated code - SquareMatrix24f nextP_sympy; + SquareMatrix25f nextP_sympy; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { nextP_sympy(row,col) = nextP(row,col); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp index 227a4a5422..c277c624f1 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp @@ -4,24 +4,24 @@ #include "../../../../../matrix/matrix/math.hpp" #include "util.h" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; template -using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; +using SparseVector25f = matrix::SparseVectorf<24, Idxs...>; int main() { // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations - SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians - Vector24f Kfusion; // Optical flow observation Kalman gains + SparseVector25f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians + Vector25f Kfusion; // Optical flow observation Kalman gains - Vector24f Hfusion_sympy; - Vector24f Kfusion_sympy; + Vector25f Hfusion_sympy; + Vector25f Kfusion_sympy; - Vector24f Hfusion_matlab; - Vector24f Kfusion_matlab; + Vector25f Hfusion_matlab; + Vector25f Kfusion_matlab; const float R_LOS = sq(0.15f); @@ -49,7 +49,7 @@ int main() Tbs.identity(); // create a symmetrical positive definite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp index 11c4a3d547..bff7172d04 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp @@ -3,10 +3,10 @@ #include #include "../../../../../matrix/matrix/math.hpp" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; template -using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; +using SparseVector25f = matrix::SparseVectorf<24, Idxs...>; float sq(float in) { return in * in; @@ -17,7 +17,7 @@ int main() // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations float H_YAW[24]; - Vector24f Kfusion; + Vector25f Kfusion; float _heading_innov_var; const float R_YAW = sq(0.3f); @@ -36,7 +36,7 @@ int main() q3 /= length; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { @@ -97,7 +97,7 @@ int main() // calculate observation jacobian // Observation jacobian and Kalman gain vectors - SparseVector24f<0,1,2,3> Hfusion; + SparseVector25f<0,1,2,3> Hfusion; Hfusion.at<0>() = -HK16*HK18; Hfusion.at<1>() = -HK18*HK24; Hfusion.at<2>() = -HK18*HK25; @@ -115,7 +115,7 @@ int main() // save output and repeat calculation using legacy matlab generated code float Hfusion_sympy[24] = {}; - Vector24f Kfusion_sympy; + Vector25f Kfusion_sympy; Hfusion_sympy[0] = Hfusion.at<0>(); Hfusion_sympy[1] = Hfusion.at<1>(); Hfusion_sympy[2] = Hfusion.at<2>(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp index 6972e1ad1f..e3bf1d7350 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp @@ -3,8 +3,8 @@ #include #include "../../../../../matrix/matrix/math.hpp" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; float sq(float in) { return in * in; @@ -15,8 +15,8 @@ int main() // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations float Hfusion[24] = {}; - Vector24f H_DECL; - Vector24f Kfusion; + Vector25f H_DECL; + Vector25f Kfusion; float decl_innov_var; const float R_DECL = sq(0.3f); @@ -40,7 +40,7 @@ int main() const float h_field_min = 1e-3f; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { @@ -95,7 +95,7 @@ int main() // save output and repeat calculation using legacy matlab generated code float Hfusion_sympy[24]; - Vector24f Kfusion_sympy; + Vector25f Kfusion_sympy; for (int row=0; row<24; row++) { Hfusion_sympy[row] = Hfusion[row]; Kfusion_sympy(row) = Kfusion(row); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp index aa1d05c8e9..a18fa12f8b 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp @@ -4,10 +4,10 @@ #include "../../../../../matrix/matrix/math.hpp" #include "util.h" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; template -using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; +using SparseVector25f = matrix::SparseVectorf<24, Idxs...>; int main() { @@ -15,13 +15,13 @@ int main() float airspeed_innov_var; - Vector24f Kfusion; // Kalman gain vector + Vector25f Kfusion; // Kalman gain vector - Vector24f Hfusion_sympy; - Vector24f Kfusion_sympy; + Vector25f Hfusion_sympy; + Vector25f Kfusion_sympy; - Vector24f Hfusion_matlab; - Vector24f Kfusion_matlab; + Vector25f Hfusion_matlab; + Vector25f Kfusion_matlab; const float R_TAS = sq(1.5f); @@ -37,7 +37,7 @@ int main() const float vwe = 3.0f; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; + SquareMatrix25f P; for (int col=0; col<=23; col++) { for (int row=0; row<=col; row++) { if (row == col) { @@ -77,7 +77,7 @@ int main() const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS); // Observation Jacobians - SparseVector24f<4,5,6,22,23> Hfusion; + SparseVector25f<4,5,6,22,23> Hfusion; Hfusion.at<4>() = HK4; Hfusion.at<5>() = HK5; Hfusion.at<6>() = HK3*vd; @@ -121,7 +121,7 @@ int main() SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; // Observation Jacobian - Vector24f H_TAS = {}; + Vector25f H_TAS = {}; H_TAS(4) = SH_TAS[2]; H_TAS(5) = SH_TAS[1]; H_TAS(6) = vd*SH_TAS[0]; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h index 36ba9f4ffc..1a1d574cb4 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h @@ -1,6 +1,6 @@ #include "../../../../../matrix/matrix/math.hpp" -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::SquareMatrix SquareMatrix25f; inline float sq(float in) { return in * in; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp index 4be87f85d9..001098deb3 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp @@ -3,8 +3,8 @@ #include #include "../../../../../matrix/matrix/math.hpp" -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; +typedef matrix::Vector Vector25f; +typedef matrix::SquareMatrix SquareMatrix25f; float sq(float in) { return in * in; diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 8179413de0..df90c33fc8 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -173,7 +173,7 @@ void Ekf::fuseSideslip() } // Observation Jacobians - SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; + SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion; Hfusion.at<0>() = HK21*HK22; Hfusion.at<1>() = HK22*HK25; Hfusion.at<2>() = HK22*HK26; @@ -185,7 +185,7 @@ void Ekf::fuseSideslip() Hfusion.at<23>() = HK16*HK32; // Calculate Kalman gains - Vector24f Kfusion; + Vector25f Kfusion; if (!update_wind_only) { diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 064165b160..20b66376db 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -183,7 +183,7 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) { - Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. + Vector25f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state // calculate kalman gain K = PHS, where S = 1/innovation variance @@ -191,7 +191,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o Kfusion(row) = P(row, state_index) / innov_var; } - SquareMatrix24f KHP; + SquareMatrix25f KHP; for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) {