mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-20 19:07:35 +08:00
Remove uninformative comments
This commit is contained in:
committed by
Mathieu Bresciani
parent
4a69b41015
commit
48f0eb16da
@@ -200,24 +200,19 @@ void Ekf::fuseAirspeed()
|
|||||||
|
|
||||||
for (int i = 0; i < _k_num_states; i++) {
|
for (int i = 0; i < _k_num_states; i++) {
|
||||||
if (P(i,i) < KHP(i,i)) {
|
if (P(i,i) < KHP(i,i)) {
|
||||||
// zero rows and columns
|
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
|
|
||||||
//flag as unhealthy
|
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
||||||
// update individual measurement health status
|
|
||||||
_fault_status.flags.bad_airspeed = true;
|
_fault_status.flags.bad_airspeed = true;
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
|
|||||||
@@ -274,28 +274,18 @@ void Ekf::fuseDrag()
|
|||||||
// the covariance matrix is unhealthy and must be corrected
|
// the covariance matrix is unhealthy and must be corrected
|
||||||
bool healthy = true;
|
bool healthy = true;
|
||||||
|
|
||||||
//_fault_status.flags.bad_sideslip = false;
|
|
||||||
for (int i = 0; i < _k_num_states; i++) {
|
for (int i = 0; i < _k_num_states; i++) {
|
||||||
if (P(i,i) < KHP(i,i)) {
|
if (P(i,i) < KHP(i,i)) {
|
||||||
// zero rows and columns
|
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
|
|
||||||
//flag as unhealthy
|
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
||||||
// update individual measurement health status
|
|
||||||
//_fault_status.flags.bad_sideslip = true;
|
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
|
|||||||
@@ -245,25 +245,20 @@ void Ekf::fuseGpsAntYaw()
|
|||||||
|
|
||||||
for (int i = 0; i < _k_num_states; i++) {
|
for (int i = 0; i < _k_num_states; i++) {
|
||||||
if (P(i,i) < KHP(i,i)) {
|
if (P(i,i) < KHP(i,i)) {
|
||||||
// zero rows and columns
|
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
|
|
||||||
//flag as unhealthy
|
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
||||||
// update individual measurement health status
|
|
||||||
_fault_status.flags.bad_hdg = true;
|
_fault_status.flags.bad_hdg = true;
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
_time_last_gps_yaw_fuse = _time_last_imu;
|
_time_last_gps_yaw_fuse = _time_last_imu;
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
|
|||||||
@@ -413,12 +413,10 @@ void Ekf::fuseMag()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
@@ -733,24 +731,19 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
|
|||||||
|
|
||||||
for (int i = 0; i < _k_num_states; i++) {
|
for (int i = 0; i < _k_num_states; i++) {
|
||||||
if (P(i,i) < KHP(i,i)) {
|
if (P(i,i) < KHP(i,i)) {
|
||||||
// zero rows and columns
|
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
|
|
||||||
//flag as unhealthy
|
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
||||||
// update individual measurement health status
|
|
||||||
_fault_status.flags.bad_hdg = true;
|
_fault_status.flags.bad_hdg = true;
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
@@ -1022,24 +1015,19 @@ void Ekf::fuseDeclination(float decl_sigma)
|
|||||||
_fault_status.flags.bad_mag_decl = false;
|
_fault_status.flags.bad_mag_decl = false;
|
||||||
for (int i = 0; i < _k_num_states; i++) {
|
for (int i = 0; i < _k_num_states; i++) {
|
||||||
if (P(i,i) < KHP(i,i)) {
|
if (P(i,i) < KHP(i,i)) {
|
||||||
// zero rows and columns
|
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
|
|
||||||
//flag as unhealthy
|
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
||||||
// update individual measurement health status
|
|
||||||
_fault_status.flags.bad_mag_decl = true;
|
_fault_status.flags.bad_mag_decl = true;
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
|
|||||||
@@ -475,13 +475,10 @@ void Ekf::fuseOptFlow()
|
|||||||
|
|
||||||
for (int i = 0; i < _k_num_states; i++) {
|
for (int i = 0; i < _k_num_states; i++) {
|
||||||
if (P(i,i) < KHP(i,i)) {
|
if (P(i,i) < KHP(i,i)) {
|
||||||
// zero rows and columns
|
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
|
|
||||||
//flag as unhealthy
|
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
||||||
// update individual measurement health status
|
|
||||||
if (obs_index == 0) {
|
if (obs_index == 0) {
|
||||||
_fault_status.flags.bad_optflow_X = true;
|
_fault_status.flags.bad_optflow_X = true;
|
||||||
|
|
||||||
@@ -491,12 +488,10 @@ void Ekf::fuseOptFlow()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
|
|||||||
@@ -249,23 +249,18 @@ void Ekf::fuseSideslip()
|
|||||||
|
|
||||||
for (int i = 0; i < _k_num_states; i++) {
|
for (int i = 0; i < _k_num_states; i++) {
|
||||||
if (P(i,i) < KHP(i,i)) {
|
if (P(i,i) < KHP(i,i)) {
|
||||||
// zero rows and columns
|
|
||||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||||
|
|
||||||
//flag as unhealthy
|
|
||||||
healthy = false;
|
healthy = false;
|
||||||
|
|
||||||
// update individual measurement health status
|
|
||||||
_fault_status.flags.bad_sideslip = true;
|
_fault_status.flags.bad_sideslip = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
|
|||||||
@@ -187,12 +187,10 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// only apply covariance and state corrections if healthy
|
|
||||||
if (healthy) {
|
if (healthy) {
|
||||||
// apply the covariance corrections
|
// apply the covariance corrections
|
||||||
P -= KHP;
|
P -= KHP;
|
||||||
|
|
||||||
// correct the covariance matrix for gross errors
|
|
||||||
fixCovarianceErrors(true);
|
fixCovarianceErrors(true);
|
||||||
|
|
||||||
// apply the state corrections
|
// apply the state corrections
|
||||||
|
|||||||
Reference in New Issue
Block a user