Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: use float arrays when not doing bounds checking

Browse Source 
g++ is doing a lousy job of inlining VectorN, so avoid it when we
don't need bounds checking for production code
This commit is contained in:
Andrew Tridgell 2013-12-30 21:27:50 +11:00
parent c7533579ac
commit 3d6cb9eade
2 changed files with 73 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1,8 +1,11 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// #define MATH_CHECK_INDEXES 1
#include <AP_HAL.h>
// uncomment this to force the optimisation of this code, note that
// this makes debugging harder
// #pragma GCC optimize("O3")
#include "AP_NavEKF.h"
#include <stdio.h>
@ -342,14 +345,6 @@ void NavEKF::CovariancePrediction()
float dvy_b;
float dvz_b;
// arrays
Vector24 processNoise;
VectorN<float,21> SF;
VectorN<float,8> SG;
VectorN<float,11> SQ;
VectorN<float,13> SPP;
Matrix24 nextP;
// calculate covariance prediction process noise
windVelSigma = dt * 0.1f;
dAngBiasSigma = dt * 5.0e-7f;
@ -1072,7 +1067,7 @@ void NavEKF::CovariancePrediction()
// growth by setting the predicted to the previous values
// This prevent an ill conditioned matrix from occurring for long periods
// without GPS
if ((P[7][7] + P[8][8]) > 1E6)
if ((P[7][7] + P[8][8]) > 1e6f)
{
for (uint8_t i=7; i<=8; i++)
{
@ -1107,7 +1102,7 @@ void NavEKF::FuseVelPosNED()
uint32_t horizRetryTime = 0;
bool velHealth = false;
bool posHealth = false;
bool hgtHealth;
bool hgtHealth = false;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
@ -1115,7 +1110,7 @@ void NavEKF::FuseVelPosNED()
// declare variables used to check measurement errors
Vector3f velInnov;
VectorN<float,2> posInnov;
Vector2 posInnov;
float hgtInnov = 0;
// declare variables used to control access to arrays
@ -1129,7 +1124,6 @@ void NavEKF::FuseVelPosNED()
Vector6 R_OBS;
Vector6 observation;
float SK;
float quatMag;
// Perform sequential fusion of GPS measurements. This assumes that the
// errors in the different velocity and position components are
@ -1253,7 +1247,7 @@ void NavEKF::FuseVelPosNED()
stateIndex = 4 + obsIndex;
// Calculate the measurement innovation, using states from a
// different time coordinate if fusing height data
if (obsIndex >= 0 && obsIndex <= 2)
if (obsIndex <= 2)
{
innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
}
@ -1261,7 +1255,7 @@ void NavEKF::FuseVelPosNED()
{
innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
}
else if (obsIndex == 5)
else
{
innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
}
@ -1617,7 +1611,6 @@ void NavEKF::FuseAirspeed()
Vector24 H_TAS;
Vector24 Kfusion;
float VtasPred;
float quatMag;
// Copy required states to local variable names
vn = statesAtVtasMeasTime[4];
@ -1732,26 +1725,18 @@ void NavEKF::FuseAirspeed()
void NavEKF::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
for (row=first; row<=last; row++)
{
for (col=0; col<=23; col++)
{
covMat[row][col] = 0;
}
memset(&covMat[row][0], 0, sizeof(float)*24);
}
}
void NavEKF::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
for (col=first; col<=last; col++)
for (row=0; row<=23; row++)
{
for (row=0; row<=23; row++)
{
covMat[row][col] = 0;
}
memset(&covMat[row][first], 0, sizeof(float)*(1+last-first));
}
}
@ -1773,7 +1758,6 @@ void NavEKF::RecallStates(Vector24 &statesForFusion, uint32_t msec)
for (uint8_t i=0; i<=49; i++)
{
timeDelta = msec - statetimeStamp[i];
if (timeDelta < 0) timeDelta = -timeDelta;
if (timeDelta < bestTimeDelta)
{
bestStoreIndex = i;
@ -1867,9 +1851,9 @@ void NavEKF::calcposNE(float lat, float lon)
bool NavEKF::getLLH(struct Location &loc)
{
loc.lat = 1.0e7 * degrees(latRef + states[7] / RADIUS_OF_EARTH);
loc.lng = 1.0e7 * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef));
loc.alt = 1.0e2 * (hgtRef - states[9]);
loc.lat = 1.0e7f * degrees(latRef + states[7] / RADIUS_OF_EARTH);
loc.lng = 1.0e7f * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef));
loc.alt = 1.0e2f * (hgtRef - states[9]);
return true;
}
@ -1957,35 +1941,31 @@ void NavEKF::readHgtData()
void NavEKF::readMagData()
{
// scale compass data to improve numerical conditioning
magData = _ahrs.get_compass()->get_field() * 0.001f;
magBias = _ahrs.get_compass()->get_offsets() * 0.001f;
if ((magData.x != magDataPrev.x) && (magData.y != magDataPrev.y) && (magData.z != magDataPrev.z))
{
magDataPrev = magData;
if (_ahrs.get_compass()->last_update != lastMagUpdate) {
lastMagUpdate = _ahrs.get_compass()->last_update;
magData = _ahrs.get_compass()->get_field() * 0.001f;
magBias = _ahrs.get_compass()->get_offsets() * 0.001f;
// Recall states from compass measurement time
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay));
newDataMag = true;
}
else
{
} else {
newDataMag = false;
}
}
void NavEKF::readAirSpdData()
{
if (!_ahrs.airspeed_estimate_true(&VtasMeas))
{
if (VtasMeas != VtasMeasPrev)
{
newDataTas = true;
VtasMeasPrev = VtasMeas;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay));
}
else
{
newDataTas = false;
}
const AP_Airspeed *aspeed = _ahrs.get_airspeed();
if (aspeed &&
aspeed->last_update_ms() != lastAirspeedUpdate &&
_ahrs.airspeed_estimate_true(&VtasMeas)) {
lastAirspeedUpdate = aspeed->last_update_ms();
newDataTas = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay));
} else {
newDataTas = false;
}
}

