fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #283 from PX4/ekf_matrix_cleanup

Browse Source 
EKF matrix typedef cleanup
This commit is contained in:
Paul Riseborough
2017-07-03 21:02:24 +10:00
committed by GitHub
parent ae118940b1 c44488fdb8
commit eb1e73ec81
11 changed files with 55 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+1 -2
View File
@@ -231,7 +231,7 @@ void Ekf::get_wind_velocity(float *wind)
void Ekf::resetWindStates() void Ekf::resetWindStates()
{ {
// get euler yaw angle // get euler yaw angle
matrix::Euler<float> euler321(_state.quat_nominal); Eulerf euler321(_state.quat_nominal);
float euler_yaw = euler321(2); float euler_yaw = euler321(2);
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < 5e5)) { 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 // If we don't have an airspeed measurement, then assume the wind is zero
_state.wind_vel(0) = 0.0f; _state.wind_vel(0) = 0.0f;
_state.wind_vel(1) = 0.0f; _state.wind_vel(1) = 0.0f;
} }
} }
EKF/common.h
+13 -9
View File
@@ -42,6 +42,15 @@
namespace estimator 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 { struct gps_message {
uint64_t time_usec; uint64_t time_usec;
int32_t lat; // Latitude in 1E-7 degrees int32_t lat; // Latitude in 1E-7 degrees
@@ -58,11 +67,6 @@ struct gps_message {
float gdop; // geometric dilution of precision float gdop; // geometric dilution of precision
}; };
typedef matrix::Vector<float, 2> Vector2f;
typedef matrix::Vector<float, 3> Vector3f;
typedef matrix::Quaternion<float> Quaternion;
typedef matrix::Matrix<float, 3, 3> Matrix3f;
struct flow_message { struct flow_message {
uint8_t quality; // Quality of Flow data uint8_t quality; // Quality of Flow data
Vector2f flowdata; // Flow data received Vector2f flowdata; // Flow data received
@@ -72,13 +76,13 @@ struct flow_message {
struct ext_vision_message { struct ext_vision_message {
Vector3f posNED; // measured NED position relative to the local origin (m) 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 posErr; // 1-Sigma spherical position accuracy (m)
float angErr; // 1-Sigma angular error (rad) float angErr; // 1-Sigma angular error (rad)
}; };
struct outputSample { 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 vel; // NED velocity estimate in earth frame in m/s
Vector3f pos; // NED position 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 uint64_t time_us; // timestamp in microseconds
@@ -141,7 +145,7 @@ struct flowSample {
struct extVisionSample { struct extVisionSample {
Vector3f posNED; // measured NED position relative to the local origin (m) 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 posErr; // 1-Sigma spherical position accuracy (m)
float angErr; // 1-Sigma angular error (rad) float angErr; // 1-Sigma angular error (rad)
uint64_t time_us; // timestamp of the measurement in microseconds uint64_t time_us; // timestamp of the measurement in microseconds
@@ -318,7 +322,7 @@ struct parameters {
}; };
struct stateSample { 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 vel; // NED velocity in earth frame in m/s
Vector3f pos; // NED position in earth frame in m Vector3f pos; // NED position in earth frame in m
Vector3f gyro_bias; // delta angle bias estimate in rad Vector3f gyro_bias; // delta angle bias estimate in rad
EKF/control.cpp
+6 -7
View File
@@ -159,21 +159,20 @@ void Ekf::controlExternalVisionFusion()
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion // reset the yaw angle to the value from the observaton quaternion
// get the roll, pitch, yaw estimates from the quaternion states // get the roll, pitch, yaw estimates from the quaternion states
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), Quatf q_init(_state.quat_nominal);
_state.quat_nominal(3)); Eulerf euler_init(q_init);
matrix::Euler<float> euler_init(q_init);
// get initial yaw from the observation quaternion // get initial yaw from the observation quaternion
extVisionSample ev_newest = _ext_vision_buffer.get_newest(); extVisionSample ev_newest = _ext_vision_buffer.get_newest();
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); Quatf q_obs(ev_newest.quat);
matrix::Euler<float> euler_obs(q_obs); Eulerf euler_obs(q_obs);
euler_init(2) = euler_obs(2); euler_init(2) = euler_obs(2);
// save a copy of the quaternion state for later use in calculating the amount of reset change // 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 // 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 // calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
EKF/drag_fusion.cpp
+1 -1
View File
@@ -80,7 +80,7 @@ void Ekf::fuseDrag()
rel_wind(0) = vn - vwn; rel_wind(0) = vn - vwn;
rel_wind(1) = ve - vwe; rel_wind(1) = ve - vwe;
rel_wind(2) = vd; rel_wind(2) = vd;
matrix::Dcm<float> earth_to_body(_state.quat_nominal); Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose(); earth_to_body = earth_to_body.transpose();
rel_wind = earth_to_body * rel_wind; rel_wind = earth_to_body * rel_wind;
EKF/ekf.cpp
+8 -8
View File
@@ -275,8 +275,8 @@ bool Ekf::initialiseFilter()
} }
// calculate initial tilt alignment // calculate initial tilt alignment
matrix::Euler<float> euler_init(roll, pitch, 0.0f); Eulerf euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init); _state.quat_nominal = Quatf(euler_init);
_output_new.quat_nominal = _state.quat_nominal; _output_new.quat_nominal = _state.quat_nominal;
// update transformation matrix from body to world frame // update transformation matrix from body to world frame
@@ -340,7 +340,7 @@ void Ekf::predictState()
corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
// convert the delta angle to a delta quaternion // convert the delta angle to a delta quaternion
Quaternion dq; Quatf dq;
dq.from_axis_angle(corrected_delta_ang); dq.from_axis_angle(corrected_delta_ang);
// rotate the previous quaternion by the delta quaternion using a quaternion multiplication // rotate the previous quaternion by the delta quaternion using a quaternion multiplication
@@ -400,13 +400,13 @@ bool Ekf::collect_imu(imuSample &imu)
// use a quaternion to accumulate delta angle data // use a quaternion to accumulate delta angle data
// this quaternion represents the rotation from the start to end of the accumulation period // 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); delta_q.rotate(imu.delta_ang);
_q_down_sampled = _q_down_sampled * delta_q; _q_down_sampled = _q_down_sampled * delta_q;
_q_down_sampled.normalize(); _q_down_sampled.normalize();
// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame // rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
matrix::Dcm<float> delta_R(delta_q.inversed()); Dcmf delta_R(delta_q.inversed());
_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel; _imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
// accumulate the most recent delta velocity data at the updated rotation frame // accumulate the most recent delta velocity data at the updated rotation frame
@@ -481,7 +481,7 @@ void Ekf::calculateOutputStates()
delta_angle += _delta_angle_corr; delta_angle += _delta_angle_corr;
// convert the delta angle to an equivalent delta quaternions // convert the delta angle to an equivalent delta quaternions
Quaternion dq; Quatf dq;
dq.from_axis_angle(delta_angle); dq.from_axis_angle(delta_angle);
// rotate the previous INS quaternion by the delta quaternions // rotate the previous INS quaternion by the delta quaternions
@@ -538,8 +538,8 @@ void Ekf::calculateOutputStates()
_output_vert_delayed = _output_vert_buffer.get_oldest(); _output_vert_delayed = _output_vert_buffer.get_oldest();
// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
Quaternion quat_inv = _state.quat_nominal.inversed(); Quatf quat_inv = _state.quat_nominal.inversed();
Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; Quatf q_error = _output_sample_delayed.quat_nominal * quat_inv;
q_error.normalize(); q_error.normalize();
// convert the quaternion delta to a delta angle // convert the quaternion delta to a delta angle
EKF/ekf.h
+3 -3
View File
@@ -222,7 +222,7 @@ private:
float velD_change; // Down velocity change due to last reset (m/s) float velD_change; // Down velocity change due to last reset (m/s)
Vector2f posNE_change; // North, East position change due to last reset (m) Vector2f posNE_change; // North, East position change due to last reset (m)
float posD_change; // Down 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{}; } _state_reset_status{};
float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; // average update rate of the ekf float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; // average update rate of the ekf
@@ -265,7 +265,7 @@ private:
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
matrix::Dcm<float> _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 // used by magnetometer fusion mode selection
Vector2f _accel_lpf_NE; // Low pass filtered horizontal earth frame acceleration (m/s**2) Vector2f _accel_lpf_NE; // Low pass filtered horizontal earth frame acceleration (m/s**2)
@@ -307,7 +307,7 @@ private:
// output predictor states // output predictor states
Vector3f _delta_angle_corr; // delta angle correction vector Vector3f _delta_angle_corr; // delta angle correction vector
imuSample _imu_down_sampled{}; // down sampled imu data (sensor rate -> filter update rate) 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 _vel_err_integ; // integral of velocity tracking error
Vector3f _pos_err_integ; // integral of position 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) float _output_tracking_error[3] {}; // contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
EKF/ekf_helper.cpp
+11 -11
View File
@@ -314,8 +314,8 @@ void Ekf::resetHeight()
void Ekf::alignOutputFilter() void Ekf::alignOutputFilter()
{ {
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon // calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
Quaternion quat_inv = _state.quat_nominal.inversed(); Quatf quat_inv = _state.quat_nominal.inversed();
Quaternion q_delta = _output_sample_delayed.quat_nominal * quat_inv; Quatf q_delta = _output_sample_delayed.quat_nominal * quat_inv;
q_delta.normalize(); q_delta.normalize();
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon // calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
@@ -340,7 +340,7 @@ void Ekf::alignOutputFilter()
bool Ekf::resetMagHeading(Vector3f &mag_init) bool Ekf::resetMagHeading(Vector3f &mag_init)
{ {
// save a copy of the quaternion state for later use in calculating the amount of reset change // 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 // calculate the variance for the rotation estimate expressed as a rotation vector
// this will be used later to reset the quaternion state covariances // this will be used later to reset the quaternion state covariances
@@ -355,16 +355,16 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// use a 321 sequence // use a 321 sequence
// rotate the magnetometer measurement into earth frame // rotate the magnetometer measurement into earth frame
matrix::Euler<float> 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 // Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
euler321(2) = 0.0f; euler321(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler321); Dcmf R_to_earth(euler321);
// calculate the observed yaw angle // calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) { if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix // convert the observed quaternion to a rotation matrix
matrix::Dcm<float> 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 // calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
@@ -381,7 +381,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// calculate initial quaternion states for the ekf // calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps // we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler321); _state.quat_nominal = Quatf(euler321);
} else { } else {
// use a 312 sequence // use a 312 sequence
@@ -404,7 +404,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
float s0 = sinf(euler312(0)); float s0 = sinf(euler312(0));
float c0 = cosf(euler312(0)); float c0 = cosf(euler312(0));
matrix::Dcm<float> R_to_earth; Dcmf R_to_earth;
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2; R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1; R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1; R_to_earth(2, 2) = c2 * c1;
@@ -418,7 +418,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// calculate the observed yaw angle // calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) { if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix // convert the observed quaternion to a rotation matrix
matrix::Dcm<float> 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 // calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
@@ -448,7 +448,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// calculate initial quaternion states for the ekf // calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps // 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 // update transformation matrix from body to world frame using the current estimate
@@ -980,7 +980,7 @@ Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3
} }
// calculate the inverse rotation matrix from a quaternion rotation // 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 q00 = quat(0) * quat(0);
float q11 = quat(1) * quat(1); float q11 = quat(1) * quat(1);
EKF/estimator_interface.h
+1 -1
View File
@@ -420,6 +420,6 @@ protected:
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2); Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2);
// calculate the inverse rotation matrix from a quaternion rotation // calculate the inverse rotation matrix from a quaternion rotation
Matrix3f quat_to_invrotmat(const Quaternion &quat); Matrix3f quat_to_invrotmat(const Quatf &quat);
}; };
EKF/mag_fusion.cpp
+9 -9
View File
@@ -72,7 +72,7 @@ void Ekf::fuseMag()
SH_MAG[8] = 2.0f*magE*q3; SH_MAG[8] = 2.0f*magE*q3;
// rotate magnetometer earth field state into body frame // rotate magnetometer earth field state into body frame
matrix::Dcm<float> R_to_body(_state.quat_nominal); Dcmf R_to_body(_state.quat_nominal);
R_to_body = R_to_body.transpose(); R_to_body = R_to_body.transpose();
Vector3f mag_I_rot = R_to_body * _state.mag_I; Vector3f mag_I_rot = R_to_body * _state.mag_I;
@@ -421,7 +421,7 @@ void Ekf::fuseHeading()
float R_YAW = 1.0f; float R_YAW = 1.0f;
float predicted_hdg; float predicted_hdg;
float H_YAW[4]; float H_YAW[4];
matrix::Vector3f mag_earth_pred; Vector3f mag_earth_pred;
float measured_hdg; float measured_hdg;
// determine if a 321 or 312 Euler sequence is best // determine if a 321 or 312 Euler sequence is best
@@ -457,12 +457,12 @@ void Ekf::fuseHeading()
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; 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 // rotate the magnetometer measurement into earth frame
matrix::Euler<float> euler321(_state.quat_nominal); Eulerf euler321(_state.quat_nominal);
predicted_hdg = euler321(2); // we will need the predicted heading to calculate the innovation 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 // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
euler321(2) = 0.0f; euler321(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler321); Dcmf R_to_earth(euler321);
// calculate the observed yaw angle // calculate the observed yaw angle
if (_control_status.flags.mag_hdg) { if (_control_status.flags.mag_hdg) {
@@ -472,7 +472,7 @@ void Ekf::fuseHeading()
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else if (_control_status.flags.ev_yaw) { } else if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix // convert the observed quaternion to a rotation matrix
matrix::Dcm<float> 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 // calculate the yaw angle for a 312 sequence
measured_hdg = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0)); measured_hdg = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
} else { } else {
@@ -531,7 +531,7 @@ void Ekf::fuseHeading()
float s0 = sinf(euler312(0)); float s0 = sinf(euler312(0));
float c0 = cosf(euler312(0)); float c0 = cosf(euler312(0));
matrix::Dcm<float> R_to_earth; Dcmf R_to_earth;
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2; R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1; R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1; R_to_earth(2, 2) = c2 * c1;
@@ -550,7 +550,7 @@ void Ekf::fuseHeading()
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else if (_control_status.flags.ev_yaw) { } else if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix // convert the observed quaternion to a rotation matrix
matrix::Dcm<float> 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 // calculate the yaw angle for a 312 sequence
measured_hdg = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1)); measured_hdg = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
} else { } else {
@@ -630,13 +630,13 @@ void Ekf::fuseHeading()
} }
// wrap the heading to the interval between +-pi // wrap the heading to the interval between +-pi
measured_hdg = matrix::wrap_pi(measured_hdg); measured_hdg = wrap_pi(measured_hdg);
// calculate the innovation // calculate the innovation
_heading_innov = predicted_hdg - measured_hdg; _heading_innov = predicted_hdg - measured_hdg;
// wrap the innovation to the interval between +-pi // wrap the innovation to the interval between +-pi
_heading_innov = matrix::wrap_pi(_heading_innov); _heading_innov = wrap_pi(_heading_innov);
// innovation test ratio // innovation test ratio
_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var); _yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
EKF/optflow_fusion.cpp
+1 -1
View File
@@ -69,7 +69,7 @@ void Ekf::fuseOptFlow()
float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance); float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance);
// get rotation nmatrix from earth to body // get rotation nmatrix from earth to body
matrix::Dcm<float> earth_to_body(_state.quat_nominal); Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose(); earth_to_body = earth_to_body.transpose();
// calculate the sensor position relative to the IMU // calculate the sensor position relative to the IMU
EKF/sideslip_fusion.cpp
+1 -1
View File
@@ -72,7 +72,7 @@ void Ekf::fuseSideslip()
rel_wind(1) = ve - vwe; rel_wind(1) = ve - vwe;
rel_wind(2) = vd; rel_wind(2) = vd;
matrix::Dcm<float> earth_to_body(_state.quat_nominal); Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose(); //Why transpose? earth_to_body = earth_to_body.transpose(); //Why transpose?
// rotate into body axes // rotate into body axes