mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
a1c117360c
All Kalman gain calculations now explicity set gains for deactivated states to zero. Previous use of loops to set gains to zero have been replaced with more efficient memset operations.
1576 lines
72 KiB
C++
1576 lines
72 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
|
|
#include "AP_NavEKF3.h"
|
|
#include "AP_NavEKF3_core.h"
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/********************************************************
|
|
* RESET FUNCTIONS *
|
|
********************************************************/
|
|
|
|
// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
|
|
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
|
|
void NavEKF3_core::ResetVelocity(void)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
velResetNE.x = stateStruct.velocity.x;
|
|
velResetNE.y = stateStruct.velocity.y;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,4,5);
|
|
zeroCols(P,4,5);
|
|
|
|
if (PV_AidingMode != AID_ABSOLUTE) {
|
|
stateStruct.velocity.zero();
|
|
// set the variances using the measurement noise parameter
|
|
P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
|
|
} else {
|
|
// reset horizontal velocity states to the GPS velocity if available
|
|
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) {
|
|
stateStruct.velocity.x = gpsDataNew.vel.x;
|
|
stateStruct.velocity.y = gpsDataNew.vel.y;
|
|
// set the variances using the reported GPS speed accuracy
|
|
P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
|
|
// clear the timeout flags and counters
|
|
velTimeout = false;
|
|
lastVelPassTime_ms = imuSampleTime_ms;
|
|
} else {
|
|
stateStruct.velocity.x = 0.0f;
|
|
stateStruct.velocity.y = 0.0f;
|
|
// set the variances using the likely speed range
|
|
P[5][5] = P[4][4] = sq(25.0f);
|
|
// clear the timeout flags and counters
|
|
velTimeout = false;
|
|
lastVelPassTime_ms = imuSampleTime_ms;
|
|
}
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].velocity.x = stateStruct.velocity.x;
|
|
storedOutput[i].velocity.y = stateStruct.velocity.y;
|
|
}
|
|
outputDataNew.velocity.x = stateStruct.velocity.x;
|
|
outputDataNew.velocity.y = stateStruct.velocity.y;
|
|
outputDataDelayed.velocity.x = stateStruct.velocity.x;
|
|
outputDataDelayed.velocity.y = stateStruct.velocity.y;
|
|
|
|
// Calculate the position jump due to the reset
|
|
velResetNE.x = stateStruct.velocity.x - velResetNE.x;
|
|
velResetNE.y = stateStruct.velocity.y - velResetNE.y;
|
|
|
|
// store the time of the reset
|
|
lastVelReset_ms = imuSampleTime_ms;
|
|
|
|
// clear reset data source preference
|
|
velResetSource = DEFAULT;
|
|
|
|
}
|
|
|
|
// resets position states to last GPS measurement or to zero if in constant position mode
|
|
void NavEKF3_core::ResetPosition(void)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetNE.x = stateStruct.position.x;
|
|
posResetNE.y = stateStruct.position.y;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,7,8);
|
|
zeroCols(P,7,8);
|
|
|
|
if (PV_AidingMode != AID_ABSOLUTE) {
|
|
// reset all position state history to the last known position
|
|
stateStruct.position.x = lastKnownPositionNE.x;
|
|
stateStruct.position.y = lastKnownPositionNE.y;
|
|
// set the variances using the position measurement noise parameter
|
|
P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise);
|
|
} else {
|
|
// Use GPS data as first preference if fresh data is available
|
|
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) {
|
|
// record the ID of the GPS for the data we are using for the reset
|
|
last_gps_idx = gpsDataNew.sensor_idx;
|
|
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
|
|
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
|
|
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
|
|
// set the variances using the position measurement noise parameter
|
|
P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
|
|
// clear the timeout flags and counters
|
|
posTimeout = false;
|
|
lastPosPassTime_ms = imuSampleTime_ms;
|
|
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == DEFAULT) || posResetSource == RNGBCN) {
|
|
// use the range beacon data as a second preference
|
|
stateStruct.position.x = receiverPos.x;
|
|
stateStruct.position.y = receiverPos.y;
|
|
// set the variances from the beacon alignment filter
|
|
P[7][7] = receiverPosCov[0][0];
|
|
P[8][8] = receiverPosCov[1][1];
|
|
// clear the timeout flags and counters
|
|
rngBcnTimeout = false;
|
|
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
|
}
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.x = stateStruct.position.x;
|
|
storedOutput[i].position.y = stateStruct.position.y;
|
|
}
|
|
outputDataNew.position.x = stateStruct.position.x;
|
|
outputDataNew.position.y = stateStruct.position.y;
|
|
outputDataDelayed.position.x = stateStruct.position.x;
|
|
outputDataDelayed.position.y = stateStruct.position.y;
|
|
|
|
// Calculate the position jump due to the reset
|
|
posResetNE.x = stateStruct.position.x - posResetNE.x;
|
|
posResetNE.y = stateStruct.position.y - posResetNE.y;
|
|
|
|
// store the time of the reset
|
|
lastPosReset_ms = imuSampleTime_ms;
|
|
|
|
// clear reset source preference
|
|
posResetSource = DEFAULT;
|
|
|
|
}
|
|
|
|
// reset the vertical position state using the last height measurement
|
|
void NavEKF3_core::ResetHeight(void)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetD = stateStruct.position.z;
|
|
|
|
// write to the state vector
|
|
stateStruct.position.z = -hgtMea;
|
|
outputDataNew.position.z = stateStruct.position.z;
|
|
outputDataDelayed.position.z = stateStruct.position.z;
|
|
|
|
// reset the terrain state height
|
|
if (onGround) {
|
|
// assume vehicle is sitting on the ground
|
|
terrainState = stateStruct.position.z + rngOnGnd;
|
|
} else {
|
|
// can make no assumption other than vehicle is not below ground level
|
|
terrainState = MAX(stateStruct.position.z + rngOnGnd , terrainState);
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.z = stateStruct.position.z;
|
|
}
|
|
|
|
// Calculate the position jump due to the reset
|
|
posResetD = stateStruct.position.z - posResetD;
|
|
|
|
// store the time of the reset
|
|
lastPosResetD_ms = imuSampleTime_ms;
|
|
|
|
// clear the timeout flags and counters
|
|
hgtTimeout = false;
|
|
lastHgtPassTime_ms = imuSampleTime_ms;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,9,9);
|
|
zeroCols(P,9,9);
|
|
|
|
// set the variances to the measurement variance
|
|
P[9][9] = posDownObsNoise;
|
|
|
|
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
|
|
// Check that GPS vertical velocity data is available and can be used
|
|
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0) {
|
|
stateStruct.velocity.z = gpsDataNew.vel.z;
|
|
} else if (onGround) {
|
|
stateStruct.velocity.z = 0.0f;
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].velocity.z = stateStruct.velocity.z;
|
|
}
|
|
outputDataNew.velocity.z = stateStruct.velocity.z;
|
|
outputDataDelayed.velocity.z = stateStruct.velocity.z;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,6,6);
|
|
zeroCols(P,6,6);
|
|
|
|
// set the variances to the measurement variance
|
|
P[6][6] = sq(frontend->_gpsVertVelNoise);
|
|
|
|
}
|
|
|
|
// Zero the EKF height datum
|
|
// Return true if the height datum reset has been performed
|
|
bool NavEKF3_core::resetHeightDatum(void)
|
|
{
|
|
if (activeHgtSource == HGT_SOURCE_RNG) {
|
|
// by definition the height datum is at ground level so cannot perform the reset
|
|
return false;
|
|
}
|
|
// record the old height estimate
|
|
float oldHgt = -stateStruct.position.z;
|
|
// reset the barometer so that it reads zero at the current height
|
|
frontend->_baro.update_calibration();
|
|
// reset the height state
|
|
stateStruct.position.z = 0.0f;
|
|
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
|
|
if (validOrigin) {
|
|
ekfGpsRefHgt += (double)oldHgt;
|
|
}
|
|
// adjust the terrain state
|
|
terrainState += oldHgt;
|
|
return true;
|
|
}
|
|
|
|
/********************************************************
|
|
* FUSE MEASURED_DATA *
|
|
********************************************************/
|
|
// select fusion of velocity, position and height measurements
|
|
void NavEKF3_core::SelectVelPosFusion()
|
|
{
|
|
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
|
|
// If so, don't fuse measurements on this time step to reduce frame over-runs
|
|
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
|
|
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
|
|
posVelFusionDelayed = true;
|
|
return;
|
|
} else {
|
|
posVelFusionDelayed = false;
|
|
}
|
|
|
|
// read GPS data from the sensor and check for new data in the buffer
|
|
readGpsData();
|
|
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
|
|
// Determine if we need to fuse position and velocity data on this time step
|
|
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
|
|
// correct GPS data for position offset of antenna phase centre relative to the IMU
|
|
Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
|
|
if (!posOffsetBody.is_zero()) {
|
|
if (fuseVelData) {
|
|
// TODO use a filtered angular rate with a group delay that matches the GPS delay
|
|
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
|
|
Vector3f velOffsetBody = angRate % posOffsetBody;
|
|
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
|
|
gpsDataDelayed.vel -= velOffsetEarth;
|
|
}
|
|
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
|
gpsDataDelayed.pos.x -= posOffsetEarth.x;
|
|
gpsDataDelayed.pos.y -= posOffsetEarth.y;
|
|
gpsDataDelayed.hgt += posOffsetEarth.z;
|
|
}
|
|
|
|
|
|
// Don't fuse velocity data if GPS doesn't support it
|
|
if (frontend->_fusionModeGPS <= 1) {
|
|
fuseVelData = true;
|
|
} else {
|
|
fuseVelData = false;
|
|
}
|
|
fusePosData = true;
|
|
|
|
} else {
|
|
fuseVelData = false;
|
|
fusePosData = false;
|
|
}
|
|
|
|
// we have GPS data to fuse and a request to align the yaw using the GPS course
|
|
if (gpsYawResetRequest) {
|
|
realignYawGPS();
|
|
}
|
|
|
|
// Select height data to be fused from the available baro, range finder and GPS sources
|
|
selectHeightForFusion();
|
|
|
|
// if we are using GPS, check for a change in receiver and reset position and height
|
|
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) {
|
|
// record the ID of the GPS that we are using for the reset
|
|
last_gps_idx = gpsDataDelayed.sensor_idx;
|
|
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetNE.x = stateStruct.position.x;
|
|
posResetNE.y = stateStruct.position.y;
|
|
|
|
// Set the position states to the position from the new GPS
|
|
stateStruct.position.x = gpsDataNew.pos.x;
|
|
stateStruct.position.y = gpsDataNew.pos.y;
|
|
|
|
// Calculate the position offset due to the reset
|
|
posResetNE.x = stateStruct.position.x - posResetNE.x;
|
|
posResetNE.y = stateStruct.position.y - posResetNE.y;
|
|
|
|
// Add the offset to the output observer states
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.x += posResetNE.x;
|
|
storedOutput[i].position.y += posResetNE.y;
|
|
}
|
|
outputDataNew.position.x += posResetNE.x;
|
|
outputDataNew.position.y += posResetNE.y;
|
|
outputDataDelayed.position.x += posResetNE.x;
|
|
outputDataDelayed.position.y += posResetNE.y;
|
|
|
|
// store the time of the reset
|
|
lastPosReset_ms = imuSampleTime_ms;
|
|
|
|
// If we are alseo using GPS as the height reference, reset the height
|
|
if (activeHgtSource == HGT_SOURCE_GPS) {
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetD = stateStruct.position.z;
|
|
|
|
// write to the state vector
|
|
stateStruct.position.z = -hgtMea;
|
|
|
|
// Calculate the position jump due to the reset
|
|
posResetD = stateStruct.position.z - posResetD;
|
|
|
|
// Add the offset to the output observer states
|
|
outputDataNew.position.z += posResetD;
|
|
outputDataDelayed.position.z += posResetD;
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.z += posResetD;
|
|
}
|
|
|
|
// store the time of the reset
|
|
lastPosResetD_ms = imuSampleTime_ms;
|
|
}
|
|
}
|
|
|
|
// If we are operating without any aiding, fuse in the last known position
|
|
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
|
|
// Do this to coincide with the height fusion
|
|
if (fuseHgtData && PV_AidingMode == AID_NONE) {
|
|
gpsDataDelayed.vel.zero();
|
|
gpsDataDelayed.pos.x = lastKnownPositionNE.x;
|
|
gpsDataDelayed.pos.y = lastKnownPositionNE.y;
|
|
|
|
fusePosData = true;
|
|
fuseVelData = false;
|
|
}
|
|
|
|
// perform fusion
|
|
if (fuseVelData || fusePosData || fuseHgtData) {
|
|
FuseVelPosNED();
|
|
// clear the flags to prevent repeated fusion of the same data
|
|
fuseVelData = false;
|
|
fuseHgtData = false;
|
|
fusePosData = false;
|
|
}
|
|
}
|
|
|
|
// fuse selected position, velocity and height measurements
|
|
void NavEKF3_core::FuseVelPosNED()
|
|
{
|
|
// start performance timer
|
|
hal.util->perf_begin(_perf_FuseVelPosNED);
|
|
|
|
// health is set bad until test passed
|
|
velHealth = false;
|
|
posHealth = false;
|
|
hgtHealth = false;
|
|
|
|
// declare variables used to check measurement errors
|
|
Vector3f velInnov;
|
|
|
|
// declare variables used to control access to arrays
|
|
bool fuseData[6] = {false,false,false,false,false,false};
|
|
uint8_t stateIndex;
|
|
uint8_t obsIndex;
|
|
|
|
// declare variables used by state and covariance update calculations
|
|
Vector6 R_OBS; // Measurement variances used for fusion
|
|
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
|
|
Vector6 observation;
|
|
float SK;
|
|
|
|
// perform sequential fusion of GPS measurements. This assumes that the
|
|
// errors in the different velocity and position components are
|
|
// uncorrelated which is not true, however in the absence of covariance
|
|
// data from the GPS receiver it is the only assumption we can make
|
|
// so we might as well take advantage of the computational efficiencies
|
|
// associated with sequential fusion
|
|
if (fuseVelData || fusePosData || fuseHgtData) {
|
|
// form the observation vector
|
|
observation[0] = gpsDataDelayed.vel.x;
|
|
observation[1] = gpsDataDelayed.vel.y;
|
|
observation[2] = gpsDataDelayed.vel.z;
|
|
observation[3] = gpsDataDelayed.pos.x;
|
|
observation[4] = gpsDataDelayed.pos.y;
|
|
observation[5] = -hgtMea;
|
|
|
|
// calculate additional error in GPS position caused by manoeuvring
|
|
float posErr = frontend->gpsPosVarAccScale * accNavMag;
|
|
|
|
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
|
// Use different errors if operating without external aiding using an assumed position or velocity of zero
|
|
if (PV_AidingMode == AID_NONE) {
|
|
if (tiltAlignComplete && motorsArmed) {
|
|
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
|
|
R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f));
|
|
} else {
|
|
// Use a smaller value to give faster initial alignment
|
|
R_OBS[0] = sq(0.5f);
|
|
}
|
|
R_OBS[1] = R_OBS[0];
|
|
R_OBS[2] = R_OBS[0];
|
|
R_OBS[3] = R_OBS[0];
|
|
R_OBS[4] = R_OBS[0];
|
|
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
|
} else {
|
|
if (gpsSpdAccuracy > 0.0f) {
|
|
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
|
|
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
|
|
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
|
|
} else {
|
|
// calculate additional error in GPS velocity caused by manoeuvring
|
|
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
|
R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
|
|
}
|
|
R_OBS[1] = R_OBS[0];
|
|
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
|
|
if (gpsPosAccuracy > 0.0f) {
|
|
R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
|
|
} else {
|
|
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
|
}
|
|
R_OBS[4] = R_OBS[3];
|
|
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
|
|
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
|
|
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
|
|
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
|
}
|
|
R_OBS[5] = posDownObsNoise;
|
|
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
|
|
|
// if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
|
|
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
|
|
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
|
|
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
|
|
// calculate innovations for height and vertical GPS vel measurements
|
|
float hgtErr = stateStruct.position.z - observation[5];
|
|
float velDErr = stateStruct.velocity.z - observation[2];
|
|
// check if they are the same sign and both more than 3-sigma out of bounds
|
|
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) {
|
|
badIMUdata = true;
|
|
} else {
|
|
badIMUdata = false;
|
|
}
|
|
}
|
|
|
|
// calculate innovations and check GPS data validity using an innovation consistency check
|
|
// test position measurements
|
|
if (fusePosData) {
|
|
// test horizontal position measurements
|
|
innovVelPos[3] = stateStruct.position.x - observation[3];
|
|
innovVelPos[4] = stateStruct.position.y - observation[4];
|
|
varInnovVelPos[3] = P[7][7] + R_OBS_DATA_CHECKS[3];
|
|
varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4];
|
|
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
|
float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
|
|
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
|
|
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
|
|
// use position data if healthy or timed out
|
|
if (PV_AidingMode == AID_NONE) {
|
|
posHealth = true;
|
|
lastPosPassTime_ms = imuSampleTime_ms;
|
|
} else if (posHealth || posTimeout) {
|
|
posHealth = true;
|
|
lastPosPassTime_ms = imuSampleTime_ms;
|
|
// if timed out or outside the specified uncertainty radius, reset to the GPS
|
|
if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
|
|
// reset the position to the current GPS position
|
|
ResetPosition();
|
|
// reset the velocity to the GPS velocity
|
|
ResetVelocity();
|
|
// don't fuse GPS data on this time step
|
|
fusePosData = false;
|
|
fuseVelData = false;
|
|
// Reset the position variances and corresponding covariances to a value that will pass the checks
|
|
zeroRows(P,7,8);
|
|
zeroCols(P,7,8);
|
|
P[7][7] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
|
|
P[8][8] = P[7][7];
|
|
// Reset the normalised innovation to avoid failing the bad fusion tests
|
|
posTestRatio = 0.0f;
|
|
velTestRatio = 0.0f;
|
|
}
|
|
} else {
|
|
posHealth = false;
|
|
}
|
|
}
|
|
|
|
// test velocity measurements
|
|
if (fuseVelData) {
|
|
// test velocity measurements
|
|
uint8_t imax = 2;
|
|
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
|
|
if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
|
|
imax = 1;
|
|
}
|
|
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
|
float varVelSum = 0; // sum of velocity innovation variances
|
|
for (uint8_t i = 0; i<=imax; i++) {
|
|
// velocity states start at index 4
|
|
stateIndex = i + 4;
|
|
// calculate innovations using blended and single IMU predicted states
|
|
velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended
|
|
// calculate innovation variance
|
|
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
|
|
// sum the innovation and innovation variances
|
|
innovVelSumSq += sq(velInnov[i]);
|
|
varVelSum += varInnovVelPos[i];
|
|
}
|
|
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
|
// calculate the test ratio
|
|
velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
|
|
// fail if the ratio is greater than 1
|
|
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
|
|
// use velocity data if healthy, timed out, or in constant position mode
|
|
if (velHealth || velTimeout) {
|
|
velHealth = true;
|
|
// restart the timeout count
|
|
lastVelPassTime_ms = imuSampleTime_ms;
|
|
// If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
|
|
if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
|
|
// reset the velocity to the GPS velocity
|
|
ResetVelocity();
|
|
// don't fuse GPS velocity data on this time step
|
|
fuseVelData = false;
|
|
// Reset the normalised innovation to avoid failing the bad fusion tests
|
|
velTestRatio = 0.0f;
|
|
}
|
|
} else {
|
|
velHealth = false;
|
|
}
|
|
}
|
|
|
|
// test height measurements
|
|
if (fuseHgtData) {
|
|
// calculate height innovations
|
|
innovVelPos[5] = stateStruct.position.z - observation[5];
|
|
varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5];
|
|
// calculate the innovation consistency test ratio
|
|
hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
|
|
// fail if the ratio is > 1, but don't fail if bad IMU data
|
|
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
|
|
// Fuse height data if healthy or timed out or in constant position mode
|
|
if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) {
|
|
// Calculate a filtered value to be used by pre-flight health checks
|
|
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
|
|
if (onGround) {
|
|
float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
|
|
const float hgtInnovFiltTC = 2.0f;
|
|
float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
|
|
hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
|
|
} else {
|
|
hgtInnovFiltState = 0.0f;
|
|
}
|
|
|
|
// if timed out, reset the height
|
|
if (hgtTimeout) {
|
|
ResetHeight();
|
|
}
|
|
|
|
// If we have got this far then declare the height data as healthy and reset the timeout counter
|
|
hgtHealth = true;
|
|
lastHgtPassTime_ms = imuSampleTime_ms;
|
|
}
|
|
}
|
|
|
|
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
|
|
if (fuseVelData && velHealth) {
|
|
fuseData[0] = true;
|
|
fuseData[1] = true;
|
|
if (useGpsVertVel) {
|
|
fuseData[2] = true;
|
|
}
|
|
}
|
|
if (fusePosData && posHealth) {
|
|
fuseData[3] = true;
|
|
fuseData[4] = true;
|
|
}
|
|
if (fuseHgtData && hgtHealth) {
|
|
fuseData[5] = true;
|
|
}
|
|
|
|
// fuse measurements sequentially
|
|
for (obsIndex=0; obsIndex<=5; obsIndex++) {
|
|
if (fuseData[obsIndex]) {
|
|
stateIndex = 4 + obsIndex;
|
|
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
|
|
// adjust scaling on GPS measurement noise variances if not enough satellites
|
|
if (obsIndex <= 2)
|
|
{
|
|
innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex];
|
|
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
|
}
|
|
else if (obsIndex == 3 || obsIndex == 4) {
|
|
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
|
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
|
} else if (obsIndex == 5) {
|
|
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
|
const float gndMaxBaroErr = 4.0f;
|
|
const float gndBaroInnovFloor = -0.5f;
|
|
|
|
if(getTouchdownExpected() && activeHgtSource == HGT_SOURCE_BARO) {
|
|
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
|
|
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
|
|
// this function looks like this:
|
|
// |/
|
|
//---------|---------
|
|
// ____/|
|
|
// / |
|
|
// / |
|
|
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
|
|
}
|
|
}
|
|
|
|
// calculate the Kalman gain and calculate innovation variances
|
|
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
|
|
SK = 1.0f/varInnovVelPos[obsIndex];
|
|
for (uint8_t i= 0; i<=9; i++) {
|
|
Kfusion[i] = P[i][stateIndex]*SK;
|
|
}
|
|
|
|
// inhibit delta angle bias state estmation by setting Kalman gains to zero
|
|
if (!inhibitDelAngBiasStates) {
|
|
for (uint8_t i = 10; i<=12; i++) {
|
|
Kfusion[i] = P[i][stateIndex]*SK;
|
|
}
|
|
} else {
|
|
// zero indexes 10 to 12 = 3*4 bytes
|
|
memset(&Kfusion[10], 0, 12);
|
|
}
|
|
|
|
// inhibit delta velocity bias state estmation by setting Kalman gains to zero
|
|
if (!inhibitDelVelBiasStates) {
|
|
for (uint8_t i = 13; i<=15; i++) {
|
|
Kfusion[i] = P[i][stateIndex]*SK;
|
|
}
|
|
} else {
|
|
// zero indexes 13 to 15 = 3*4 bytes
|
|
memset(&Kfusion[13], 0, 12);
|
|
}
|
|
|
|
// inhibit magnetic field state estimation by setting Kalman gains to zero
|
|
if (!inhibitMagStates) {
|
|
for (uint8_t i = 16; i<=21; i++) {
|
|
Kfusion[i] = P[i][stateIndex]*SK;
|
|
}
|
|
} else {
|
|
// zero indexes 16 to 21 = 6*4 bytes
|
|
memset(&Kfusion[16], 0, 24);
|
|
}
|
|
|
|
// inhibit wind state estimation by setting Kalman gains to zero
|
|
if (!inhibitWindStates) {
|
|
Kfusion[22] = P[22][stateIndex]*SK;
|
|
Kfusion[23] = P[23][stateIndex]*SK;
|
|
} else {
|
|
// zero indexes 22 to 23 = 2*4 bytes
|
|
memset(&Kfusion[22], 0, 8);
|
|
}
|
|
|
|
// update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
|
|
// this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
for (uint8_t j= 0; j<=stateIndexLim; j++)
|
|
{
|
|
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
|
|
}
|
|
}
|
|
// Check that we are not going to drive any variances negative and skip the update if so
|
|
bool healthyFusion = true;
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
if (KHP[i][i] > P[i][i]) {
|
|
healthyFusion = false;
|
|
}
|
|
}
|
|
if (healthyFusion) {
|
|
// update the covariance matrix
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
|
P[i][j] = P[i][j] - KHP[i][j];
|
|
}
|
|
}
|
|
|
|
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
|
|
ForceSymmetry();
|
|
ConstrainVariances();
|
|
|
|
// update states and renormalise the quaternions
|
|
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
|
statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
|
|
}
|
|
stateStruct.quat.normalize();
|
|
|
|
// record good fusion status
|
|
if (obsIndex == 0) {
|
|
faultStatus.bad_nvel = false;
|
|
} else if (obsIndex == 1) {
|
|
faultStatus.bad_evel = false;
|
|
} else if (obsIndex == 2) {
|
|
faultStatus.bad_dvel = false;
|
|
} else if (obsIndex == 3) {
|
|
faultStatus.bad_npos = false;
|
|
} else if (obsIndex == 4) {
|
|
faultStatus.bad_epos = false;
|
|
} else if (obsIndex == 5) {
|
|
faultStatus.bad_dpos = false;
|
|
}
|
|
} else {
|
|
// record bad fusion status
|
|
if (obsIndex == 0) {
|
|
faultStatus.bad_nvel = true;
|
|
} else if (obsIndex == 1) {
|
|
faultStatus.bad_evel = true;
|
|
} else if (obsIndex == 2) {
|
|
faultStatus.bad_dvel = true;
|
|
} else if (obsIndex == 3) {
|
|
faultStatus.bad_npos = true;
|
|
} else if (obsIndex == 4) {
|
|
faultStatus.bad_epos = true;
|
|
} else if (obsIndex == 5) {
|
|
faultStatus.bad_dpos = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// stop performance timer
|
|
hal.util->perf_end(_perf_FuseVelPosNED);
|
|
}
|
|
|
|
/********************************************************
|
|
* MISC FUNCTIONS *
|
|
********************************************************/
|
|
|
|
// select the height measurement to be fused from the available baro, range finder and GPS sources
|
|
void NavEKF3_core::selectHeightForFusion()
|
|
{
|
|
// Read range finder data and check for new data in the buffer
|
|
// This data is used by both height and optical flow fusion processing
|
|
readRangeFinder();
|
|
rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
|
|
|
|
// correct range data for the body frame position offset relative to the IMU
|
|
// the corrected reading is the reading that would have been taken if the sensor was
|
|
// co-located with the IMU
|
|
if (rangeDataToFuse) {
|
|
Vector3f posOffsetBody = frontend->_rng.get_pos_offset(rangeDataDelayed.sensor_idx) - accelPosOffset;
|
|
if (!posOffsetBody.is_zero()) {
|
|
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
|
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
|
|
}
|
|
}
|
|
|
|
// read baro height data from the sensor and check for new data in the buffer
|
|
readBaroData();
|
|
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
|
|
|
// select height source
|
|
if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
|
|
if (frontend->_altSource == 1) {
|
|
// always use range finder
|
|
activeHgtSource = HGT_SOURCE_RNG;
|
|
} else {
|
|
// determine if we are above or below the height switch region
|
|
float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
|
|
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
|
|
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
|
|
|
|
// If the terrain height is consistent and we are moving slowly, then it can be
|
|
// used as a height reference in combination with a range finder
|
|
// apply a hysteresis to the speed check to prevent rapid switching
|
|
bool dontTrustTerrain, trustTerrain;
|
|
if (filterStatus.flags.horiz_vel) {
|
|
// We can use the velocity estimate
|
|
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
|
|
dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable;
|
|
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
|
|
trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
|
|
} else {
|
|
// We can't use the velocity estimate
|
|
dontTrustTerrain = !terrainHgtStable;
|
|
trustTerrain = terrainHgtStable;
|
|
}
|
|
|
|
/*
|
|
* Switch between range finder and primary height source using height above ground and speed thresholds with
|
|
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
|
|
* which cannot be assumed if the vehicle is moving horizontally.
|
|
*/
|
|
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
|
|
// cannot trust terrain or range finder so stop using range finder height
|
|
if (frontend->_altSource == 0) {
|
|
activeHgtSource = HGT_SOURCE_BARO;
|
|
} else if (frontend->_altSource == 2) {
|
|
activeHgtSource = HGT_SOURCE_GPS;
|
|
}
|
|
} else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) {
|
|
// reliable terrain and range finder so start using range finder height
|
|
activeHgtSource = HGT_SOURCE_RNG;
|
|
}
|
|
}
|
|
} else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
|
|
activeHgtSource = HGT_SOURCE_GPS;
|
|
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
|
|
activeHgtSource = HGT_SOURCE_BCN;
|
|
} else {
|
|
activeHgtSource = HGT_SOURCE_BARO;
|
|
}
|
|
|
|
// Use Baro alt as a fallback if we lose range finder or GPS
|
|
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500));
|
|
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
|
|
if (lostRngHgt || lostGpsHgt) {
|
|
activeHgtSource = HGT_SOURCE_BARO;
|
|
}
|
|
|
|
// if there is new baro data to fuse, calculate filtered baro data required by other processes
|
|
if (baroDataToFuse) {
|
|
// calculate offset to baro data that enables us to switch to Baro height use during operation
|
|
if (activeHgtSource != HGT_SOURCE_BARO) {
|
|
calcFiltBaroOffset();
|
|
}
|
|
// filtered baro data used to provide a reference for takeoff
|
|
// it is is reset to last height measurement on disarming in performArmingChecks()
|
|
if (!getTakeoffExpected()) {
|
|
const float gndHgtFiltTC = 0.5f;
|
|
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
|
|
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
|
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
|
}
|
|
}
|
|
|
|
// If we are not using GPS as the primary height sensor, correct EKF origin height so that
|
|
// combined local NED position height and origin height remains consistent with the GPS altitude
|
|
// This also enables the GPS height to be used as a backup height source
|
|
if (gpsDataToFuse &&
|
|
(((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) ||
|
|
((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG)))
|
|
) {
|
|
correctEkfOriginHeight();
|
|
}
|
|
|
|
// Select the height measurement source
|
|
if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
|
|
// using range finder data
|
|
// correct for tilt using a flat earth model
|
|
if (prevTnb.c.z >= 0.7) {
|
|
// calculate height above ground
|
|
hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
|
|
// correct for terrain position relative to datum
|
|
hgtMea -= terrainState;
|
|
// enable fusion
|
|
fuseHgtData = true;
|
|
// set the observation noise
|
|
posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
|
|
// add uncertainty created by terrain gradient and vehicle tilt
|
|
posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
|
|
} else {
|
|
// disable fusion if tilted too far
|
|
fuseHgtData = false;
|
|
}
|
|
} else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) {
|
|
// using GPS data
|
|
hgtMea = gpsDataDelayed.hgt;
|
|
// enable fusion
|
|
fuseHgtData = true;
|
|
// set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
|
|
if (gpsHgtAccuracy > 0.0f) {
|
|
posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
|
|
} else {
|
|
posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
|
|
}
|
|
} else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) {
|
|
// using Baro data
|
|
hgtMea = baroDataDelayed.hgt - baroHgtOffset;
|
|
// enable fusion
|
|
fuseHgtData = true;
|
|
// set the observation noise
|
|
posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
|
|
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
|
if (getTakeoffExpected() || getTouchdownExpected()) {
|
|
posDownObsNoise *= frontend->gndEffectBaroScaler;
|
|
}
|
|
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
|
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
|
|
if (motorsArmed && getTakeoffExpected()) {
|
|
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
|
|
}
|
|
} else {
|
|
fuseHgtData = false;
|
|
}
|
|
|
|
// If we haven't fused height data for a while, then declare the height data as being timed out
|
|
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
|
|
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
|
|
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
|
|
hgtTimeout = true;
|
|
} else {
|
|
hgtTimeout = false;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Fuse body frame velocity measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
|
|
* The script file used to generate these and other equations in this filter can be found here:
|
|
* https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
|
|
*/
|
|
void NavEKF3_core::FuseBodyVel()
|
|
{
|
|
Vector24 H_VEL;
|
|
Vector3f bodyVelPred;
|
|
|
|
// Copy required states to local variable names
|
|
float q0 = stateStruct.quat[0];
|
|
float q1 = stateStruct.quat[1];
|
|
float q2 = stateStruct.quat[2];
|
|
float q3 = stateStruct.quat[3];
|
|
float vn = stateStruct.velocity.x;
|
|
float ve = stateStruct.velocity.y;
|
|
float vd = stateStruct.velocity.z;
|
|
|
|
// Fuse X, Y and Z axis measurements sequentially assuming observation errors are uncorrelated
|
|
for (uint8_t obsIndex=0; obsIndex<=2; obsIndex++) {
|
|
|
|
// calculate relative velocity in sensor frame including the relative motion due to rotation
|
|
bodyVelPred = (prevTnb * stateStruct.velocity);
|
|
|
|
// correct sensor offset body frame position offset relative to IMU
|
|
Vector3f posOffsetBody = (*bodyOdmDataDelayed.body_offset) - accelPosOffset;
|
|
|
|
// correct prediction for relative motion due to rotation
|
|
// note - % operator overloaded for cross product
|
|
if (imuDataDelayed.delAngDT > 0.001f) {
|
|
bodyVelPred += (imuDataDelayed.delAng * (1.0f / imuDataDelayed.delAngDT)) % posOffsetBody;
|
|
}
|
|
|
|
// calculate observation jacobians and Kalman gains
|
|
if (obsIndex == 0) {
|
|
// calculate X axis observation Jacobian
|
|
H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
|
|
H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
|
|
H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f;
|
|
H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
|
|
H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3;
|
|
H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f;
|
|
H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f;
|
|
for (uint8_t index = 7; index < 24; index++) {
|
|
H_VEL[index] = 0.0f;
|
|
}
|
|
|
|
// calculate intermediate expressions for X axis Kalman gains
|
|
float R_VEL = bodyOdmDataDelayed.velErr;
|
|
float t2 = q0*q3*2.0f;
|
|
float t3 = q1*q2*2.0f;
|
|
float t4 = t2+t3;
|
|
float t5 = q0*q0;
|
|
float t6 = q1*q1;
|
|
float t7 = q2*q2;
|
|
float t8 = q3*q3;
|
|
float t9 = t5+t6-t7-t8;
|
|
float t10 = q0*q2*2.0f;
|
|
float t25 = q1*q3*2.0f;
|
|
float t11 = t10-t25;
|
|
float t12 = q3*ve*2.0f;
|
|
float t13 = q0*vn*2.0f;
|
|
float t26 = q2*vd*2.0f;
|
|
float t14 = t12+t13-t26;
|
|
float t15 = q3*vd*2.0f;
|
|
float t16 = q2*ve*2.0f;
|
|
float t17 = q1*vn*2.0f;
|
|
float t18 = t15+t16+t17;
|
|
float t19 = q0*vd*2.0f;
|
|
float t20 = q2*vn*2.0f;
|
|
float t27 = q1*ve*2.0f;
|
|
float t21 = t19+t20-t27;
|
|
float t22 = q1*vd*2.0f;
|
|
float t23 = q0*ve*2.0f;
|
|
float t28 = q3*vn*2.0f;
|
|
float t24 = t22+t23-t28;
|
|
float t29 = P[0][0]*t14;
|
|
float t30 = P[1][1]*t18;
|
|
float t31 = P[4][5]*t9;
|
|
float t32 = P[5][5]*t4;
|
|
float t33 = P[0][5]*t14;
|
|
float t34 = P[1][5]*t18;
|
|
float t35 = P[3][5]*t24;
|
|
float t79 = P[6][5]*t11;
|
|
float t80 = P[2][5]*t21;
|
|
float t36 = t31+t32+t33+t34+t35-t79-t80;
|
|
float t37 = t4*t36;
|
|
float t38 = P[4][6]*t9;
|
|
float t39 = P[5][6]*t4;
|
|
float t40 = P[0][6]*t14;
|
|
float t41 = P[1][6]*t18;
|
|
float t42 = P[3][6]*t24;
|
|
float t81 = P[6][6]*t11;
|
|
float t82 = P[2][6]*t21;
|
|
float t43 = t38+t39+t40+t41+t42-t81-t82;
|
|
float t44 = P[4][0]*t9;
|
|
float t45 = P[5][0]*t4;
|
|
float t46 = P[1][0]*t18;
|
|
float t47 = P[3][0]*t24;
|
|
float t84 = P[6][0]*t11;
|
|
float t85 = P[2][0]*t21;
|
|
float t48 = t29+t44+t45+t46+t47-t84-t85;
|
|
float t49 = t14*t48;
|
|
float t50 = P[4][1]*t9;
|
|
float t51 = P[5][1]*t4;
|
|
float t52 = P[0][1]*t14;
|
|
float t53 = P[3][1]*t24;
|
|
float t86 = P[6][1]*t11;
|
|
float t87 = P[2][1]*t21;
|
|
float t54 = t30+t50+t51+t52+t53-t86-t87;
|
|
float t55 = t18*t54;
|
|
float t56 = P[4][2]*t9;
|
|
float t57 = P[5][2]*t4;
|
|
float t58 = P[0][2]*t14;
|
|
float t59 = P[1][2]*t18;
|
|
float t60 = P[3][2]*t24;
|
|
float t78 = P[2][2]*t21;
|
|
float t88 = P[6][2]*t11;
|
|
float t61 = t56+t57+t58+t59+t60-t78-t88;
|
|
float t62 = P[4][3]*t9;
|
|
float t63 = P[5][3]*t4;
|
|
float t64 = P[0][3]*t14;
|
|
float t65 = P[1][3]*t18;
|
|
float t66 = P[3][3]*t24;
|
|
float t90 = P[6][3]*t11;
|
|
float t91 = P[2][3]*t21;
|
|
float t67 = t62+t63+t64+t65+t66-t90-t91;
|
|
float t68 = t24*t67;
|
|
float t69 = P[4][4]*t9;
|
|
float t70 = P[5][4]*t4;
|
|
float t71 = P[0][4]*t14;
|
|
float t72 = P[1][4]*t18;
|
|
float t73 = P[3][4]*t24;
|
|
float t92 = P[6][4]*t11;
|
|
float t93 = P[2][4]*t21;
|
|
float t74 = t69+t70+t71+t72+t73-t92-t93;
|
|
float t75 = t9*t74;
|
|
float t83 = t11*t43;
|
|
float t89 = t21*t61;
|
|
float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89;
|
|
float t77;
|
|
|
|
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
|
if (t76 > R_VEL) {
|
|
t77 = 1.0f/t76;
|
|
faultStatus.bad_xvel = false;
|
|
} else {
|
|
t76 = R_VEL;
|
|
t77 = 1.0f/R_VEL;
|
|
faultStatus.bad_xvel = true;
|
|
return;
|
|
}
|
|
varInnovBodyVel[0] = t77;
|
|
|
|
// calculate innovation for X axis observation
|
|
innovBodyVel[0] = bodyVelPred.x - bodyOdmDataDelayed.vel.x;
|
|
|
|
// calculate Kalman gains for X-axis observation
|
|
Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24);
|
|
Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24);
|
|
Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24);
|
|
Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21);
|
|
Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24);
|
|
Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24);
|
|
Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24);
|
|
Kfusion[7] = t77*(P[7][5]*t4+P[7][4]*t9+P[7][0]*t14-P[7][6]*t11+P[7][1]*t18-P[7][2]*t21+P[7][3]*t24);
|
|
Kfusion[8] = t77*(P[8][5]*t4+P[8][4]*t9+P[8][0]*t14-P[8][6]*t11+P[8][1]*t18-P[8][2]*t21+P[8][3]*t24);
|
|
Kfusion[9] = t77*(P[9][5]*t4+P[9][4]*t9+P[9][0]*t14-P[9][6]*t11+P[9][1]*t18-P[9][2]*t21+P[9][3]*t24);
|
|
|
|
if (!inhibitDelAngBiasStates) {
|
|
Kfusion[10] = t77*(P[10][5]*t4+P[10][4]*t9+P[10][0]*t14-P[10][6]*t11+P[10][1]*t18-P[10][2]*t21+P[10][3]*t24);
|
|
Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24);
|
|
Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24);
|
|
} else {
|
|
// zero indexes 10 to 12 = 3*4 bytes
|
|
memset(&Kfusion[10], 0, 12);
|
|
}
|
|
|
|
if (!inhibitDelVelBiasStates) {
|
|
Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24);
|
|
Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24);
|
|
Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24);
|
|
} else {
|
|
// zero indexes 13 to 15 = 3*4 bytes
|
|
memset(&Kfusion[13], 0, 12);
|
|
}
|
|
|
|
if (!inhibitMagStates) {
|
|
Kfusion[16] = t77*(P[16][5]*t4+P[16][4]*t9+P[16][0]*t14-P[16][6]*t11+P[16][1]*t18-P[16][2]*t21+P[16][3]*t24);
|
|
Kfusion[17] = t77*(P[17][5]*t4+P[17][4]*t9+P[17][0]*t14-P[17][6]*t11+P[17][1]*t18-P[17][2]*t21+P[17][3]*t24);
|
|
Kfusion[18] = t77*(P[18][5]*t4+P[18][4]*t9+P[18][0]*t14-P[18][6]*t11+P[18][1]*t18-P[18][2]*t21+P[18][3]*t24);
|
|
Kfusion[19] = t77*(P[19][5]*t4+P[19][4]*t9+P[19][0]*t14-P[19][6]*t11+P[19][1]*t18-P[19][2]*t21+P[19][3]*t24);
|
|
Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24);
|
|
Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24);
|
|
} else {
|
|
// zero indexes 16 to 21 = 6*4 bytes
|
|
memset(&Kfusion[16], 0, 24);
|
|
}
|
|
|
|
if (!inhibitWindStates) {
|
|
Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24);
|
|
Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24);
|
|
} else {
|
|
// zero indexes 22 to 23 = 2*4 bytes
|
|
memset(&Kfusion[22], 0, 8);
|
|
}
|
|
} else if (obsIndex == 1) {
|
|
// calculate Y axis observation Jacobian
|
|
H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
|
|
H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
|
|
H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
|
|
H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f;
|
|
H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f;
|
|
H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3;
|
|
H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f;
|
|
for (uint8_t index = 7; index < 24; index++) {
|
|
H_VEL[index] = 0.0f;
|
|
}
|
|
|
|
// calculate intermediate expressions for Y axis Kalman gains
|
|
float R_VEL = bodyOdmDataDelayed.velErr;
|
|
float t2 = q0*q3*2.0f;
|
|
float t9 = q1*q2*2.0f;
|
|
float t3 = t2-t9;
|
|
float t4 = q0*q0;
|
|
float t5 = q1*q1;
|
|
float t6 = q2*q2;
|
|
float t7 = q3*q3;
|
|
float t8 = t4-t5+t6-t7;
|
|
float t10 = q0*q1*2.0f;
|
|
float t11 = q2*q3*2.0f;
|
|
float t12 = t10+t11;
|
|
float t13 = q1*vd*2.0f;
|
|
float t14 = q0*ve*2.0f;
|
|
float t26 = q3*vn*2.0f;
|
|
float t15 = t13+t14-t26;
|
|
float t16 = q0*vd*2.0f;
|
|
float t17 = q2*vn*2.0f;
|
|
float t27 = q1*ve*2.0f;
|
|
float t18 = t16+t17-t27;
|
|
float t19 = q3*vd*2.0f;
|
|
float t20 = q2*ve*2.0f;
|
|
float t21 = q1*vn*2.0f;
|
|
float t22 = t19+t20+t21;
|
|
float t23 = q3*ve*2.0f;
|
|
float t24 = q0*vn*2.0f;
|
|
float t28 = q2*vd*2.0f;
|
|
float t25 = t23+t24-t28;
|
|
float t29 = P[0][0]*t15;
|
|
float t30 = P[1][1]*t18;
|
|
float t31 = P[5][4]*t8;
|
|
float t32 = P[6][4]*t12;
|
|
float t33 = P[0][4]*t15;
|
|
float t34 = P[1][4]*t18;
|
|
float t35 = P[2][4]*t22;
|
|
float t78 = P[4][4]*t3;
|
|
float t79 = P[3][4]*t25;
|
|
float t36 = t31+t32+t33+t34+t35-t78-t79;
|
|
float t37 = P[5][6]*t8;
|
|
float t38 = P[6][6]*t12;
|
|
float t39 = P[0][6]*t15;
|
|
float t40 = P[1][6]*t18;
|
|
float t41 = P[2][6]*t22;
|
|
float t81 = P[4][6]*t3;
|
|
float t82 = P[3][6]*t25;
|
|
float t42 = t37+t38+t39+t40+t41-t81-t82;
|
|
float t43 = t12*t42;
|
|
float t44 = P[5][0]*t8;
|
|
float t45 = P[6][0]*t12;
|
|
float t46 = P[1][0]*t18;
|
|
float t47 = P[2][0]*t22;
|
|
float t83 = P[4][0]*t3;
|
|
float t84 = P[3][0]*t25;
|
|
float t48 = t29+t44+t45+t46+t47-t83-t84;
|
|
float t49 = t15*t48;
|
|
float t50 = P[5][1]*t8;
|
|
float t51 = P[6][1]*t12;
|
|
float t52 = P[0][1]*t15;
|
|
float t53 = P[2][1]*t22;
|
|
float t85 = P[4][1]*t3;
|
|
float t86 = P[3][1]*t25;
|
|
float t54 = t30+t50+t51+t52+t53-t85-t86;
|
|
float t55 = t18*t54;
|
|
float t56 = P[5][2]*t8;
|
|
float t57 = P[6][2]*t12;
|
|
float t58 = P[0][2]*t15;
|
|
float t59 = P[1][2]*t18;
|
|
float t60 = P[2][2]*t22;
|
|
float t87 = P[4][2]*t3;
|
|
float t88 = P[3][2]*t25;
|
|
float t61 = t56+t57+t58+t59+t60-t87-t88;
|
|
float t62 = t22*t61;
|
|
float t63 = P[5][3]*t8;
|
|
float t64 = P[6][3]*t12;
|
|
float t65 = P[0][3]*t15;
|
|
float t66 = P[1][3]*t18;
|
|
float t67 = P[2][3]*t22;
|
|
float t89 = P[4][3]*t3;
|
|
float t90 = P[3][3]*t25;
|
|
float t68 = t63+t64+t65+t66+t67-t89-t90;
|
|
float t69 = P[5][5]*t8;
|
|
float t70 = P[6][5]*t12;
|
|
float t71 = P[0][5]*t15;
|
|
float t72 = P[1][5]*t18;
|
|
float t73 = P[2][5]*t22;
|
|
float t92 = P[4][5]*t3;
|
|
float t93 = P[3][5]*t25;
|
|
float t74 = t69+t70+t71+t72+t73-t92-t93;
|
|
float t75 = t8*t74;
|
|
float t80 = t3*t36;
|
|
float t91 = t25*t68;
|
|
float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91;
|
|
float t77;
|
|
|
|
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
|
|
if (t76 > R_VEL) {
|
|
t77 = 1.0f/t76;
|
|
faultStatus.bad_yvel = false;
|
|
} else {
|
|
t76 = R_VEL;
|
|
t77 = 1.0f/R_VEL;
|
|
faultStatus.bad_yvel = true;
|
|
return;
|
|
}
|
|
varInnovBodyVel[1] = t77;
|
|
|
|
// calculate innovation for Y axis observation
|
|
innovBodyVel[1] = bodyVelPred.y - bodyOdmDataDelayed.vel.y;
|
|
|
|
// calculate Kalman gains for Y-axis observation
|
|
Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25);
|
|
Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25);
|
|
Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25);
|
|
Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22);
|
|
Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25);
|
|
Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25);
|
|
Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25);
|
|
Kfusion[7] = t77*(-P[7][4]*t3+P[7][5]*t8+P[7][0]*t15+P[7][6]*t12+P[7][1]*t18+P[7][2]*t22-P[7][3]*t25);
|
|
Kfusion[8] = t77*(-P[8][4]*t3+P[8][5]*t8+P[8][0]*t15+P[8][6]*t12+P[8][1]*t18+P[8][2]*t22-P[8][3]*t25);
|
|
Kfusion[9] = t77*(-P[9][4]*t3+P[9][5]*t8+P[9][0]*t15+P[9][6]*t12+P[9][1]*t18+P[9][2]*t22-P[9][3]*t25);
|
|
|
|
if (!inhibitDelAngBiasStates) {
|
|
Kfusion[10] = t77*(-P[10][4]*t3+P[10][5]*t8+P[10][0]*t15+P[10][6]*t12+P[10][1]*t18+P[10][2]*t22-P[10][3]*t25);
|
|
Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25);
|
|
Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25);
|
|
} else {
|
|
// zero indexes 10 to 12 = 3*4 bytes
|
|
memset(&Kfusion[10], 0, 12);
|
|
}
|
|
|
|
if (!inhibitDelVelBiasStates) {
|
|
Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25);
|
|
Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25);
|
|
Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25);
|
|
} else {
|
|
// zero indexes 13 to 15 = 3*4 bytes
|
|
memset(&Kfusion[13], 0, 12);
|
|
}
|
|
|
|
if (!inhibitMagStates) {
|
|
Kfusion[16] = t77*(-P[16][4]*t3+P[16][5]*t8+P[16][0]*t15+P[16][6]*t12+P[16][1]*t18+P[16][2]*t22-P[16][3]*t25);
|
|
Kfusion[17] = t77*(-P[17][4]*t3+P[17][5]*t8+P[17][0]*t15+P[17][6]*t12+P[17][1]*t18+P[17][2]*t22-P[17][3]*t25);
|
|
Kfusion[18] = t77*(-P[18][4]*t3+P[18][5]*t8+P[18][0]*t15+P[18][6]*t12+P[18][1]*t18+P[18][2]*t22-P[18][3]*t25);
|
|
Kfusion[19] = t77*(-P[19][4]*t3+P[19][5]*t8+P[19][0]*t15+P[19][6]*t12+P[19][1]*t18+P[19][2]*t22-P[19][3]*t25);
|
|
Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25);
|
|
Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25);
|
|
} else {
|
|
// zero indexes 16 to 21 = 6*4 bytes
|
|
memset(&Kfusion[16], 0, 24);
|
|
}
|
|
|
|
if (!inhibitWindStates) {
|
|
Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25);
|
|
Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25);
|
|
} else {
|
|
// zero indexes 22 to 23 = 2*4 bytes
|
|
memset(&Kfusion[22], 0, 8);
|
|
}
|
|
} else if (obsIndex == 2) {
|
|
// calculate Z axis observation Jacobian
|
|
H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
|
|
H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f;
|
|
H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
|
|
H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
|
|
H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f;
|
|
H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f;
|
|
H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3;
|
|
for (uint8_t index = 7; index < 24; index++) {
|
|
H_VEL[index] = 0.0f;
|
|
}
|
|
|
|
// calculate intermediate expressions for Z axis Kalman gains
|
|
float R_VEL = bodyOdmDataDelayed.velErr;
|
|
float t2 = q0*q2*2.0f;
|
|
float t3 = q1*q3*2.0f;
|
|
float t4 = t2+t3;
|
|
float t5 = q0*q0;
|
|
float t6 = q1*q1;
|
|
float t7 = q2*q2;
|
|
float t8 = q3*q3;
|
|
float t9 = t5-t6-t7+t8;
|
|
float t10 = q0*q1*2.0f;
|
|
float t25 = q2*q3*2.0f;
|
|
float t11 = t10-t25;
|
|
float t12 = q0*vd*2.0f;
|
|
float t13 = q2*vn*2.0f;
|
|
float t26 = q1*ve*2.0f;
|
|
float t14 = t12+t13-t26;
|
|
float t15 = q1*vd*2.0f;
|
|
float t16 = q0*ve*2.0f;
|
|
float t27 = q3*vn*2.0f;
|
|
float t17 = t15+t16-t27;
|
|
float t18 = q3*ve*2.0f;
|
|
float t19 = q0*vn*2.0f;
|
|
float t28 = q2*vd*2.0f;
|
|
float t20 = t18+t19-t28;
|
|
float t21 = q3*vd*2.0f;
|
|
float t22 = q2*ve*2.0f;
|
|
float t23 = q1*vn*2.0f;
|
|
float t24 = t21+t22+t23;
|
|
float t29 = P[0][0]*t14;
|
|
float t30 = P[6][4]*t9;
|
|
float t31 = P[4][4]*t4;
|
|
float t32 = P[0][4]*t14;
|
|
float t33 = P[2][4]*t20;
|
|
float t34 = P[3][4]*t24;
|
|
float t78 = P[5][4]*t11;
|
|
float t79 = P[1][4]*t17;
|
|
float t35 = t30+t31+t32+t33+t34-t78-t79;
|
|
float t36 = t4*t35;
|
|
float t37 = P[6][5]*t9;
|
|
float t38 = P[4][5]*t4;
|
|
float t39 = P[0][5]*t14;
|
|
float t40 = P[2][5]*t20;
|
|
float t41 = P[3][5]*t24;
|
|
float t80 = P[5][5]*t11;
|
|
float t81 = P[1][5]*t17;
|
|
float t42 = t37+t38+t39+t40+t41-t80-t81;
|
|
float t43 = P[6][0]*t9;
|
|
float t44 = P[4][0]*t4;
|
|
float t45 = P[2][0]*t20;
|
|
float t46 = P[3][0]*t24;
|
|
float t83 = P[5][0]*t11;
|
|
float t84 = P[1][0]*t17;
|
|
float t47 = t29+t43+t44+t45+t46-t83-t84;
|
|
float t48 = t14*t47;
|
|
float t49 = P[6][1]*t9;
|
|
float t50 = P[4][1]*t4;
|
|
float t51 = P[0][1]*t14;
|
|
float t52 = P[2][1]*t20;
|
|
float t53 = P[3][1]*t24;
|
|
float t85 = P[5][1]*t11;
|
|
float t86 = P[1][1]*t17;
|
|
float t54 = t49+t50+t51+t52+t53-t85-t86;
|
|
float t55 = P[6][2]*t9;
|
|
float t56 = P[4][2]*t4;
|
|
float t57 = P[0][2]*t14;
|
|
float t58 = P[2][2]*t20;
|
|
float t59 = P[3][2]*t24;
|
|
float t88 = P[5][2]*t11;
|
|
float t89 = P[1][2]*t17;
|
|
float t60 = t55+t56+t57+t58+t59-t88-t89;
|
|
float t61 = t20*t60;
|
|
float t62 = P[6][3]*t9;
|
|
float t63 = P[4][3]*t4;
|
|
float t64 = P[0][3]*t14;
|
|
float t65 = P[2][3]*t20;
|
|
float t66 = P[3][3]*t24;
|
|
float t90 = P[5][3]*t11;
|
|
float t91 = P[1][3]*t17;
|
|
float t67 = t62+t63+t64+t65+t66-t90-t91;
|
|
float t68 = t24*t67;
|
|
float t69 = P[6][6]*t9;
|
|
float t70 = P[4][6]*t4;
|
|
float t71 = P[0][6]*t14;
|
|
float t72 = P[2][6]*t20;
|
|
float t73 = P[3][6]*t24;
|
|
float t92 = P[5][6]*t11;
|
|
float t93 = P[1][6]*t17;
|
|
float t74 = t69+t70+t71+t72+t73-t92-t93;
|
|
float t75 = t9*t74;
|
|
float t82 = t11*t42;
|
|
float t87 = t17*t54;
|
|
float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87;
|
|
float t77;
|
|
|
|
// calculate innovation variance for Z axis observation and protect against a badly conditioned calculation
|
|
if (t76 > R_VEL) {
|
|
t77 = 1.0f/t76;
|
|
faultStatus.bad_zvel = false;
|
|
} else {
|
|
t76 = R_VEL;
|
|
t77 = 1.0f/R_VEL;
|
|
faultStatus.bad_zvel = true;
|
|
return;
|
|
}
|
|
varInnovBodyVel[2] = t77;
|
|
|
|
// calculate innovation for Z axis observation
|
|
innovBodyVel[2] = bodyVelPred.z - bodyOdmDataDelayed.vel.z;
|
|
|
|
// calculate Kalman gains for X-axis observation
|
|
Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24);
|
|
Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24);
|
|
Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24);
|
|
Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20);
|
|
Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24);
|
|
Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24);
|
|
Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24);
|
|
Kfusion[7] = t77*(P[7][4]*t4+P[7][0]*t14+P[7][6]*t9-P[7][5]*t11-P[7][1]*t17+P[7][2]*t20+P[7][3]*t24);
|
|
Kfusion[8] = t77*(P[8][4]*t4+P[8][0]*t14+P[8][6]*t9-P[8][5]*t11-P[8][1]*t17+P[8][2]*t20+P[8][3]*t24);
|
|
Kfusion[9] = t77*(P[9][4]*t4+P[9][0]*t14+P[9][6]*t9-P[9][5]*t11-P[9][1]*t17+P[9][2]*t20+P[9][3]*t24);
|
|
|
|
if (!inhibitDelAngBiasStates) {
|
|
Kfusion[10] = t77*(P[10][4]*t4+P[10][0]*t14+P[10][6]*t9-P[10][5]*t11-P[10][1]*t17+P[10][2]*t20+P[10][3]*t24);
|
|
Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24);
|
|
Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24);
|
|
} else {
|
|
// zero indexes 10 to 12 = 3*4 bytes
|
|
memset(&Kfusion[10], 0, 12);
|
|
|
|
}
|
|
|
|
if (!inhibitDelVelBiasStates) {
|
|
Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24);
|
|
Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24);
|
|
Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24);
|
|
} else {
|
|
// zero indexes 13 to 15 = 3*4 bytes
|
|
memset(&Kfusion[13], 0, 12);
|
|
}
|
|
|
|
if (!inhibitMagStates) {
|
|
Kfusion[16] = t77*(P[16][4]*t4+P[16][0]*t14+P[16][6]*t9-P[16][5]*t11-P[16][1]*t17+P[16][2]*t20+P[16][3]*t24);
|
|
Kfusion[17] = t77*(P[17][4]*t4+P[17][0]*t14+P[17][6]*t9-P[17][5]*t11-P[17][1]*t17+P[17][2]*t20+P[17][3]*t24);
|
|
Kfusion[18] = t77*(P[18][4]*t4+P[18][0]*t14+P[18][6]*t9-P[18][5]*t11-P[18][1]*t17+P[18][2]*t20+P[18][3]*t24);
|
|
Kfusion[19] = t77*(P[19][4]*t4+P[19][0]*t14+P[19][6]*t9-P[19][5]*t11-P[19][1]*t17+P[19][2]*t20+P[19][3]*t24);
|
|
Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24);
|
|
Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24);
|
|
} else {
|
|
// zero indexes 16 to 21 = 6*4 bytes
|
|
memset(&Kfusion[16], 0, 24);
|
|
}
|
|
|
|
if (!inhibitWindStates) {
|
|
Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24);
|
|
Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24);
|
|
} else {
|
|
// zero indexes 22 to 23 = 2*4 bytes
|
|
memset(&Kfusion[22], 0, 8);
|
|
}
|
|
} else {
|
|
return;
|
|
}
|
|
|
|
// calculate the innovation consistency test ratio
|
|
// TODO add tuning parameter for gate
|
|
bodyVelTestRatio[obsIndex] = sq(innovBodyVel[obsIndex]) / (sq(5.0f) * varInnovBodyVel[obsIndex]);
|
|
|
|
// Check the innovation for consistency and don't fuse if out of bounds
|
|
// TODO also apply angular velocity magnitude check
|
|
if ((bodyVelTestRatio[obsIndex]) < 1.0f) {
|
|
// record the last time observations were accepted for fusion
|
|
prevBodyVelFuseTime_ms = imuSampleTime_ms;
|
|
// notify first time only
|
|
if (!bodyVelFusionActive) {
|
|
bodyVelFusionActive = true;
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing odometry",(unsigned)imu_index);
|
|
}
|
|
// correct the covariance P = (I - K*H)*P
|
|
// take advantage of the empty columns in KH to reduce the
|
|
// number of operations
|
|
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
|
for (unsigned j = 0; j<=6; j++) {
|
|
KH[i][j] = Kfusion[i] * H_VEL[j];
|
|
}
|
|
for (unsigned j = 7; j<=stateIndexLim; j++) {
|
|
KH[i][j] = 0.0f;
|
|
}
|
|
}
|
|
for (unsigned j = 0; j<=stateIndexLim; j++) {
|
|
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
|
ftype res = 0;
|
|
res += KH[i][0] * P[0][j];
|
|
res += KH[i][1] * P[1][j];
|
|
res += KH[i][2] * P[2][j];
|
|
res += KH[i][3] * P[3][j];
|
|
res += KH[i][4] * P[4][j];
|
|
res += KH[i][5] * P[5][j];
|
|
res += KH[i][6] * P[6][j];
|
|
KHP[i][j] = res;
|
|
}
|
|
}
|
|
|
|
// Check that we are not going to drive any variances negative and skip the update if so
|
|
bool healthyFusion = true;
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
if (KHP[i][i] > P[i][i]) {
|
|
healthyFusion = false;
|
|
}
|
|
}
|
|
|
|
if (healthyFusion) {
|
|
// update the covariance matrix
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
|
P[i][j] = P[i][j] - KHP[i][j];
|
|
}
|
|
}
|
|
|
|
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
|
|
ForceSymmetry();
|
|
ConstrainVariances();
|
|
|
|
// correct the state vector
|
|
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
|
statesArray[j] = statesArray[j] - Kfusion[j] * innovBodyVel[obsIndex];
|
|
}
|
|
stateStruct.quat.normalize();
|
|
|
|
} else {
|
|
// record bad axis
|
|
if (obsIndex == 0) {
|
|
faultStatus.bad_xvel = true;
|
|
} else if (obsIndex == 1) {
|
|
faultStatus.bad_yvel = true;
|
|
} else if (obsIndex == 2) {
|
|
faultStatus.bad_zvel = true;
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// select fusion of body odometry measurements
|
|
void NavEKF3_core::SelectBodyOdomFusion()
|
|
{
|
|
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
|
|
// If so, don't fuse measurements on this time step to reduce frame over-runs
|
|
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
|
|
if (magFusePerformed && (dtIMUavg < 0.005f) && !bodyVelFusionDelayed) {
|
|
bodyVelFusionDelayed = true;
|
|
return;
|
|
} else {
|
|
bodyVelFusionDelayed = false;
|
|
}
|
|
|
|
// Check for data at the fusion time horizon
|
|
if (storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms)) {
|
|
|
|
// start performance timer
|
|
hal.util->perf_begin(_perf_FuseBodyOdom);
|
|
|
|
// Fuse data into the main filter
|
|
FuseBodyVel();
|
|
|
|
// stop the performance timer
|
|
hal.util->perf_end(_perf_FuseBodyOdom);
|
|
}
|
|
}
|
|
|
|
#endif // HAL_CPU_CLASS
|