fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-04 11:17:59 -04:00
parent 6ebc88fed7
commit bce4237963
11 changed files with 173 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/mathlib/CMakeLists.txt
View File

@ -45,4 +45,5 @@ px4_add_unit_gtest(SRC math/test/MedianFilterTest.cpp)
px4_add_unit_gtest(SRC math/test/NotchFilterTest.cpp)
px4_add_unit_gtest(SRC math/test/second_order_reference_model_test.cpp)
px4_add_unit_gtest(SRC math/FunctionsTest.cpp)
px4_add_unit_gtest(SRC math/test/UtilitiesTest.cpp)
px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp)

85
src/modules/ekf2/EKF/utils.cpp → src/lib/mathlib/math/Utilities.hpp
View File

@ -31,9 +31,27 @@
*
****************************************************************************/
#include "utils.hpp"
#ifndef MATH_UTILITIES_H
#define MATH_UTILITIES_H
matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
#include <matrix/math.hpp>
namespace math
{
namespace Utilities
{
// 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
inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
{
// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence
const float c2 = cosf(rot312(2)); // third rotation is pitch
@ -57,15 +75,7 @@ matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
return R;
}
float kahanSummation(float sum_previous, float input, float &accumulator)
{
const float y = input - accumulator;
const float t = sum_previous + y;
accumulator = (t - sum_previous) - y;
return t;
}
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
{
const float q00 = quat(0) * quat(0);
const float q11 = quat(1) * quat(1);
@ -92,7 +102,25 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
return dcm;
}
float getEuler321Yaw(const matrix::Quatf &q)
// 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));
}
inline float getEuler321Yaw(const matrix::Quatf &q)
{
// Values from yaw_input_321.c file produced by
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
@ -101,7 +129,7 @@ float getEuler321Yaw(const matrix::Quatf &q)
return atan2f(a, b);
}
float getEuler312Yaw(const matrix::Quatf &q)
inline float getEuler312Yaw(const matrix::Quatf &q)
{
// Values from yaw_input_312.c file produced by
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
@ -110,14 +138,24 @@ float getEuler312Yaw(const matrix::Quatf &q)
return atan2f(a, b);
}
matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
inline float getEulerYaw(const matrix::Dcmf &R)
{
if (shouldUse321RotationSequence(R)) {
return getEuler321Yaw(R);
} else {
return getEuler312Yaw(R);
}
}
inline matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
{
matrix::Eulerf euler321(rot_in);
euler321(2) = yaw;
return matrix::Dcmf(euler321);
}
matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
{
const matrix::Vector3f rotVec312(yaw, // yaw
asinf(rot_in(2, 1)), // roll
@ -125,9 +163,18 @@ matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
return taitBryan312ToRotMat(rotVec312);
}
matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in)
// Checks which euler rotation sequence to use and update yaw in rotation matrix
inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in)
{
return shouldUse321RotationSequence(rot_in) ?
updateEuler321YawInRotMat(yaw, rot_in) :
updateEuler312YawInRotMat(yaw, rot_in);
if (shouldUse321RotationSequence(rot_in)) {
return updateEuler321YawInRotMat(yaw, rot_in);
} else {
return updateEuler312YawInRotMat(yaw, rot_in);
}
}
} // namespace Utilities
} // namespace math
#endif // MATH_UTILITIES_H

86
src/lib/mathlib/math/test/UtilitiesTest.cpp Normal file
View File

@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (c) 2019 ECL Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_EKF_utils.cpp
*
* @brief Unit tests for the miscellaneous EKF utilities
*/
#include <gtest/gtest.h>
#include <cmath>
#include <vector>
#include <mathlib/mathlib.h>
using namespace math::Utilities;
TEST(euler312YawTest, fromQuaternion)
{
matrix::Quatf q1(3.5f, 2.4f, -0.5f, -3.f);
q1.normalize();
const matrix::Eulerf euler1(q1);
EXPECT_FLOAT_EQ(euler1(2), getEuler321Yaw(q1));
matrix::Quatf q2(0.f, 0, -1.f, 0.f);
q2.normalize();
const matrix::Eulerf euler2(q2);
EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2));
}
TEST(shouldUse321RotationSequenceTest, pitch90)
{
matrix::Eulerf euler(0.f, math::radians(90), 0.f);
matrix::Dcmf R(euler);
EXPECT_FALSE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, roll90)
{
matrix::Eulerf euler(math::radians(90.f), 0.f, 0.f);
matrix::Dcmf R(euler);
EXPECT_TRUE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, moreRollThanPitch)
{
matrix::Eulerf euler(math::radians(45.f), math::radians(30.f), 0.f);
matrix::Dcmf R(euler);
EXPECT_TRUE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, morePitchThanRoll)
{
matrix::Eulerf euler(math::radians(30.f), math::radians(45.f), 0.f);
matrix::Dcmf R(euler);
EXPECT_FALSE(shouldUse321RotationSequence(R));
}