46
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -28,18 +28,40 @@
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AP_Compass.h>
// #define MATH_CHECK_INDEXES 1
#include <vectorN.h>
class NavEKF
{
public:
#if MATH_CHECK_INDEXES
typedef VectorN<float,2> Vector2;
typedef VectorN<float,3> Vector3;
typedef VectorN<float,6> Vector6;
typedef VectorN<float,8> Vector8;
typedef VectorN<float,11> Vector11;
typedef VectorN<float,13> Vector13;
typedef VectorN<float,21> Vector21;
typedef VectorN<float,24> Vector24;
typedef VectorN<VectorN<float,3>,3> Matrix3;
typedef VectorN<VectorN<float,24>,24> Matrix24;
typedef VectorN<VectorN<float,50>,24> Matrix24_50;
#else
typedef float Vector2[2];
typedef float Vector3[3];
typedef float Vector6[6];
typedef float Vector8[8];
typedef float Vector11[11];
typedef float Vector13[13];
typedef float Vector21[21];
typedef float Vector24[24];
typedef float Matrix3[3][3];
typedef float Matrix24[24][24];
typedef float Matrix24_50[24][50];
#endif
// Constructor
NavEKF(const AP_AHRS &ahrs, AP_Baro &baro);
@ -141,12 +163,13 @@ private:
bool statesInitialised;
Vector24 states; // state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field
Matrix24 KH; // intermediate result used for covariance updates
Matrix24 KHP; // intermediate result used for covariance updates
Matrix24 P; // covariance matrix
Vector24 states; // state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field
Matrix24_50 storedStates; // state vectors stored for the last 50 time steps
VectorN<uint32_t,50> statetimeStamp; // time stamp for each state vector stored
uint32_t statetimeStamp[50]; // time stamp for each state vector stored
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad)
@ -206,6 +229,12 @@ private:
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps
// last time compass was updated
uint32_t lastMagUpdate;
// last time airspeed was updated
uint32_t lastAirspeedUpdate;
// Estimated time delays (msec) for different measurements relative to IMU
const uint32_t msecVelDelay;
const uint32_t msecPosDelay;
@ -234,11 +263,9 @@ private:
uint32_t MAGtime;
uint32_t lastMAGtime;
bool newDataMag;
Vector3f magDataPrev;
// TAS input variables
bool newDataTas;
float VtasMeasPrev;
// AHRS input data variables
Vector3f ahrsEul;
@ -280,6 +307,15 @@ private:
Vector3f lastAngRate;
Vector3f lastAccel;
// CovariancePrediction variables
Matrix24 nextP;
Vector24 processNoise;
Vector21 SF;
Vector8 SG;
Vector11 SQ;
Vector13 SPP;
};
#endif // AP_NavEKF