aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Add function to LPF and spike filter baro data

Browse Source
This commit is contained in:
Paul Riseborough 2015-11-12 18:29:53 +11:00 committed by Andrew Tridgell
parent 34311bedfa
commit 4c72a14e22
2 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -712,6 +712,15 @@ void NavEKF2_core::readHgtData()
} }
} }
// calculate filtered offset between baro height measurement and EKF height estimate
// offset should be subtracted from baro measurement to match filter estimate
// offset is used to enable reversion to baro if alternate height data sources fail
void NavEKF2_core::calcFiltBaroOffset()
{
// Apply a first order LPF with spike protection
baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
}
// store baro in a history array // store baro in a history array
void NavEKF2_core::StoreBaro() void NavEKF2_core::StoreBaro()
{ {

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -629,6 +629,9 @@ private:
// using a simple observer // using a simple observer
void calcOutputStatesFast(); void calcOutputStatesFast();
// calculate a filtered offset between baro height measurement and EKF height estimate
void calcFiltBaroOffset();
// Length of FIFO buffers used for non-IMU sensor data. // Length of FIFO buffers used for non-IMU sensor data.
// Must be larger than the time period defined by IMU_BUFFER_LENGTH // Must be larger than the time period defined by IMU_BUFFER_LENGTH
static const uint32_t OBS_BUFFER_LENGTH = 5; static const uint32_t OBS_BUFFER_LENGTH = 5;