fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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
commit eb1e73ec81
11 changed files with 55 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/airspeed_fusion.cpp
View File

@ -231,7 +231,7 @@ void Ekf::get_wind_velocity(float *wind)
void Ekf::resetWindStates()
{
// get euler yaw angle
matrix::Euler<float> 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;
}
}

22
EKF/common.h
View File

@ -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<float, 2> Vector2f;
typedef matrix::Vector<float, 3> Vector3f;
typedef matrix::Quaternion<float> Quaternion;
typedef matrix::Matrix<float, 3, 3> 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
@ -318,7 +322,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

13
EKF/control.cpp
View File

@ -159,21 +159,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<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
_state.quat_nominal(3));
matrix::Euler<float> 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<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
matrix::Euler<float> 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();

2
EKF/drag_fusion.cpp
View File

@ -80,7 +80,7 @@ void Ekf::fuseDrag()
rel_wind(0) = vn - vwn;
rel_wind(1) = ve - vwe;
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();
rel_wind = earth_to_body * rel_wind;

16
EKF/ekf.cpp
View File

@ -275,8 +275,8 @@ bool Ekf::initialiseFilter()
}
// calculate initial tilt alignment
matrix::Euler<float> 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
@ -340,7 +340,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
@ -400,13 +400,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<float> 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
@ -481,7 +481,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
@ -538,8 +538,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

6
EKF/ekf.h
View File

@ -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
@ -265,7 +265,7 @@ private:
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
Vector2f _accel_lpf_NE; // Low pass filtered horizontal earth frame acceleration (m/s**2)
@ -307,7 +307,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)

22
EKF/ekf_helper.cpp
View File

@ -314,8 +314,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
@ -340,7 +340,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
@ -355,16 +355,16 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// use a 321 sequence
// 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
euler321(2) = 0.0f;
matrix::Dcm<float> 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<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
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
// 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
@ -404,7 +404,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
float s0 = sinf(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(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
@ -418,7 +418,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<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
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
// 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
@ -980,7 +980,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);

2
EKF/estimator_interface.h
View File

@ -420,6 +420,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);
};

18
EKF/mag_fusion.cpp
View File

@ -72,7 +72,7 @@ void Ekf::fuseMag()
SH_MAG[8] = 2.0f*magE*q3;
// 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();
Vector3f mag_I_rot = R_to_body * _state.mag_I;
@ -421,7 +421,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
@ -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;
// 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
// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
euler321(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler321);
Dcmf R_to_earth(euler321);
// calculate the observed yaw angle
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;
} else if (_control_status.flags.ev_yaw) {
// 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
measured_hdg = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
} else {
@ -531,7 +531,7 @@ void Ekf::fuseHeading()
float s0 = sinf(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(1, 1) = c0 * 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;
} else if (_control_status.flags.ev_yaw) {
// 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
measured_hdg = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
} else {
@ -630,13 +630,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);

2
EKF/optflow_fusion.cpp
View File

@ -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<float> 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

2
EKF/sideslip_fusion.cpp
View File

@ -72,7 +72,7 @@ void Ekf::fuseSideslip()
rel_wind(1) = ve - vwe;
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?
// rotate into body axes