mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AP_NavEKF3: add fusion method for body frame odometry data
This commit is contained in:
priseborough
Randy Mackay
parent
595d37ec70
commit
fb7104f4e3
@ -1108,6 +1108,40 @@ void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Write body frame linear and angular displacement measurements from a visual odometry sensor
|
||||
*
|
||||
* quality is a normalised confidence value from 0 to 100
|
||||
* delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m)
|
||||
* delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
|
||||
* delTime is the time interval for the measurement of delPos and delAng (sec)
|
||||
* timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
|
||||
* posOffset is the XYZ body frame position of the camera focal point (m)
|
||||
*/
|
||||
void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)
|
||||
{
|
||||
if (core) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
core[i].writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// return data for debugging body frame odometry fusion
|
||||
uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar)
|
||||
{
|
||||
uint32_t ret = 0;
|
||||
if (instance < 0 || instance >= num_cores) {
|
||||
instance = primary;
|
||||
}
|
||||
|
||||
if (core) {
|
||||
ret = core[instance].getBodyFrameOdomDebug(velInnov, velInnovVar);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
// return data for debugging range beacon fusion
|
||||
bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
|
||||
float &offsetHigh, float &offsetLow, Vector3f &posNED)
|
||||
|
||||
@ -196,6 +196,28 @@ public:
|
||||
// posOffset is the XYZ flow sensor position in the body frame in m
|
||||
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset);
|
||||
|
||||
/*
|
||||
* Write body frame linear and angular displacement measurements from a visual odometry sensor
|
||||
*
|
||||
* quality is a normalised confidence value from 0 to 100
|
||||
* delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m)
|
||||
* delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
|
||||
* delTime is the time interval for the measurement of delPos and delAng (sec)
|
||||
* timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
|
||||
* posOffset is the XYZ body frame position of the camera focal point (m)
|
||||
*/
|
||||
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
|
||||
|
||||
/*
|
||||
* Return data for debugging body frame odometry fusion:
|
||||
*
|
||||
* velInnov are the XYZ body frame velocity innovations (m/s)
|
||||
* velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2
|
||||
*
|
||||
* Return the system time stamp of the last update (msec)
|
||||
*/
|
||||
uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar);
|
||||
|
||||
// return data for debugging optical flow fusion for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr);
|
||||
|
||||
@ -191,7 +191,7 @@ void NavEKF3_core::setAidingMode()
|
||||
// GPS aiding is the preferred option unless excluded by the user
|
||||
if(readyToUseGPS() || readyToUseRangeBeacon()) {
|
||||
PV_AidingMode = AID_ABSOLUTE;
|
||||
} else if (readyToUseOptFlow()) {
|
||||
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
|
||||
PV_AidingMode = AID_RELATIVE;
|
||||
}
|
||||
} else if (PV_AidingMode == AID_RELATIVE) {
|
||||
@ -199,11 +199,15 @@ void NavEKF3_core::setAidingMode()
|
||||
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
|
||||
// Check if the fusion has timed out (flow measurements have been rejected for too long)
|
||||
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
|
||||
// Check if the body odometry flow sensor has timed out
|
||||
bool bodyOdmSensorTimeout = ((imuSampleTime_ms - bodyOdmMeasTime_ms) > 5000);
|
||||
// Check if the fusion has timed out (body odometry measurements have been rejected for too long)
|
||||
bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
|
||||
// Enable switch to absolute position mode if GPS or range beacon data is available
|
||||
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
|
||||
if(readyToUseGPS() || readyToUseRangeBeacon()) {
|
||||
PV_AidingMode = AID_ABSOLUTE;
|
||||
} else if (flowSensorTimeout || flowFusionTimeout) {
|
||||
} else if ((flowSensorTimeout || flowFusionTimeout) && (bodyOdmSensorTimeout || bodyOdmFusionTimeout)) {
|
||||
PV_AidingMode = AID_NONE;
|
||||
}
|
||||
} else if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
@ -213,6 +217,9 @@ void NavEKF3_core::setAidingMode()
|
||||
// Check if optical flow data is being used
|
||||
bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
|
||||
|
||||
// Check if body odometry data is being used
|
||||
bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_ms <= minTestTime_ms);
|
||||
|
||||
// Check if airspeed data is being used
|
||||
bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
|
||||
|
||||
@ -224,10 +231,10 @@ void NavEKF3_core::setAidingMode()
|
||||
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
|
||||
|
||||
// Check if attitude drift has been constrained by a measurement source
|
||||
bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
|
||||
bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed;
|
||||
|
||||
// check if velocity drift has been constrained by a measurement source
|
||||
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
|
||||
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed;
|
||||
|
||||
// check if position drift has been constrained by a measurement source
|
||||
bool posAiding = gpsPosUsed || rngBcnUsed;
|
||||
@ -278,7 +285,7 @@ void NavEKF3_core::setAidingMode()
|
||||
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
|
||||
if (PV_AidingMode == AID_NONE) {
|
||||
// We have ceased aiding
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u has stopped aiding",(unsigned)imu_index);
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);
|
||||
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
@ -293,15 +300,23 @@ void NavEKF3_core::setAidingMode()
|
||||
meaHgtAtTakeOff = baroDataDelayed.hgt;
|
||||
// reset the vertical position state to faster recover from baro errors experienced during touchdown
|
||||
stateStruct.position.z = -meaHgtAtTakeOff;
|
||||
// reset relative aiding sensor fusion activity status
|
||||
flowFusionActive = false;
|
||||
bodyVelFusionActive = false;
|
||||
} else if (PV_AidingMode == AID_RELATIVE) {
|
||||
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using optical flow",(unsigned)imu_index);
|
||||
// We are doing relative position navigation where velocity errors are constrained, but position drift will occur
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
|
||||
if (readyToUseOptFlow()) {
|
||||
// Reset time stamps
|
||||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||||
} else if (readyToUseBodyOdm()) {
|
||||
// Reset time stamps
|
||||
lastbodyVelPassTime_ms = imuSampleTime_ms;
|
||||
prevBodyVelFuseTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
// Reset the last valid flow measurement time
|
||||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||||
// Reset the last valid flow fusion time
|
||||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||||
} else if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
if (readyToUseGPS()) {
|
||||
// We are commencing aiding using GPS - this is the preferred method
|
||||
@ -376,6 +391,16 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
|
||||
return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
|
||||
}
|
||||
|
||||
// return true if the filter is ready to start using body frame odometry measurements
|
||||
bool NavEKF3_core::readyToUseBodyOdm(void) const
|
||||
{
|
||||
// We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use these measurements
|
||||
return (imuSampleTime_ms - bodyOdmMeasTime_ms < 200)
|
||||
&& bodyOdmDataNew.velErr < 1.0f
|
||||
&& tiltAlignComplete
|
||||
&& delAngBiasLearned;
|
||||
}
|
||||
|
||||
// return true if the filter to be ready to use gps
|
||||
bool NavEKF3_core::readyToUseGPS(void) const
|
||||
{
|
||||
@ -467,13 +492,12 @@ void NavEKF3_core::updateFilterStatus(void)
|
||||
{
|
||||
// init return value
|
||||
filterStatus.value = 0;
|
||||
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
|
||||
bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000);
|
||||
bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid;
|
||||
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
|
||||
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
|
||||
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
|
||||
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
|
||||
bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
|
||||
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
|
||||
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav || doingBodyVelNav;
|
||||
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
|
||||
|
||||
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
|
||||
@ -483,13 +507,13 @@ void NavEKF3_core::updateFilterStatus(void)
|
||||
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
|
||||
filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
|
||||
filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
|
||||
filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
|
||||
filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy; // relative horizontal position estimate valid
|
||||
filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
|
||||
filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
|
||||
filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
|
||||
filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
|
||||
filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
|
||||
filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
|
||||
filterStatus.flags.pred_horiz_pos_rel = filterStatus.flags.horiz_pos_rel; // EKF3 enters the required mode before flight
|
||||
filterStatus.flags.pred_horiz_pos_abs = filterStatus.flags.horiz_pos_abs; // EKF3 enters the required mode before flight
|
||||
filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
|
||||
filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
|
||||
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
|
||||
|
||||
@ -106,6 +106,30 @@ void NavEKF3_core::readRangeFinder(void)
|
||||
}
|
||||
}
|
||||
|
||||
void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)
|
||||
{
|
||||
// limit update rate to maximum allowed by sensor buffers and fusion process
|
||||
// don't try to write to buffer until the filter has been initialised
|
||||
if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
bodyOdmDataNew.body_offset = &posOffset;
|
||||
bodyOdmDataNew.vel = delPos * (1.0f/delTime);
|
||||
bodyOdmDataNew.time_ms = timeStamp_ms;
|
||||
bodyOdmDataNew.angRate = delAng * (1.0f/delTime);
|
||||
bodyOdmMeasTime_ms = timeStamp_ms;
|
||||
|
||||
// simple model of accuracy
|
||||
// TODO move this calculation outside of EKF into the sensor driver
|
||||
const float minVelErr = 0.5f;
|
||||
const float maxVelErr = 10.0f;
|
||||
bodyOdmDataNew.velErr = minVelErr + (maxVelErr - minVelErr) * (1.0f - 0.01f * quality);
|
||||
|
||||
storedBodyOdm.push(bodyOdmDataNew);
|
||||
|
||||
}
|
||||
|
||||
// write the raw optical flow measurements
|
||||
// this needs to be called externally.
|
||||
void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#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;
|
||||
|
||||
@ -647,7 +648,11 @@ void NavEKF3_core::FuseOptFlow()
|
||||
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
|
||||
// record the last time observations were accepted for fusion
|
||||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||||
|
||||
// notify first time only
|
||||
if (!flowFusionActive) {
|
||||
flowFusionActive = true;
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(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
|
||||
|
||||
@ -67,6 +67,18 @@ void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
|
||||
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
|
||||
}
|
||||
|
||||
// return data for debugging body frame odometry fusion
|
||||
uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar)
|
||||
{
|
||||
velInnov.x = innovBodyVel[0];
|
||||
velInnov.y = innovBodyVel[1];
|
||||
velInnov.z = innovBodyVel[2];
|
||||
velInnovVar.x = varInnovBodyVel[0];
|
||||
velInnovVar.y = varInnovBodyVel[1];
|
||||
velInnovVar.z = varInnovBodyVel[2];
|
||||
return bodyOdmDataDelayed.time_ms;
|
||||
}
|
||||
|
||||
// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
|
||||
bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
|
||||
float &offsetHigh, float &offsetLow, Vector3f &posNED)
|
||||
@ -339,7 +351,13 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
|
||||
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
|
||||
void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
|
||||
{
|
||||
if (PV_AidingMode == AID_RELATIVE) {
|
||||
// If in the last 10 seconds we have received flow data and no odometry data, then we are relying on optical flow
|
||||
bool relyingOnFlowData = (imuSampleTime_ms - prevBodyVelFuseTime_ms > 1000)
|
||||
&& (imuSampleTime_ms - flowValidMeaTime_ms <= 10000);
|
||||
|
||||
// If relying on optical flow, limit speed to prevent sensor limit being exceeded and adjust
|
||||
// nav gains to prevent body rate feedback into flow rates destabilising the control loop
|
||||
if (PV_AidingMode == AID_RELATIVE && relyingOnFlowData) {
|
||||
// allow 1.0 rad/sec margin for angular motion
|
||||
ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd);
|
||||
// use standard gains up to 5.0 metres height and reduce above that
|
||||
|
||||
@ -8,6 +8,7 @@
|
||||
#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;
|
||||
|
||||
@ -886,4 +887,627 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* 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);
|
||||
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);
|
||||
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);
|
||||
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 {
|
||||
for (uint8_t i = 16; i <= 21; i++) {
|
||||
Kfusion[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
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 {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
} 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);
|
||||
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);
|
||||
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);
|
||||
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 {
|
||||
for (uint8_t i = 16; i <= 21; i++) {
|
||||
Kfusion[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
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 {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
} 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);
|
||||
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);
|
||||
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);
|
||||
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 {
|
||||
for (uint8_t i = 16; i <= 21; i++) {
|
||||
Kfusion[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
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 {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
} 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
|
||||
|
||||
@ -23,7 +23,8 @@ NavEKF3_core::NavEKF3_core(void) :
|
||||
_perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseAirspeed")),
|
||||
_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_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_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");
|
||||
@ -122,6 +123,9 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
|
||||
if(!storedOF.init(obs_buffer_length)) {
|
||||
return false;
|
||||
}
|
||||
if(!storedBodyOdm.init(obs_buffer_length)) {
|
||||
return false;
|
||||
}
|
||||
// Note: the use of dual range finders potentially doubles the amount of data to be stored
|
||||
if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) {
|
||||
return false;
|
||||
@ -279,6 +283,7 @@ void NavEKF3_core::InitialiseVariables()
|
||||
posDown = 0.0f;
|
||||
posVelFusionDelayed = false;
|
||||
optFlowFusionDelayed = false;
|
||||
flowFusionActive = false;
|
||||
airSpdFusionDelayed = false;
|
||||
sideSlipFusionDelayed = false;
|
||||
posResetNE.zero();
|
||||
@ -356,6 +361,19 @@ void NavEKF3_core::InitialiseVariables()
|
||||
bcnPosOffsetNED.zero();
|
||||
bcnOriginEstInit = false;
|
||||
|
||||
// body frame displacement fusion
|
||||
memset(&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew));
|
||||
memset(&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed));
|
||||
bodyOdmStoreIndex = 0;
|
||||
lastbodyVelPassTime_ms = 0;
|
||||
memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio));
|
||||
memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel));
|
||||
memset(&innovBodyVel, 0, sizeof(innovBodyVel));
|
||||
prevBodyVelFuseTime_ms = 0;
|
||||
bodyOdmMeasTime_ms = 0;
|
||||
bodyVelFusionDelayed = false;
|
||||
bodyVelFusionActive = false;
|
||||
|
||||
// zero data buffers
|
||||
storedIMU.reset();
|
||||
storedGPS.reset();
|
||||
@ -365,6 +383,7 @@ void NavEKF3_core::InitialiseVariables()
|
||||
storedRange.reset();
|
||||
storedOutput.reset();
|
||||
storedRangeBeacon.reset();
|
||||
storedBodyOdm.reset();
|
||||
}
|
||||
|
||||
// Initialise the states from accelerometer and magnetometer data (if present)
|
||||
@ -548,6 +567,9 @@ void NavEKF3_core::UpdateFilter(bool predict)
|
||||
// Update states using optical flow data
|
||||
SelectFlowFusion();
|
||||
|
||||
// Update states using body frame odometry data
|
||||
SelectBodyOdomFusion();
|
||||
|
||||
// Update states using airspeed data
|
||||
SelectTasFusion();
|
||||
|
||||
|
||||
@ -202,6 +202,28 @@ public:
|
||||
// return data for debugging optical flow fusion
|
||||
void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
|
||||
|
||||
/*
|
||||
* Write body frame linear and angular displacement measurements from a visual odometry sensor
|
||||
*
|
||||
* quality is a normalised confidence value from 0 to 100
|
||||
* delPos is the XYZ change in linear position meaasured in body frame and relative to the inertial reference at time_ms (m)
|
||||
* delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at time_ms (rad)
|
||||
* delTime is the time interval for the measurement of delPos and delAng (sec)
|
||||
* timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
|
||||
* posOffset is the XYZ body frame position of the camera focal point (m)
|
||||
*/
|
||||
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
|
||||
|
||||
/*
|
||||
* Return data for debugging body frame odometry fusion:
|
||||
*
|
||||
* velInnov are the XYZ body frame velocity innovations (m/s)
|
||||
* velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2
|
||||
*
|
||||
* Return the time stamp of the last odometry fusion update (msec)
|
||||
*/
|
||||
uint32_t getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar);
|
||||
|
||||
/*
|
||||
Returns the following data for debugging range beacon fusion
|
||||
ID : beacon identifier
|
||||
@ -376,73 +398,81 @@ private:
|
||||
// memory)
|
||||
Vector24 statesArray;
|
||||
struct state_elements {
|
||||
Quaternion quat; // 0..3
|
||||
Vector3f velocity; // 4..6
|
||||
Vector3f position; // 7..9
|
||||
Vector3f gyro_bias; // 10..12
|
||||
Vector3f accel_bias; // 13..15
|
||||
Vector3f earth_magfield; // 16..18
|
||||
Vector3f body_magfield; // 19..21
|
||||
Vector2f wind_vel; // 22..23
|
||||
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame
|
||||
Vector3f velocity; // velocity of IMU in local NED earth frame (m/sec)
|
||||
Vector3f position; // position of IMU in local NED earth frame (m)
|
||||
Vector3f gyro_bias; // body frame delta angle IMU bias vector (rad)
|
||||
Vector3f accel_bias; // body frame delta velocity IMU bias vector (m/sec)
|
||||
Vector3f earth_magfield; // earth frame magnetic field vector (Gauss)
|
||||
Vector3f body_magfield; // body frame magnetic field vector (Gauss)
|
||||
Vector2f wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec)
|
||||
} &stateStruct;
|
||||
|
||||
struct output_elements {
|
||||
Quaternion quat; // 0..3
|
||||
Vector3f velocity; // 4..6
|
||||
Vector3f position; // 7..9
|
||||
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame
|
||||
Vector3f velocity; // velocity of body frame origin in local NED earth frame (m/sec)
|
||||
Vector3f position; // position of body frame origin in local NED earth frame (m)
|
||||
};
|
||||
|
||||
struct imu_elements {
|
||||
Vector3f delAng; // 0..2
|
||||
Vector3f delVel; // 3..5
|
||||
float delAngDT; // 6
|
||||
float delVelDT; // 7
|
||||
uint32_t time_ms; // 8
|
||||
Vector3f delAng; // IMU delta angle measurements in body frame (rad)
|
||||
Vector3f delVel; // IMU delta velocity measurements in body frame (m/sec)
|
||||
float delAngDT; // time interval over which delAng has been measured (sec)
|
||||
float delVelDT; // time interval over which delVelDT has been measured (sec)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
};
|
||||
|
||||
struct gps_elements {
|
||||
Vector2f pos; // 0..1
|
||||
float hgt; // 2
|
||||
Vector3f vel; // 3..5
|
||||
uint32_t time_ms; // 6
|
||||
uint8_t sensor_idx; // 7
|
||||
Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m)
|
||||
float hgt; // height of the GPS antenna in local NED earth frame (m)
|
||||
Vector3f vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
uint8_t sensor_idx; // unique integer identifying the GPS sensor
|
||||
};
|
||||
|
||||
struct mag_elements {
|
||||
Vector3f mag; // 0..2
|
||||
uint32_t time_ms; // 3
|
||||
Vector3f mag; // body frame magnetic field measurements (Gauss)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
};
|
||||
|
||||
struct baro_elements {
|
||||
float hgt; // 0
|
||||
uint32_t time_ms; // 1
|
||||
float hgt; // height of the pressure sensor in local NED earth frame (m)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
};
|
||||
|
||||
struct range_elements {
|
||||
float rng; // 0
|
||||
uint32_t time_ms; // 1
|
||||
uint8_t sensor_idx; // 2
|
||||
float rng; // distance measured by the range sensor (m)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors
|
||||
};
|
||||
|
||||
struct rng_bcn_elements {
|
||||
float rng; // range measurement to each beacon (m)
|
||||
Vector3f beacon_posNED; // NED position of the beacon (m)
|
||||
float rngErr; // range measurement error 1-std (m)
|
||||
uint8_t beacon_ID; // beacon identification number
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
float rng; // range measurement to each beacon (m)
|
||||
Vector3f beacon_posNED; // NED position of the beacon (m)
|
||||
float rngErr; // range measurement error 1-std (m)
|
||||
uint8_t beacon_ID; // beacon identification number
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
};
|
||||
|
||||
struct tas_elements {
|
||||
float tas; // 0
|
||||
uint32_t time_ms; // 1
|
||||
float tas; // true airspeed measurement (m/sec)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
};
|
||||
|
||||
struct of_elements {
|
||||
Vector2f flowRadXY; // 0..1
|
||||
Vector2f flowRadXYcomp; // 2..3
|
||||
uint32_t time_ms; // 4
|
||||
Vector3f bodyRadXYZ; //8..10
|
||||
const Vector3f *body_offset;// 5..7
|
||||
Vector2f flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
|
||||
Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
|
||||
const Vector3f *body_offset;// pointer to XYZ position of the optical flow sensor in body frame (m)
|
||||
};
|
||||
|
||||
struct bfodm_elements {
|
||||
Vector3f vel; // XYZ velocity measured in body frame (m/s)
|
||||
float velErr; // velocity measurement error 1-std (m/s)
|
||||
const Vector3f *body_offset;// pointer to XYZ position of the velocity sensor in body frame (m)
|
||||
Vector3f angRate; // angular rate estimated from odometry (rad/sec)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
};
|
||||
|
||||
// update the navigation filter status
|
||||
@ -469,6 +499,9 @@ private:
|
||||
// fuse selected position, velocity and height measurements
|
||||
void FuseVelPosNED();
|
||||
|
||||
// fuse body frame velocity measurements
|
||||
void FuseBodyVel();
|
||||
|
||||
// fuse range beacon measurements
|
||||
void FuseRngBcn();
|
||||
|
||||
@ -624,12 +657,18 @@ private:
|
||||
// return true if the filter is ready to start using optical flow measurements
|
||||
bool readyToUseOptFlow(void) const;
|
||||
|
||||
// return true if the filter is ready to start using body frame odometry measurements
|
||||
bool readyToUseBodyOdm(void) const;
|
||||
|
||||
// return true if we should use the range finder sensor
|
||||
bool useRngFinder(void) const;
|
||||
|
||||
// determine when to perform fusion of optical flow measurements
|
||||
void SelectFlowFusion();
|
||||
|
||||
// determine when to perform fusion of body frame odometry measurements
|
||||
void SelectBodyOdomFusion();
|
||||
|
||||
// Estimate terrain offset using a single state EKF
|
||||
void EstimateTerrainOffset();
|
||||
|
||||
@ -986,8 +1025,9 @@ private:
|
||||
bool gndOffsetValid; // true when the ground offset state can still be considered valid
|
||||
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
|
||||
float delTimeOF; // time that delAngBodyOF is summed across
|
||||
Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m)
|
||||
bool flowFusionActive; // true when optical flow fusion is active
|
||||
|
||||
Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m)
|
||||
|
||||
// Range finder
|
||||
float baroHgtOffset; // offset applied when when switching to use of Baro height
|
||||
@ -999,6 +1039,21 @@ private:
|
||||
bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference
|
||||
uint32_t terrainHgtStableSet_ms; // system time at which terrainHgtStable was set
|
||||
|
||||
// body frame odometry fusion
|
||||
obs_ring_buffer_t<bfodm_elements> storedBodyOdm; // body velocity data buffer
|
||||
bfodm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon
|
||||
bfodm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon
|
||||
uint8_t bodyOdmStoreIndex; // Body frame odometry data storage index
|
||||
uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innvovation consistency checks (msec)
|
||||
Vector3 bodyVelTestRatio; // Innovation test ratios for body velocity XYZ measurements
|
||||
Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (rad/sec)^2
|
||||
Vector3 innovBodyVel; // Body velocity XYZ innovations (rad/sec)
|
||||
uint32_t prevBodyVelFuseTime_ms; // previous time all body velocity measurement components passed their innovation consistency checks (msec)
|
||||
uint32_t bodyOdmMeasTime_ms; // time body velocity measurements were accepted for input to the data buffer (msec)
|
||||
bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed
|
||||
bool bodyVelFusionActive; // true when body frame velocity fusion is active
|
||||
|
||||
|
||||
// Range Beacon Sensor Fusion
|
||||
obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
|
||||
rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon
|
||||
@ -1094,6 +1149,9 @@ private:
|
||||
bool bad_xflow:1;
|
||||
bool bad_yflow:1;
|
||||
bool bad_rngbcn:1;
|
||||
bool bad_xvel:1;
|
||||
bool bad_yvel:1;
|
||||
bool bad_zvel:1;
|
||||
} faultStatus;
|
||||
|
||||
// flags indicating which GPS quality checks are failing
|
||||
@ -1144,6 +1202,7 @@ private:
|
||||
AP_HAL::Util::perf_counter_t _perf_FuseSideslip;
|
||||
AP_HAL::Util::perf_counter_t _perf_TerrainOffset;
|
||||
AP_HAL::Util::perf_counter_t _perf_FuseOptFlow;
|
||||
AP_HAL::Util::perf_counter_t _perf_FuseBodyOdom;
|
||||
AP_HAL::Util::perf_counter_t _perf_test[10];
|
||||
|
||||
// should we assume zero sideslip?
|
||||
|
||||
Loading…
Reference in New Issue
Block a user