Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

use common variables for EKF

Browse Source
This commit is contained in:
Andrew Tridgell 2019-10-01 16:17:48 +10:00
parent 9ad9fe42bc
commit 43df0022e1
6 changed files with 38 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -663,15 +663,14 @@ bool NavEKF2::InitialiseFilter(void)
//Call Constructors on all cores
for (uint8_t i = 0; i < num_cores; i++) {
//Call Constructors
new (&core[i]) NavEKF2_core();
new (&core[i]) NavEKF2_core(this);
}
// set the IMU index for the cores
num_cores = 0;
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
if(!core[num_cores].setup_core(this, i, num_cores)) {
if(!core[num_cores].setup_core(i, num_cores)) {
return false;
}
num_cores++;

10
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -34,7 +34,7 @@ extern const AP_HAL::HAL& hal;
#define ENABLE_EKF_TIMING 0
// constructor
NavEKF2_core::NavEKF2_core(void) :
NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
_perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")),
_perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_CovariancePrediction")),
_perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseVelPosNED")),
@ -58,9 +58,8 @@ NavEKF2_core::NavEKF2_core(void) :
}
// setup this core backend
bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index)
bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
{
frontend = _frontend;
imu_index = _imu_index;
gyro_index_active = _imu_index;
accel_index_active = _imu_index;
@ -894,6 +893,10 @@ void NavEKF2_core::CovariancePrediction()
float day_s; // Y axis delta angle measurement scale factor
float daz_s; // Z axis delta angle measurement scale factor
float dvz_b; // Z axis delta velocity measurement bias (rad)
Vector25 SF;
Vector5 SG;
Vector8 SQ;
Vector24 processNoise;
// calculate covariance prediction process noise
// use filtered height rate to increase wind process noise when climbing or descending
@ -991,6 +994,7 @@ void NavEKF2_core::CovariancePrediction()
SQ[6] = 2*q1*q2;
SQ[7] = SG[4];
Vector23 SPP;
SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);
SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);
SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -63,10 +63,10 @@ class NavEKF2_core : public NavEKF_core_common
{
public:
// Constructor
NavEKF2_core(void);
NavEKF2_core(NavEKF2 *_frontend);
// setup this core backend
bool setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index);
bool setup_core(uint8_t _imu_index, uint8_t _core_index);
// Initialise the states from accelerometer and magnetometer data (if present)
// This method can only be used when the vehicle is static

