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

Estimator: Clean up delta quat calculations, put them in a sweet spot between accuracy and runtime.

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-29 11:55:44 +02:00
parent cd2ef000eb
commit 092ede366a
1 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+8 -4
View File
@@ -71,7 +71,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
float rotationMag; float rotationMag;
float qUpdated[4]; float qUpdated[4];
float quatMag; float quatMag;
double deltaQuat[4]; float deltaQuat[4];
const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
// Remove sensor bias errors // Remove sensor bias errors
@@ -104,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
} }
else else
{ {
deltaQuat[0] = cos(0.5f*rotationMag); // We are using double here as we are unsure how small
double rotScaler = (sin(0.5f*rotationMag))/rotationMag; // the angle differences are and if we get into numeric
// issues with float. The runtime impact is not measurable
// for these quantities.
deltaQuat[0] = cos(0.5*(double)rotationMag);
float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[1] = correctedDelAng.x*rotScaler;
deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler;
deltaQuat[3] = correctedDelAng.z*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler;
@@ -1659,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder()
// Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
SH_RNG[4] = sin(rngFinderPitch); SH_RNG[4] = sin(rngFinderPitch);
cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
if (useRangeFinder && cosRngTilt > 0.87f) if (useRangeFinder && cosRngTilt > 0.87f)
{ {
// Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset