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

Increase matrix library usage even more

Browse Source
This commit is contained in:
kamilritz 2020-06-21 13:31:51 +02:00 committed by Mathieu Bresciani
parent 630be60930
commit 4a69b41015
7 changed files with 10 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/airspeed_fusion.cpp
View File

@ -215,7 +215,7 @@ void Ekf::fuseAirspeed()
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
P = P - KHP;
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);

7
EKF/drag_fusion.cpp
View File

@ -292,11 +292,8 @@ void Ekf::fuseDrag()
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);

6
EKF/gps_yaw_fusion.cpp
View File

@ -261,11 +261,7 @@ void Ekf::fuseGpsAntYaw()
if (healthy) {
_time_last_gps_yaw_fuse = _time_last_imu;
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);

18
EKF/mag_fusion.cpp
View File

@ -416,11 +416,7 @@ void Ekf::fuseMag()
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);
@ -752,11 +748,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);
@ -1045,11 +1037,7 @@ void Ekf::fuseDeclination(float decl_sigma)
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);

6
EKF/optflow_fusion.cpp
View File

@ -494,11 +494,7 @@ void Ekf::fuseOptFlow()
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);

6
EKF/sideslip_fusion.cpp
View File

@ -263,11 +263,7 @@ void Ekf::fuseSideslip()
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);

6
EKF/vel_pos_fusion.cpp
View File

@ -190,11 +190,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row, column) = P(row, column) - KHP(row, column);
}
}
P -= KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors(true);