1
src/lib/mathlib/mathlib.h
View File

@ -45,5 +45,6 @@
#include "math/Functions.hpp"
#include "math/SearchMin.hpp"
#include "math/TrajMath.hpp"
#include "math/Utilities.hpp"
#endif

1
src/modules/ekf2/CMakeLists.txt
View File

@ -70,7 +70,6 @@ px4_add_module(
EKF/sensor_range_finder.cpp
EKF/sideslip_fusion.cpp
EKF/terrain_estimator.cpp
EKF/utils.cpp
EKF/vel_pos_fusion.cpp
EKF/zero_innovation_heading_update.cpp
EKF/zero_velocity_update.cpp

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -55,7 +55,6 @@ add_library(ecl_EKF
sensor_range_finder.cpp
sideslip_fusion.cpp
terrain_estimator.cpp
utils.cpp
vel_pos_fusion.cpp
zero_innovation_heading_update.cpp
zero_velocity_update.cpp

7
src/modules/ekf2/EKF/common.h
View File

@ -45,6 +45,7 @@
#include <cstdint>
#include <matrix/math.hpp>
#include <mathlib/math/Utilities.hpp>
namespace estimator
{
@ -58,6 +59,12 @@ using matrix::Vector2f;
using matrix::Vector3f;
using matrix::wrap_pi;
using math::Utilities::getEulerYaw;
using math::Utilities::quatToInverseRotMat;
using math::Utilities::shouldUse321RotationSequence;
using math::Utilities::sq;
using math::Utilities::updateYawInRotMat;
// maximum sensor intervals in usec
#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)

4
src/modules/ekf2/EKF/control.cpp
View File

@ -362,7 +362,7 @@ void Ekf::controlExternalVisionFusion()
// determine if we should use the yaw observation
resetEstimatorAidStatus(_aid_src_ev_yaw);
const float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat);
const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
if (PX4_ISFINITE(measured_hdg)) {
@ -383,7 +383,7 @@ void Ekf::controlExternalVisionFusion()
} else {
// populate estimator_aid_src_ev_yaw with delta heading innovations for logging
// use the change in yaw since the last measurement
const float measured_hdg_prev = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed_prev.quat) : getEuler312Yaw(_ev_sample_delayed_prev.quat);
const float measured_hdg_prev = getEulerYaw(_ev_sample_delayed_prev.quat);
// calculate the change in yaw since the last measurement
const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev);

4
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -70,7 +70,7 @@ void Ekf::controlMagFusion()
// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
const Vector3f mag_earth_pred = R_to_earth * mag_observation;
resetEstimatorAidStatus(_aid_src_mag_heading);
@ -355,7 +355,7 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
} else if (_control_status.flags.mag_hdg && !_is_yaw_fusion_inhibited) {
// Rotate the measurements into earth frame using the zero yaw angle
Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);

50
src/modules/ekf2/EKF/utils.hpp
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

41
src/modules/ekf2/test/test_EKF_utils.cpp
View File

@ -56,44 +56,3 @@ TEST(eclPowfTest, compareToStandardImplementation)
}
}
}
TEST(euler312YawTest, fromQuaternion)
{
matrix::Quatf q1(3.5f, 2.4f, -0.5f, -3.f);
q1.normalize();
const matrix::Eulerf euler1(q1);
EXPECT_FLOAT_EQ(euler1(2), getEuler321Yaw(q1));
matrix::Quatf q2(0.f, 0, -1.f, 0.f);
q2.normalize();
const matrix::Eulerf euler2(q2);
EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2));
}
TEST(shouldUse321RotationSequenceTest, pitch90)
{
matrix::Eulerf euler(0.f, math::radians(90), 0.f);
matrix::Dcmf R(euler);
EXPECT_FALSE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, roll90)
{
matrix::Eulerf euler(math::radians(90.f), 0.f, 0.f);
matrix::Dcmf R(euler);
EXPECT_TRUE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, moreRollThanPitch)
{
matrix::Eulerf euler(math::radians(45.f), math::radians(30.f), 0.f);
matrix::Dcmf R(euler);
EXPECT_TRUE(shouldUse321RotationSequence(R));
}
TEST(shouldUse321RotationSequenceTest, morePitchThanRoll)
{
matrix::Eulerf euler(math::radians(30.f), math::radians(45.f), 0.f);
matrix::Dcmf R(euler);
EXPECT_FALSE(shouldUse321RotationSequence(R));
}