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

EKF matrix typedef cleanup

Browse Source
This commit is contained in:
Daniel Agar
2017-06-19 11:10:01 -04:00
parent b255d4a53a
commit c44488fdb8
11 changed files with 55 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+8 -8
View File
@@ -299,8 +299,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
@@ -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<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
@@ -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