diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 64 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.h | 120 |
2 files changed, 92 insertions, 92 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 6e654f496..d3f3fde7b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -177,7 +177,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float deltaQuat[4]; const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS); -// Remove sensor bias errors + // Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; @@ -192,14 +192,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED() delAngTotal.y += correctedDelAng.y; delAngTotal.z += correctedDelAng.z; -// Save current measurements + // Save current measurements Vector3f prevDelAng = correctedDelAng; -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded + // Apply corrections for earths rotation rate and coning errors + // * and + operators have been overloaded correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); -// Convert the rotation vector to its equivalent quaternion + // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) { @@ -221,14 +221,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED() deltaQuat[3] = correctedDelAng.z*rotScaler; } -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion + // Update the quaternions by rotating from the previous attitude through + // the delta angle rotation quaternion qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; -// Normalise the quaternions and update the quaternion states + // Normalise the quaternions and update the quaternion states quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { @@ -239,7 +239,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() states[3] = quatMagInv*qUpdated[3]; } -// Calculate the body to nav cosine matrix + // Calculate the body to nav cosine matrix q00 = sq(states[0]); q11 = sq(states[1]); q22 = sq(states[2]); @@ -263,29 +263,29 @@ void AttPosEKF::UpdateStrapdownEquationsNED() Tnb = Tbn.transpose(); -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded + // transform body delta velocities to delta velocities in the nav frame + // * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU; delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU; delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU; -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) + // calculate the magnitude of the nav acceleration (required for GPS + // variance estimation) accNavMag = delVelNav.length()/dtIMU; -// If calculating position save previous velocity + // If calculating position save previous velocity float lastVelocity[3]; lastVelocity[0] = states[4]; lastVelocity[1] = states[5]; lastVelocity[2] = states[6]; -// Sum delta velocities to get velocity + // Sum delta velocities to get velocity states[4] = states[4] + delVelNav.x; states[5] = states[5] + delVelNav.y; states[6] = states[6] + delVelNav.z; -// If calculating postions, do a trapezoidal integration for position + // If calculating postions, do a trapezoidal integration for position states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; @@ -965,24 +965,24 @@ void AttPosEKF::updateDtVelPosFilt(float dt) void AttPosEKF::FuseVelposNED() { -// declare variables used by fault isolation logic + // declare variables used by fault isolation logic uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available uint32_t hgtRetryTime = 500; // height measurement retry time uint32_t horizRetryTime; -// declare variables used to check measurement errors + // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; float posInnov[2] = {0.0f,0.0f}; float hgtInnov = 0.0f; -// declare variables used to control access to arrays + // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; uint8_t indexLimit = 21; -// declare variables used by state and covariance update calculations + // declare variables used by state and covariance update calculations float velErr; float posErr; float R_OBS[6]; @@ -990,12 +990,12 @@ void AttPosEKF::FuseVelposNED() float SK; float quatMag; -// Perform sequential fusion of GPS measurements. This assumes that the -// errors in the different velocity and position components are -// uncorrelated which is not true, however in the absence of covariance -// data from the GPS receiver it is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion + // Perform sequential fusion of GPS measurements. This assumes that the + // errors in the different velocity and position components are + // uncorrelated which is not true, however in the absence of covariance + // data from the GPS receiver it is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { uint64_t tNow = getMicros(); @@ -1251,12 +1251,12 @@ void AttPosEKF::FuseMagnetometer() H_MAG[i] = 0.0f; } -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion + // Perform sequential fusion of Magnetometer measurements. + // This assumes that the errors in the different components are + // uncorrelated which is not true, however in the absence of covariance + // data fit is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion if (useCompass && fuseMagData && (obsIndex < 3)) { // Calculate observation jacobians and Kalman gains diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index f8cd743cc..a4b95f14a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -248,115 +248,115 @@ public: float minFlowRng; // minimum range over which to fuse optical flow measurements float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate -void updateDtGpsFilt(float dt); + void updateDtGpsFilt(float dt); -void updateDtHgtFilt(float dt); + void updateDtHgtFilt(float dt); -void UpdateStrapdownEquationsNED(); + void UpdateStrapdownEquationsNED(); -void CovariancePrediction(float dt); + void CovariancePrediction(float dt); -void FuseVelposNED(); + void FuseVelposNED(); -void FuseMagnetometer(); + void FuseMagnetometer(); -void FuseAirspeed(); + void FuseAirspeed(); -void FuseOptFlow(); + void FuseOptFlow(); -void OpticalFlowEKF(); + void OpticalFlowEKF(); -void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); + void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); -void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); + void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); -void quatNorm(float (&quatOut)[4], const float quatIn[4]); + void quatNorm(float (&quatOut)[4], const float quatIn[4]); -// store staes along with system time stamp in msces -void StoreStates(uint64_t timestamp_ms); + // store staes along with system time stamp in msces + void StoreStates(uint64_t timestamp_ms); -/** - * Recall the state vector. - * - * Recalls the vector stored at closest time to the one specified by msec - *FuseOptFlow - * @return zero on success, integer indicating the number of invalid states on failure. - * Does only copy valid states, if the statesForFusion vector was initialized - * correctly by the caller, the result can be safely used, but is a mixture - * time-wise where valid states were updated and invalid remained at the old - * value. - */ -int RecallStates(float *statesForFusion, uint64_t msec); + /** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + *FuseOptFlow + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ + int RecallStates(float *statesForFusion, uint64_t msec); -void RecallOmega(float *omegaForFusion, uint64_t msec); + void RecallOmega(float *omegaForFusion, uint64_t msec); -void ResetStoredStates(); + void ResetStoredStates(); -void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); + void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); -void calcEarthRateNED(Vector3f &omega, float latitude); + void calcEarthRateNED(Vector3f &omega, float latitude); -static void eul2quat(float (&quat)[4], const float (&eul)[3]); + static void eul2quat(float (&quat)[4], const float (&eul)[3]); -static void quat2eul(float (&eul)[3], const float (&quat)[4]); + static void quat2eul(float (&eul)[3], const float (&quat)[4]); -static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); + void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); + static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); -static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); -static float sq(float valIn); + static float sq(float valIn); -static float maxf(float valIn1, float valIn2); + static float maxf(float valIn1, float valIn2); -static float min(float valIn1, float valIn2); + static float min(float valIn1, float valIn2); -void OnGroundCheck(); + void OnGroundCheck(); -void CovarianceInit(); + void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); + void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); -float ConstrainFloat(float val, float min, float maxf); + float ConstrainFloat(float val, float min, float maxf); -void ConstrainVariances(); + void ConstrainVariances(); -void ConstrainStates(); + void ConstrainStates(); -void ForceSymmetry(); + void ForceSymmetry(); -int CheckAndBound(struct ekf_status_report *last_error); + int CheckAndBound(struct ekf_status_report *last_error); -void ResetPosition(); + void ResetPosition(); -void ResetVelocity(); + void ResetVelocity(); -void ZeroVariables(); + void ZeroVariables(); -void GetFilterState(struct ekf_status_report *state); + void GetFilterState(struct ekf_status_report *state); -void GetLastErrorState(struct ekf_status_report *last_error); + void GetLastErrorState(struct ekf_status_report *last_error); -bool StatesNaN(); + bool StatesNaN(); -void InitializeDynamic(float (&initvelNED)[3], float declination); + void InitializeDynamic(float (&initvelNED)[3], float declination); protected: -void updateDtVelPosFilt(float dt); + void updateDtVelPosFilt(float dt); -bool FilterHealthy(); + bool FilterHealthy(); -bool GyroOffsetsDiverged(); + bool GyroOffsetsDiverged(); -bool VelNEDDiverged(); + bool VelNEDDiverged(); -void ResetHeight(void); + void ResetHeight(void); -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); }; |