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

move ekf2 Matrix helper utilities to mathlib

Browse Source
This commit is contained in:
Daniel Agar
2022-08-05 09:58:07 -04:00
parent 6ebc88fed7
commit bce4237963
11 changed files with 173 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/utils.hpp
+8 -42
View File
@@ -36,54 +36,18 @@
#ifndef EKF_UTILS_HPP
#define EKF_UTILS_HPP
// return the square of two floating point numbers - used in auto coded sections
static constexpr float sq(float var) { return var * var; }
// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
// to the corresponding rotation matrix that rotates from frame 2 to frame 1
// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
// rot312(1) - Second rotation is a RH rotation about the X axis (rad)
// rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
// See http://www.atacolorado.com/eulersequences.doc
matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312);
// Use Kahan summation algorithm to get the sum of "sum_previous" and "input".
// This function relies on the caller to be responsible for keeping a copy of
// "accumulator" and passing this value at the next iteration.
// Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm
float kahanSummation(float sum_previous, float input, float &accumulator);
// calculate the inverse rotation matrix from a quaternion rotation
// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
// when there is more roll than pitch tilt and a 3-1-2 rotation sequence
// when there is more pitch than roll tilt to avoid gimbal lock.
inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); }
inline float getEuler321Yaw(const matrix::Dcmf &R) { return atan2f(R(1, 0), R(0, 0)); }
inline float getEuler312Yaw(const matrix::Dcmf &R) { return atan2f(-R(0, 1), R(1, 1)); }
float getEuler321Yaw(const matrix::Quatf &q);
float getEuler312Yaw(const matrix::Quatf &q);
inline float getEulerYaw(const matrix::Dcmf &R)
inline float kahanSummation(float sum_previous, float input, float &accumulator)
{
if (shouldUse321RotationSequence(R)) {
return getEuler321Yaw(R);
} else {
return getEuler312Yaw(R);
}
const float y = input - accumulator;
const float t = sum_previous + y;
accumulator = (t - sum_previous) - y;
return t;
}
matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in);
matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in);
// Checks which euler rotation sequence to use and update yaw in rotation matrix
matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in);
namespace ecl
{
inline float powf(float x, int exp)
@@ -105,5 +69,7 @@ inline float powf(float x, int exp)
return 1.0f;
}
}
} // namespace ecl
#endif // EKF_UTILS_HPP