7
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -682,9 +682,10 @@ bool NavEKF3::InitialiseFilter(void)
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: allocation failed");
return false;
}
// Call constructors on all cores
for (uint8_t i = 0; i < num_cores; i++) {
//Call Constructors
new (&core[i]) NavEKF3_core();
new (&core[i]) NavEKF3_core(this);
}
}
@ -694,7 +695,7 @@ bool NavEKF3::InitialiseFilter(void)
bool core_setup_success = true;
for (uint8_t core_index=0; core_index<num_cores; core_index++) {
if (coreSetupRequired[core_index]) {
coreSetupRequired[core_index] = !core[core_index].setup_core(this, coreImuIndex[core_index], core_index);
coreSetupRequired[core_index] = !core[core_index].setup_core(coreImuIndex[core_index], core_index);
if (coreSetupRequired[core_index]) {
core_setup_success = false;
}

32
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -10,7 +10,7 @@
extern const AP_HAL::HAL& hal;
// constructor
NavEKF3_core::NavEKF3_core(void) :
NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) :
_perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_UpdateFilter")),
_perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_CovariancePrediction")),
_perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseVelPosNED")),
@ -19,7 +19,8 @@ NavEKF3_core::NavEKF3_core(void) :
_perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseSideslip")),
_perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_TerrainOffset")),
_perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")),
_perf_FuseBodyOdom(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseBodyOdom"))
_perf_FuseBodyOdom(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseBodyOdom")),
frontend(_frontend)
{
_perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test0");
_perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test1");
@ -36,9 +37,8 @@ NavEKF3_core::NavEKF3_core(void) :
}
// setup this core backend
bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _core_index)
bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
{
frontend = _frontend;
imu_index = _imu_index;
gyro_index_active = imu_index;
accel_index_active = imu_index;
@ -59,15 +59,15 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
}
// find the maximum time delay for all potential sensors
uint16_t maxTimeDelay_ms = MAX(_frontend->_hgtDelay_ms ,
MAX(_frontend->_flowDelay_ms ,
MAX(_frontend->_rngBcnDelay_ms ,
MAX(_frontend->magDelay_ms ,
uint16_t maxTimeDelay_ms = MAX(frontend->_hgtDelay_ms ,
MAX(frontend->_flowDelay_ms ,
MAX(frontend->_rngBcnDelay_ms ,
MAX(frontend->magDelay_ms ,
(uint16_t)(EKF_TARGET_DT_MS)
))));
// GPS sensing can have large delays and should not be included if disabled
if (_frontend->_fusionModeGPS != 3) {
if (frontend->_fusionModeGPS != 3) {
// Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay
float gps_delay_sec = 0;
if (!AP::gps().get_lag(gps_delay_sec)) {
@ -90,7 +90,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
// airspeed sensing can have large delays and should not be included if disabled
if (_ahrs->airspeed_sensor_enabled()) {
maxTimeDelay_ms = MAX(maxTimeDelay_ms , _frontend->tasDelay_ms);
maxTimeDelay_ms = MAX(maxTimeDelay_ms , frontend->tasDelay_ms);
}
// calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter
@ -100,13 +100,13 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
// with the worst case delay from current time to ekf fusion time
// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
uint16_t ekf_delay_ms = maxTimeDelay_ms + (int)(ceilf((float)maxTimeDelay_ms * 0.5f));
obs_buffer_length = (ekf_delay_ms / _frontend->sensorIntervalMin_ms) + 1;
obs_buffer_length = (ekf_delay_ms / frontend->sensorIntervalMin_ms) + 1;
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
obs_buffer_length = MIN(obs_buffer_length,imu_buffer_length);
// calculate buffer size for optical flow data
const uint8_t flow_buffer_length = MIN((ekf_delay_ms / _frontend->flowIntervalMin_ms) + 1, imu_buffer_length);
const uint8_t flow_buffer_length = MIN((ekf_delay_ms / frontend->flowIntervalMin_ms) + 1, imu_buffer_length);
if(!storedGPS.init(obs_buffer_length)) {
return false;
@ -211,6 +211,8 @@ void NavEKF3_core::InitialiseVariables()
lastKnownPositionNE.zero();
prevTnb.zero();
memset(&P[0][0], 0, sizeof(P));
memset(&KH[0][0], 0, sizeof(KH));
memset(&KHP[0][0], 0, sizeof(KHP));
memset(&nextP[0][0], 0, sizeof(nextP));
flowDataValid = false;
rangeDataToFuse = false;
@ -577,10 +579,12 @@ void NavEKF3_core::UpdateFilter(bool predict)
// start the timer used for load measurement
#if EK3_DISABLE_INTERRUPTS
irqstate_t istate = irqsave();
void *istate = hal.scheduler->disable_interrupts_save();
#endif
hal.util->perf_begin(_perf_UpdateFilter);
fill_scratch_variables();
// TODO - in-flight restart method
// Check arm status and perform required checks and mode changes
@ -628,7 +632,7 @@ void NavEKF3_core::UpdateFilter(bool predict)
// stop the timer used for load measurement
hal.util->perf_end(_perf_UpdateFilter);
#if EK3_DISABLE_INTERRUPTS
irqrestore(istate);
hal.scheduler->restore_interrupts(istate);
#endif
}

13
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -27,6 +27,7 @@
#include <AP_Math/AP_Math.h>
#include "AP_NavEKF3.h"
#include <AP_Math/vectorN.h>
#include <AP_NavEKF/AP_NavEKF_core_common.h>
#include <AP_NavEKF3/AP_NavEKF3_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
@ -63,14 +64,14 @@
class AP_AHRS;
class NavEKF3_core
class NavEKF3_core : public NavEKF_core_common
{
public:
// Constructor
NavEKF3_core(void);
NavEKF3_core(NavEKF3 *_frontend);
// setup this core backend
bool setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _core_index);
bool setup_core(uint8_t _imu_index, uint8_t _core_index);
// Initialise the states from accelerometer and magnetometer data (if present)
// This method can only be used when the vehicle is static
@ -397,7 +398,6 @@ private:
typedef VectorN<ftype,24> Vector24;
typedef VectorN<ftype,25> Vector25;
typedef VectorN<ftype,31> Vector31;
typedef VectorN<ftype,28> Vector28;
typedef VectorN<VectorN<ftype,3>,3> Matrix3;
typedef VectorN<VectorN<ftype,24>,24> Matrix24;
typedef VectorN<VectorN<ftype,34>,50> Matrix34_50;
@ -421,7 +421,6 @@ private:
typedef ftype Vector23[23];
typedef ftype Vector24[24];
typedef ftype Vector25[25];
typedef ftype Vector28[28];
typedef ftype Matrix3[3][3];
typedef ftype Matrix24[24][24];
typedef ftype Matrix34_50[34][50];
@ -855,9 +854,6 @@ private:
bool badIMUdata; // boolean true if the bad IMU data is detected
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Vector28 Kfusion; // Kalman gain vector
Matrix24 KH; // intermediate result used for covariance updates
Matrix24 KHP; // intermediate result used for covariance updates
Matrix24 P; // covariance matrix
imu_ring_buffer_t<imu_elements> storedIMU; // IMU data buffer
obs_ring_buffer_t<gps_elements> storedGPS; // GPS data buffer
@ -911,7 +907,6 @@ private:
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastSynthYawTime_ms; // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals
Vector2f lastKnownPositionNE; // last known position
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold