From c44488fdb85833d7f17efcd604f54732b6287a83 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 19 Jun 2017 11:10:01 -0400 Subject: [PATCH] EKF matrix typedef cleanup --- EKF/airspeed_fusion.cpp | 3 +-- EKF/common.h | 22 +++++++++++++--------- EKF/control.cpp | 13 ++++++------- EKF/drag_fusion.cpp | 2 +- EKF/ekf.cpp | 16 ++++++++-------- EKF/ekf.h | 6 +++--- EKF/ekf_helper.cpp | 22 +++++++++++----------- EKF/estimator_interface.h | 2 +- EKF/mag_fusion.cpp | 18 +++++++++--------- EKF/optflow_fusion.cpp | 2 +- EKF/sideslip_fusion.cpp | 2 +- 11 files changed, 55 insertions(+), 53 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index c7c44a186e..3adae54bb8 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -231,7 +231,7 @@ void Ekf::get_wind_velocity(float *wind) void Ekf::resetWindStates() { // get euler yaw angle - matrix::Euler euler321(_state.quat_nominal); + Eulerf euler321(_state.quat_nominal); float euler_yaw = euler321(2); if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < 5e5)) { @@ -243,6 +243,5 @@ void Ekf::resetWindStates() // If we don't have an airspeed measurement, then assume the wind is zero _state.wind_vel(0) = 0.0f; _state.wind_vel(1) = 0.0f; - } } diff --git a/EKF/common.h b/EKF/common.h index 022545732f..40c721c3d4 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -42,6 +42,15 @@ namespace estimator { + +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Matrix3f; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector3f; +using matrix::wrap_pi; + struct gps_message { uint64_t time_usec; int32_t lat; // Latitude in 1E-7 degrees @@ -58,11 +67,6 @@ struct gps_message { float gdop; // geometric dilution of precision }; -typedef matrix::Vector Vector2f; -typedef matrix::Vector Vector3f; -typedef matrix::Quaternion Quaternion; -typedef matrix::Matrix Matrix3f; - struct flow_message { uint8_t quality; // Quality of Flow data Vector2f flowdata; // Flow data received @@ -72,13 +76,13 @@ struct flow_message { struct ext_vision_message { Vector3f posNED; // measured NED position relative to the local origin (m) - Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame + Quatf quat; // measured quaternion orientation defining rotation from NED to body frame float posErr; // 1-Sigma spherical position accuracy (m) float angErr; // 1-Sigma angular error (rad) }; struct outputSample { - Quaternion quat_nominal; // nominal quaternion describing vehicle attitude + Quatf quat_nominal; // nominal quaternion describing vehicle attitude Vector3f vel; // NED velocity estimate in earth frame in m/s Vector3f pos; // NED position estimate in earth frame in m/s uint64_t time_us; // timestamp in microseconds @@ -141,7 +145,7 @@ struct flowSample { struct extVisionSample { Vector3f posNED; // measured NED position relative to the local origin (m) - Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame + Quatf quat; // measured quaternion orientation defining rotation from NED to body frame float posErr; // 1-Sigma spherical position accuracy (m) float angErr; // 1-Sigma angular error (rad) uint64_t time_us; // timestamp of the measurement in microseconds @@ -311,7 +315,7 @@ struct parameters { }; struct stateSample { - Quaternion quat_nominal; // quaternion defining the rotaton from earth to body frame + Quatf quat_nominal; // quaternion defining the rotaton from earth to body frame Vector3f vel; // NED velocity in earth frame in m/s Vector3f pos; // NED position in earth frame in m Vector3f gyro_bias; // delta angle bias estimate in rad diff --git a/EKF/control.cpp b/EKF/control.cpp index 1f2a7b1903..4f8794cdd2 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -148,21 +148,20 @@ void Ekf::controlExternalVisionFusion() if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observaton quaternion // get the roll, pitch, yaw estimates from the quaternion states - matrix::Quaternion q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), - _state.quat_nominal(3)); - matrix::Euler euler_init(q_init); + Quatf q_init(_state.quat_nominal); + Eulerf euler_init(q_init); // get initial yaw from the observation quaternion extVisionSample ev_newest = _ext_vision_buffer.get_newest(); - matrix::Quaternion q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); - matrix::Euler euler_obs(q_obs); + Quatf q_obs(ev_newest.quat); + Eulerf euler_obs(q_obs); euler_init(2) = euler_obs(2); // save a copy of the quaternion state for later use in calculating the amount of reset change - Quaternion quat_before_reset = _state.quat_nominal; + Quatf quat_before_reset = _state.quat_nominal; // calculate initial quaternion states for the ekf - _state.quat_nominal = Quaternion(euler_init); + _state.quat_nominal = Quatf(euler_init); // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 4ce84402f4..7e6a173720 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -80,7 +80,7 @@ void Ekf::fuseDrag() rel_wind(0) = vn - vwn; rel_wind(1) = ve - vwe; rel_wind(2) = vd; - matrix::Dcm earth_to_body(_state.quat_nominal); + Dcmf earth_to_body(_state.quat_nominal); earth_to_body = earth_to_body.transpose(); rel_wind = earth_to_body * rel_wind; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2c35687f86..49eaa5d45c 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -299,8 +299,8 @@ bool Ekf::initialiseFilter() } // calculate initial tilt alignment - matrix::Euler euler_init(roll, pitch, 0.0f); - _state.quat_nominal = Quaternion(euler_init); + Eulerf euler_init(roll, pitch, 0.0f); + _state.quat_nominal = Quatf(euler_init); _output_new.quat_nominal = _state.quat_nominal; // update transformation matrix from body to world frame @@ -364,7 +364,7 @@ void Ekf::predictState() corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; // convert the delta angle to a delta quaternion - Quaternion dq; + Quatf dq; dq.from_axis_angle(corrected_delta_ang); // rotate the previous quaternion by the delta quaternion using a quaternion multiplication @@ -424,13 +424,13 @@ bool Ekf::collect_imu(imuSample &imu) // use a quaternion to accumulate delta angle data // this quaternion represents the rotation from the start to end of the accumulation period - Quaternion delta_q; + Quatf delta_q; delta_q.rotate(imu.delta_ang); _q_down_sampled = _q_down_sampled * delta_q; _q_down_sampled.normalize(); // rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame - matrix::Dcm delta_R(delta_q.inversed()); + Dcmf delta_R(delta_q.inversed()); _imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel; // accumulate the most recent delta velocity data at the updated rotation frame @@ -505,7 +505,7 @@ void Ekf::calculateOutputStates() delta_angle += _delta_angle_corr; // convert the delta angle to an equivalent delta quaternions - Quaternion dq; + Quatf dq; dq.from_axis_angle(delta_angle); // rotate the previous INS quaternion by the delta quaternions @@ -562,8 +562,8 @@ void Ekf::calculateOutputStates() _output_vert_delayed = _output_vert_buffer.get_oldest(); // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon - Quaternion quat_inv = _state.quat_nominal.inversed(); - Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; + Quatf quat_inv = _state.quat_nominal.inversed(); + Quatf q_error = _output_sample_delayed.quat_nominal * quat_inv; q_error.normalize(); // convert the quaternion delta to a delta angle diff --git a/EKF/ekf.h b/EKF/ekf.h index 017ae3a449..e8504a08d3 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -222,7 +222,7 @@ private: float velD_change; // Down velocity change due to last reset (m/s) Vector2f posNE_change; // North, East position change due to last reset (m) float posD_change; // Down position change due to last reset (m) - Quaternion quat_change; // quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion + Quatf quat_change; // quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion } _state_reset_status{}; float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; // average update rate of the ekf @@ -264,7 +264,7 @@ private: Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s - matrix::Dcm _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition + Dcmf _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition // used by magnetometer fusion mode selection Vector2f _accel_lpf_NE; // Low pass filtered horizontal earth frame acceleration (m/s**2) @@ -306,7 +306,7 @@ private: // output predictor states Vector3f _delta_angle_corr; // delta angle correction vector imuSample _imu_down_sampled{}; // down sampled imu data (sensor rate -> filter update rate) - Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) + Quatf _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) Vector3f _vel_err_integ; // integral of velocity tracking error Vector3f _pos_err_integ; // integral of position tracking error float _output_tracking_error[3] {}; // contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 47122d2e81..2e8f4c9f34 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -310,8 +310,8 @@ void Ekf::resetHeight() void Ekf::alignOutputFilter() { // calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon - Quaternion quat_inv = _state.quat_nominal.inversed(); - Quaternion q_delta = _output_sample_delayed.quat_nominal * quat_inv; + Quatf quat_inv = _state.quat_nominal.inversed(); + Quatf q_delta = _output_sample_delayed.quat_nominal * quat_inv; q_delta.normalize(); // calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon @@ -336,7 +336,7 @@ void Ekf::alignOutputFilter() bool Ekf::resetMagHeading(Vector3f &mag_init) { // save a copy of the quaternion state for later use in calculating the amount of reset change - Quaternion quat_before_reset = _state.quat_nominal; + Quatf quat_before_reset = _state.quat_nominal; // calculate the variance for the rotation estimate expressed as a rotation vector // this will be used later to reset the quaternion state covariances @@ -351,16 +351,16 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // use a 321 sequence // rotate the magnetometer measurement into earth frame - matrix::Euler euler321(_state.quat_nominal); + Eulerf euler321(_state.quat_nominal); // Set the yaw angle to zero and calculate the rotation matrix from body to earth frame euler321(2) = 0.0f; - matrix::Dcm R_to_earth(euler321); + Dcmf R_to_earth(euler321); // calculate the observed yaw angle if (_params.fusion_mode & MASK_USE_EVYAW) { // convert the observed quaternion to a rotation matrix - matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); @@ -377,7 +377,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate initial quaternion states for the ekf // we don't change the output attitude to avoid jumps - _state.quat_nominal = Quaternion(euler321); + _state.quat_nominal = Quatf(euler321); } else { // use a 312 sequence @@ -400,7 +400,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) float s0 = sinf(euler312(0)); float c0 = cosf(euler312(0)); - matrix::Dcm R_to_earth; + Dcmf R_to_earth; R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2; R_to_earth(1, 1) = c0 * c1; R_to_earth(2, 2) = c2 * c1; @@ -414,7 +414,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate the observed yaw angle if (_params.fusion_mode & MASK_USE_EVYAW) { // convert the observed quaternion to a rotation matrix - matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); @@ -444,7 +444,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate initial quaternion states for the ekf // we don't change the output attitude to avoid jumps - _state.quat_nominal = Quaternion(R_to_earth); + _state.quat_nominal = Quatf(R_to_earth); } // update transformation matrix from body to world frame using the current estimate @@ -976,7 +976,7 @@ Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3 } // calculate the inverse rotation matrix from a quaternion rotation -Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion &quat) +Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat) { float q00 = quat(0) * quat(0); float q11 = quat(1) * quat(1); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index a8e61effc1..2b2e59f949 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -417,6 +417,6 @@ protected: Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2); // calculate the inverse rotation matrix from a quaternion rotation - Matrix3f quat_to_invrotmat(const Quaternion &quat); + Matrix3f quat_to_invrotmat(const Quatf &quat); }; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 3835082f4a..8357629e88 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -72,7 +72,7 @@ void Ekf::fuseMag() SH_MAG[8] = 2.0f*magE*q3; // rotate magnetometer earth field state into body frame - matrix::Dcm R_to_body(_state.quat_nominal); + Dcmf R_to_body(_state.quat_nominal); R_to_body = R_to_body.transpose(); Vector3f mag_I_rot = R_to_body * _state.mag_I; @@ -394,7 +394,7 @@ void Ekf::fuseHeading() float R_YAW = 1.0f; float predicted_hdg; float H_YAW[4]; - matrix::Vector3f mag_earth_pred; + Vector3f mag_earth_pred; float measured_hdg; // determine if a 321 or 312 Euler sequence is best @@ -430,12 +430,12 @@ void Ekf::fuseHeading() H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; // rotate the magnetometer measurement into earth frame - matrix::Euler euler321(_state.quat_nominal); + Eulerf euler321(_state.quat_nominal); predicted_hdg = euler321(2); // we will need the predicted heading to calculate the innovation // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle euler321(2) = 0.0f; - matrix::Dcm R_to_earth(euler321); + Dcmf R_to_earth(euler321); // calculate the observed yaw angle if (_control_status.flags.mag_hdg) { @@ -445,7 +445,7 @@ void Ekf::fuseHeading() measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; } else if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix - matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence measured_hdg = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0)); } else { @@ -504,7 +504,7 @@ void Ekf::fuseHeading() float s0 = sinf(euler312(0)); float c0 = cosf(euler312(0)); - matrix::Dcm R_to_earth; + Dcmf R_to_earth; R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2; R_to_earth(1, 1) = c0 * c1; R_to_earth(2, 2) = c2 * c1; @@ -523,7 +523,7 @@ void Ekf::fuseHeading() measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; } else if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix - matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence measured_hdg = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1)); } else { @@ -603,13 +603,13 @@ void Ekf::fuseHeading() } // wrap the heading to the interval between +-pi - measured_hdg = matrix::wrap_pi(measured_hdg); + measured_hdg = wrap_pi(measured_hdg); // calculate the innovation _heading_innov = predicted_hdg - measured_hdg; // wrap the innovation to the interval between +-pi - _heading_innov = matrix::wrap_pi(_heading_innov); + _heading_innov = wrap_pi(_heading_innov); // innovation test ratio _yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 1e88d6ab7c..ee01a301ec 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -69,7 +69,7 @@ void Ekf::fuseOptFlow() float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance); // get rotation nmatrix from earth to body - matrix::Dcm earth_to_body(_state.quat_nominal); + Dcmf earth_to_body(_state.quat_nominal); earth_to_body = earth_to_body.transpose(); // calculate the sensor position relative to the IMU diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 5173816f55..3604f3269a 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -72,7 +72,7 @@ void Ekf::fuseSideslip() rel_wind(1) = ve - vwe; rel_wind(2) = vd; - matrix::Dcm earth_to_body(_state.quat_nominal); + Dcmf earth_to_body(_state.quat_nominal); earth_to_body = earth_to_body.transpose(); //Why transpose? // rotate into body axes