Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Don't update accel bias states if vibration affected

This commit is contained in:
Paul Riseborough 2021-07-16 07:53:49 +10:00 committed by Randy Mackay
parent 36ead940d6
commit f37caffb31
3 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -467,7 +467,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
zero_range(&Kfusion[0], 10, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates && !badIMUdata) {
for (uint8_t index = 0; index < 3; index++) { for (uint8_t index = 0; index < 3; index++) {
const uint8_t stateIndex = index + 13; const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) { if (!dvelBiasAxisInhibit[index]) {
@ -643,7 +643,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
zero_range(&Kfusion[0], 10, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates && !badIMUdata) {
for (uint8_t index = 0; index < 3; index++) { for (uint8_t index = 0; index < 3; index++) {
const uint8_t stateIndex = index + 13; const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) { if (!dvelBiasAxisInhibit[index]) {

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -925,7 +925,7 @@ void NavEKF3_core::FuseVelPosNED()
// Don't use 'fake' horizontal measurements used to constrain attitude drift during // Don't use 'fake' horizontal measurements used to constrain attitude drift during
// periods of non-aiding to learn bias as these can give incorrect esitmates. // periods of non-aiding to learn bias as these can give incorrect esitmates.
const bool horizInhibit = PV_AidingMode == AID_NONE && obsIndex != 2 && obsIndex != 5; const bool horizInhibit = PV_AidingMode == AID_NONE && obsIndex != 2 && obsIndex != 5;
if (!horizInhibit && !inhibitDelVelBiasStates) { if (!horizInhibit && !inhibitDelVelBiasStates && !badIMUdata) {
for (uint8_t i = 13; i<=15; i++) { for (uint8_t i = 13; i<=15; i++) {
if (!dvelBiasAxisInhibit[i-13]) { if (!dvelBiasAxisInhibit[i-13]) {
Kfusion[i] = P[i][stateIndex]*SK; Kfusion[i] = P[i][stateIndex]*SK;
@ -1404,7 +1404,7 @@ void NavEKF3_core::FuseBodyVel()
zero_range(&Kfusion[0], 10, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates && !badIMUdata) {
for (uint8_t index = 0; index < 3; index++) { for (uint8_t index = 0; index < 3; index++) {
const uint8_t stateIndex = index + 13; const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) { if (!dvelBiasAxisInhibit[index]) {
@ -1581,7 +1581,7 @@ void NavEKF3_core::FuseBodyVel()
zero_range(&Kfusion[0], 10, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates && !badIMUdata) {
for (uint8_t index = 0; index < 3; index++) { for (uint8_t index = 0; index < 3; index++) {
const uint8_t stateIndex = index + 13; const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) { if (!dvelBiasAxisInhibit[index]) {
@ -1759,7 +1759,7 @@ void NavEKF3_core::FuseBodyVel()
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates && !badIMUdata) {
for (uint8_t index = 0; index < 3; index++) { for (uint8_t index = 0; index < 3; index++) {
const uint8_t stateIndex = index + 13; const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) { if (!dvelBiasAxisInhibit[index]) {

2
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -148,7 +148,7 @@ void NavEKF3_core::FuseRngBcn()
zero_range(&Kfusion[0], 10, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates && !badIMUdata) {
for (uint8_t index = 0; index < 3; index++) { for (uint8_t index = 0; index < 3; index++) {
const uint8_t stateIndex = index + 13; const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) { if (!dvelBiasAxisInhibit[index]) {