fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 11:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Extract general functions into utils

Browse Source
This commit is contained in:
kamilritz
2020-06-21 15:28:10 +02:00
committed by Mathieu Bresciani
parent fda30d8cec
commit ff8b5ec69d
13 changed files with 88 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/CMakeLists.txt
+1
View File
@@ -50,6 +50,7 @@ add_library(ecl_EKF
imu_down_sampler.cpp
EKFGSF_yaw.cpp
sensor_range_finder.cpp
utils.cpp
)
add_dependencies(ecl_EKF prebuild_targets)
EKF/EKFGSF_yaw.cpp
-24
View File
@@ -536,30 +536,6 @@ bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw
return false;
}
Dcmf EKFGSF_yaw::taitBryan312ToRotMat(const Vector3f &rot312)
{
// Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence
const float c2 = cosf(rot312(2));
const float s2 = sinf(rot312(2));
const float s1 = sinf(rot312(1));
const float c1 = cosf(rot312(1));
const float s0 = sinf(rot312(0));
const float c0 = cosf(rot312(0));
Dcmf R;
R(0, 0) = c0 * c2 - s0 * s1 * s2;
R(1, 1) = c0 * c1;
R(2, 2) = c2 * c1;
R(0, 1) = -c1 * s0;
R(0, 2) = s2 * c0 + c2 * s1 * s0;
R(1, 0) = c2 * s0 + s2 * s1 * c0;
R(1, 2) = s0 * s2 - s1 * c0 * c2;
R(2, 0) = -s2 * c1;
R(2, 1) = s1;
return R;
}
float EKFGSF_yaw::ahrsCalcAccelGain() const
{
// Calculate the acceleration fusion gain using a continuous function that is unity at 1g and zero
EKF/EKFGSF_yaw.h
+1 -8
View File
@@ -5,6 +5,7 @@
#include <mathlib/mathlib.h>
#include "common.h"
#include "utils.hpp"
using matrix::AxisAnglef;
using matrix::Dcmf;
@@ -124,14 +125,6 @@ private:
inline float sq(float x) const { return x * x; };
// 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
Dcmf taitBryan312ToRotMat(const Vector3f &rot312);
// Declarations used by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates
matrix::Vector<float, N_MODELS_EKFGSF> _model_weights{};
EKF/drag_fusion.cpp
+1 -1
View File
@@ -83,7 +83,7 @@ void Ekf::fuseDrag()
rel_wind(0) = vn - vwn;
rel_wind(1) = ve - vwe;
rel_wind(2) = vd;
const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
rel_wind = earth_to_body * rel_wind;
// perform sequential fusion of XY specific forces
EKF/ekf.h
-14
View File
@@ -822,12 +822,6 @@ private:
// uncorrelate quaternion states from other states
void uncorrelateQuatFromOtherStates();
// 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) const;
// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
// sensor measurement
float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
@@ -870,14 +864,6 @@ private:
// update_buffer : true if the state change should be also applied to the output observer buffer
void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer);
// 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
Dcmf taitBryan312ToRotMat(const Vector3f &rot312);
// Declarations used to control use of the EKF-GSF yaw estimator
int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
EKF/ekf_helper.cpp
-61
View File
@@ -1219,35 +1219,6 @@ void Ekf::update_deadreckoning_status()
_deadreckon_time_exceeded = (_time_last_aiding == 0) || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max);
}
// 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
Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
{
float q00 = quat(0) * quat(0);
float q11 = quat(1) * quat(1);
float q22 = quat(2) * quat(2);
float q33 = quat(3) * quat(3);
float q01 = quat(0) * quat(1);
float q02 = quat(0) * quat(2);
float q03 = quat(0) * quat(3);
float q12 = quat(1) * quat(2);
float q13 = quat(1) * quat(3);
float q23 = quat(2) * quat(3);
Matrix3f dcm;
dcm(0, 0) = q00 + q11 - q22 - q33;
dcm(1, 1) = q00 - q11 + q22 - q33;
dcm(2, 2) = q00 - q11 - q22 + q33;
dcm(1, 0) = 2.0f * (q12 - q03);
dcm(2, 0) = 2.0f * (q13 + q02);
dcm(0, 1) = 2.0f * (q12 + q03);
dcm(2, 1) = 2.0f * (q23 - q01);
dcm(0, 2) = 2.0f * (q13 - q02);
dcm(1, 2) = 2.0f * (q23 + q01);
return dcm;
}
// calculate the variances for the rotation vector equivalent
Vector3f Ekf::calcRotVecVariances()
{
@@ -1640,14 +1611,6 @@ void Ekf::loadMagCovData()
}
}
float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) const
{
float y = input - accumulator;
float t = sum_previous + y;
accumulator = (t - sum_previous) - y;
return t;
}
void Ekf::stopGpsFusion()
{
stopGpsPosFusion();
@@ -1825,30 +1788,6 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
return yawEstimator.getLogData(yaw_composite,yaw_variance,yaw,innov_VN,innov_VE,weight);
}
Dcmf Ekf::taitBryan312ToRotMat(const 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
const float s2 = sinf(rot312(2));
const float s1 = sinf(rot312(1)); // second rotation is roll
const float c1 = cosf(rot312(1));
const float s0 = sinf(rot312(0)); // first rotation is yaw
const float c0 = cosf(rot312(0));
Dcmf R;
R(0, 0) = c0 * c2 - s0 * s1 * s2;
R(1, 1) = c0 * c1;
R(2, 2) = c2 * c1;
R(0, 1) = -c1 * s0;
R(0, 2) = s2 * c0 + c2 * s1 * s0;
R(1, 0) = c2 * s0 + s2 * s1 * c0;
R(1, 2) = s0 * s2 - s1 * c0 * c2;
R(2, 0) = -s2 * c1;
R(2, 1) = s1;
return R;
}
void Ekf::runYawEKFGSF()
{
float TAS;
EKF/estimator_interface.h
+1 -3
View File
@@ -48,6 +48,7 @@
#include "imu_down_sampler.hpp"
#include "EKFGSF_yaw.h"
#include "sensor_range_finder.hpp"
#include "utils.hpp"
#include <geo/geo.h>
#include <matrix/math.hpp>
@@ -580,9 +581,6 @@ protected:
// this is the previous status of the filter control modes - used to detect mode transitions
filter_control_status_u _control_status_prev{};
// calculate the inverse rotation matrix from a quaternion rotation
Matrix3f quat_to_invrotmat(const Quatf &quat);
inline void setDragData();
inline void computeVibrationMetric();
EKF/mag_fusion.cpp
+1 -1
View File
@@ -72,7 +72,7 @@ void Ekf::fuseMag()
SH_MAG[8] = 2.0f*magE*q3;
// rotate magnetometer earth field state into body frame
const Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal);
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
const Vector3f mag_I_rot = R_to_body * _state.mag_I;
EKF/optflow_fusion.cpp
+1 -1
View File
@@ -67,7 +67,7 @@ void Ekf::fuseOptFlow()
float Kfusion[24][2] = {}; // Optical flow Kalman gains
// get rotation matrix from earth to body
const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
// calculate the sensor position relative to the IMU
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
EKF/sideslip_fusion.cpp
+1 -1
View File
@@ -73,7 +73,7 @@ void Ekf::fuseSideslip()
rel_wind(1) = ve - vwe;
rel_wind(2) = vd;
const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
// rotate into body axes
rel_wind = earth_to_body * rel_wind;
EKF/terrain_estimator.cpp
+1 -1
View File
@@ -203,7 +203,7 @@ void Ekf::fuseFlowForTerrain()
const float R_LOS = calcOptFlowMeasVar();
// get rotation matrix from earth to body
const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
// calculate the sensor position relative to the IMU
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
EKF/utils.cpp
+60
View File
@@ -0,0 +1,60 @@
#include "utils.hpp"
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
const float s2 = sinf(rot312(2));
const float s1 = sinf(rot312(1)); // second rotation is roll
const float c1 = cosf(rot312(1));
const float s0 = sinf(rot312(0)); // first rotation is yaw
const float c0 = cosf(rot312(0));
matrix::Dcmf R;
R(0, 0) = c0 * c2 - s0 * s1 * s2;
R(1, 1) = c0 * c1;
R(2, 2) = c2 * c1;
R(0, 1) = -c1 * s0;
R(0, 2) = s2 * c0 + c2 * s1 * s0;
R(1, 0) = c2 * s0 + s2 * s1 * c0;
R(1, 2) = s0 * s2 - s1 * c0 * c2;
R(2, 0) = -s2 * c1;
R(2, 1) = s1;
return R;
}
float kahanSummation(float sum_previous, float input, float &accumulator)
{
float y = input - accumulator;
float t = sum_previous + y;
accumulator = (t - sum_previous) - y;
return t;
}
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
{
float q00 = quat(0) * quat(0);
float q11 = quat(1) * quat(1);
float q22 = quat(2) * quat(2);
float q33 = quat(3) * quat(3);
float q01 = quat(0) * quat(1);
float q02 = quat(0) * quat(2);
float q03 = quat(0) * quat(3);
float q12 = quat(1) * quat(2);
float q13 = quat(1) * quat(3);
float q23 = quat(2) * quat(3);
matrix::Dcmf dcm;
dcm(0, 0) = q00 + q11 - q22 - q33;
dcm(1, 1) = q00 - q11 + q22 - q33;
dcm(2, 2) = q00 - q11 - q22 + q33;
dcm(1, 0) = 2.0f * (q12 - q03);
dcm(2, 0) = 2.0f * (q13 + q02);
dcm(0, 1) = 2.0f * (q12 + q03);
dcm(2, 1) = 2.0f * (q23 - q01);
dcm(0, 2) = 2.0f * (q13 - q02);
dcm(1, 2) = 2.0f * (q23 + q01);
return dcm;
}
EKF/utils.hpp
+20
View File
@@ -0,0 +1,20 @@
#include <matrix/math.hpp>
// 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);