// reset the vertical position state using the last height measurement
void NavEKF2_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,8,8);
    zeroCols(P,8,8);

    // set the variances to the measurement variance
    P[8][8] = 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,5,5);
    zeroCols(P,5,5);

    // set the variances to the measurement variance
    P[5][5] = sq(frontend->_gpsVertVelNoise);

}
void StringUtil::addZeros(int** matrix, int rows, int cols) {
	vector<bool> zeroRows(rows, false);
	vector<bool> zeroCols(cols, false);

	for (int i = 0; i < rows; i++) {
		for (int j = 0; j < cols; j++) {
			if (matrix[i][j] == 0) {
				zeroRows[i] = true;
				zeroCols[j] = true;
			}
		}
	}

	for (int i = 0; i < rows; i++) {
		if (zeroRows[i]) {
			for (int j = 0; j < cols; j++)
				matrix[i][j] = 0;
		}
	}

	for (int i = 0; i < cols; i++) {
		if (zeroCols[i]) {
			for (int j = 0; j < cols; j++)
				matrix[j][i] = 0;
		}
	}

}
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF2_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,6,7);
    zeroCols(P,6,7);

    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[6][6] = P[7][7] = sq(frontend->_gpsHorizPosNoise);
    } else  {
        // Use GPS data as first preference if fresh data is available
        if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
            // 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[6][6] = P[7][7] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
            // clear the timeout flags and counters
            posTimeout = false;
            lastPosPassTime_ms = imuSampleTime_ms;
        } else if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) {
            // 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[6][6] = receiverPosCov[0][0];
            P[7][7] = 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;

}
示例#4
0
// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances
// Assume that the calibration is performed to an accuracy of 0.5 deg/sec which will require averaging under static conditions
// WARNING - a non-blocking calibration method must be used
void NavEKF2_core::resetGyroBias(void)
{
    stateStruct.gyro_bias.zero();
    zeroRows(P,9,11);
    zeroCols(P,9,11);

    P[9][9] = sq(radians(0.5f * dtIMUavg));
    P[10][10] = P[9][9];
    P[11][11] = P[9][9];
}
示例#5
0
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
	// If we don't a tilt estimate then we cannot initialise the yaw
	if (!_control_status.flags.tilt_align) {
		return false;
	}

	// get the roll, pitch, yaw estimates and set the yaw to zero
	matrix::Quaternion<float> q(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
				    _state.quat_nominal(3));
	matrix::Euler<float> euler_init(q);
	euler_init(2) = 0.0f;

	// rotate the magnetometer measurements into earth axes
	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
	euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));

	// calculate initial quaternion states for the ekf
	// we don't change the output attitude to avoid jumps
	_state.quat_nominal = Quaternion(euler_init);

	// reset the angle error variances because the yaw angle could have changed by a significant amount
	// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
	// will grow them again.
	zeroRows(P, 0, 2);
	zeroCols(P, 0, 2);

	// calculate initial earth magnetic field states
	matrix::Dcm<float> R_to_earth(euler_init);
	_state.mag_I = R_to_earth * mag_init;

	// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
	zeroRows(P, 16, 21);
	zeroCols(P, 16, 21);

	for (uint8_t index = 16; index <= 21; index ++) {
		P[index][index] = sq(_params.mag_noise);
	}

	return true;
}
// 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 NavEKF2_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,3,4);
    zeroCols(P,3,4);

    if (PV_AidingMode != AID_ABSOLUTE) {
        stateStruct.velocity.zero();
        // set the variances using the measurement noise parameter
        P[4][4] = P[3][3] = sq(frontend->_gpsHorizVelNoise);
    } else {
        // reset horizontal velocity states to the GPS velocity if available
        if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
            stateStruct.velocity.x  = gpsDataNew.vel.x;
            stateStruct.velocity.y  = gpsDataNew.vel.y;
            // set the variances using the reported GPS speed accuracy
            P[4][4] = P[3][3] = 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[4][4] = P[3][3] = 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;


}
// reset the vertical position state using the last height measurement
void NavEKF2_core::ResetHeight(void)
{
    // write to the state vector
    stateStruct.position.z = -hgtMea;
    terrainState = stateStruct.position.z + rngOnGnd;
    for (uint8_t i=0; i<imu_buffer_length; i++) {
        storedOutput[i].position.z = stateStruct.position.z;
    }
    outputDataNew.position.z = stateStruct.position.z;
    outputDataDelayed.position.z = stateStruct.position.z;

    // reset the corresponding covariances
    zeroRows(P,8,8);
    zeroCols(P,8,8);

    // set the variances to the measurement variance
    P[8][8] = 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,5,5);
    zeroCols(P,5,5);

    // set the variances to the measurement variance
    P[5][5] = sq(frontend->_gpsVertVelNoise);

}
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF2_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;

    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;
    } else  {
        // 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));
    }
    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;

    // reset the corresponding covariances
    zeroRows(P,6,7);
    zeroCols(P,6,7);

    // set the variances to the measurement variance
    P[6][6] = P[7][7] = sq(frontend->_gpsHorizPosNoise);

}
// 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 NavEKF2_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;

    if (PV_AidingMode != AID_ABSOLUTE) {
        stateStruct.velocity.zero();
    } else {
        // reset horizontal velocity states to the GPS velocity
        stateStruct.velocity.x  = gpsDataNew.vel.x; // north velocity from blended accel data
        stateStruct.velocity.y  = gpsDataNew.vel.y; // east velocity from blended accel data
    }
    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;

    // reset the corresponding covariances
    zeroRows(P,3,4);
    zeroCols(P,3,4);

    // set the variances to the measurement variance
    P[4][4] = P[3][3] = sq(frontend->_gpsHorizVelNoise);

}
// fuse selected position, velocity and height measurements
void NavEKF2_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
    float posErr;
    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) {
        // set the GPS data timeout depending on whether airspeed data is present
        uint32_t gpsRetryTime;
        if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms;
        else gpsRetryTime = frontend->gpsRetryTimeNoTAS_ms;

        // 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
        posErr = frontend->gpsPosVarAccScale * accNavMag;

        // estimate the GPS Velocity, GPS horiz position and height measurement variances.
        // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
        // otherwise we scale it using manoeuvre acceleration
        // Use different errors if flying without GPS using synthetic position and velocity data
        if (PV_AidingMode == AID_NONE && inFlight) {
            // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
            // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
            R_OBS[0] = sq(10.0f);
            R_OBS[1] = R_OBS[0];
            R_OBS[2] = R_OBS[0];
            // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
            R_OBS[3] = sq(25.0f);
            R_OBS[4] = R_OBS[3];
        } else {
            if (gpsSpdAccuracy > 0.0f) {
                // use GPS receivers reported speed accuracy if available and floor at value set by gps 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];
            R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
            R_OBS[4] = R_OBS[3];
        }
        R_OBS[5] = posDownObsNoise;

        // 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<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
        for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];

        // if vertical GPS velocity data and an independant 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[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + 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[6][6] + R_OBS_DATA_CHECKS[3];
            varInnovVelPos[4] = P[7][7] + 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);
            // declare a timeout condition if we have been too long without data or not aiding
            posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
            // use position data if healthy, timed out, or in constant position mode
            if (posHealth || posTimeout || (PV_AidingMode == AID_NONE)) {
                posHealth = true;
                // only reset the failed time and do glitch timeout checks if we are doing full aiding
                if (PV_AidingMode == AID_ABSOLUTE) {
                    lastPosPassTime_ms = imuSampleTime_ms;
                    // if timed out or outside the specified uncertainty radius, reset to the GPS
                    if (posTimeout || ((P[6][6] + 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,6,7);
                        zeroCols(P,6,7);
                        P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
                        P[7][7] = P[6][6];
                        // 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 3
                stateIndex   = i + 3;
                // 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);
            // declare a timeout if we have not fused velocity data for too long or not aiding
            velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
            // 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[8][8] + 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();
                    hgtTimeout = false;
                }

                // 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;
            }
            tiltErrVec.zero();
        }
        if (fusePosData && posHealth) {
            fuseData[3] = true;
            fuseData[4] = true;
            tiltErrVec.zero();
        }
        if (fuseHgtData && hgtHealth) {
            fuseData[5] = true;
        }

        // fuse measurements sequentially
        for (obsIndex=0; obsIndex<=5; obsIndex++) {
            if (fuseData[obsIndex]) {
                stateIndex = 3 + 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()) {
                        // 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<=15; i++) {
                    Kfusion[i] = P[i][stateIndex]*SK;
                }

                // 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 {
                    for (uint8_t i = 16; i<=21; i++) {
                        Kfusion[i] = 0.0f;
                    }
                }

                // 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 {
                    Kfusion[22] = 0.0f;
                    Kfusion[23] = 0.0f;
                }

                // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
                stateStruct.angErr.zero();

                // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
                for (uint8_t i = 0; i<=stateIndexLim; i++) {
                    statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
                }

                // the first 3 states represent the angular misalignment vector. This is
                // is used to correct the estimated quaternion
                stateStruct.quat.rotate(stateStruct.angErr);

                // sum the attitude error from velocity and position fusion only
                // used as a metric for convergence monitoring
                if (obsIndex != 5) {
                    tiltErrVec += stateStruct.angErr;
                }

                // 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];
                    }
                }
                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();

    // stop performance timer
    hal.util->perf_end(_perf_FuseVelPosNED);
}
示例#11
0
文件: control.cpp 项目: vvranjek/ecl
void Ekf::controlOpticalFlowFusion()
{
	// Check for new optical flow data that has fallen behind the fusion time horizon
	if (_flow_data_ready) {

		// optical flow fusion mode selection logic
		if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
				&& !_control_status.flags.opt_flow // we are not yet using flow data
				&& _control_status.flags.tilt_align // we know our tilt attitude
				&& (_time_last_imu - _time_last_hagl_fuse) < 5e5) // we have a valid distance to ground estimate
		{

			// If the heading is not aligned, reset the yaw and magnetic field states
			if (!_control_status.flags.yaw_align) {
				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
			}

			// If the heading is valid, start using optical flow aiding
			if (_control_status.flags.yaw_align) {
				// set the flag and reset the fusion timeout
				_control_status.flags.opt_flow = true;
				_time_last_of_fuse = _time_last_imu;

				// if we are not using GPS then the velocity and position states and covariances need to be set
				if (!_control_status.flags.gps) {
					// constrain height above ground to be above minimum possible
					float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);

					// calculate absolute distance from focal point to centre of frame assuming a flat earth
					float range = heightAboveGndEst / _R_to_earth(2, 2);

					if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
						// we should have reliable OF measurements so
						// calculate X and Y body relative velocities from OF measurements
						Vector3f vel_optflow_body;
						vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
						vel_optflow_body(1) =   range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
						vel_optflow_body(2) = 0.0f;

						// rotate from body to earth frame
						Vector3f vel_optflow_earth;
						vel_optflow_earth = _R_to_earth * vel_optflow_body;

						// take x and Y components
						_state.vel(0) = vel_optflow_earth(0);
						_state.vel(1) = vel_optflow_earth(1);

					} else {
						_state.vel(0) = 0.0f;
						_state.vel(1) = 0.0f;
					}

					// reset the velocity covariance terms
					zeroRows(P,4,5);
					zeroCols(P,4,5);

					// reset the horizontal velocity variance using the optical flow noise variance
					P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();

					if (!_control_status.flags.in_air) {
						// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
						_state.pos(0) = 0.0f;
						_state.pos(1) = 0.0f;

						// reset the corresponding covariances
						// we are by definition at the origin at commencement so variances are also zeroed
						zeroRows(P,7,8);
						zeroCols(P,7,8);

						// align the output observer to the EKF states
						alignOutputFilter();

					}
				}
			}

		} else if (!(_params.fusion_mode & MASK_USE_OF)) {
			_control_status.flags.opt_flow = false;

		}

		// handle the case when we are relying on optical flow fusion and lose it
		if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
			// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
			if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
				// Switch to the non-aiding mode, zero the velocity states
				// and set the synthetic position to the current estimate
				_control_status.flags.opt_flow = false;
				_last_known_posNE(0) = _state.pos(0);
				_last_known_posNE(1) = _state.pos(1);
				_state.vel.setZero();

			}
		}

		// fuse the data
		if (_control_status.flags.opt_flow) {
			// Update optical flow bias estimates
			calcOptFlowBias();

			// Fuse optical flow LOS rate observations into the main filter
			fuseOptFlow();
			_last_known_posNE(0) = _state.pos(0);
			_last_known_posNE(1) = _state.pos(1);

		}
	}
}
示例#12
0
// initialise the quaternion covariances using rotation vector variances
void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
{
	// calculate an equivalent rotation vector from the quaternion
	float q0,q1,q2,q3;
	if (_state.quat_nominal(0) >= 0.0f) {
		q0 = _state.quat_nominal(0);
		q1 = _state.quat_nominal(1);
		q2 = _state.quat_nominal(2);
		q3 = _state.quat_nominal(3);
	} else {
		q0 = -_state.quat_nominal(0);
		q1 = -_state.quat_nominal(1);
		q2 = -_state.quat_nominal(2);
		q3 = -_state.quat_nominal(3);
	}
	float delta = 2.0f*acosf(q0);
	float scaler = (delta/sinf(delta*0.5f));
	float rotX = scaler*q1;
	float rotY = scaler*q2;
	float rotZ = scaler*q3;

	// autocode generated using matlab symbolic toolbox
	float t2 = rotX*rotX;
	float t4 = rotY*rotY;
	float t5 = rotZ*rotZ;
	float t6 = t2+t4+t5;
	if (t6 > 1e-9f) {
		float t7 = sqrtf(t6);
		float t8 = t7*0.5f;
		float t3 = sinf(t8);
		float t9 = t3*t3;
		float t10 = 1.0f/t6;
		float t11 = 1.0f/sqrtf(t6);
		float t12 = cosf(t8);
		float t13 = 1.0f/powf(t6,1.5f);
		float t14 = t3*t11;
		float t15 = rotX*rotY*t3*t13;
		float t16 = rotX*rotZ*t3*t13;
		float t17 = rotY*rotZ*t3*t13;
		float t18 = t2*t10*t12*0.5f;
		float t27 = t2*t3*t13;
		float t19 = t14+t18-t27;
		float t23 = rotX*rotY*t10*t12*0.5f;
		float t28 = t15-t23;
		float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;
		float t25 = rotX*rotZ*t10*t12*0.5f;
		float t31 = t16-t25;
		float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f;
		float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f;
		float t24 = t15-t23;
		float t26 = t16-t25;
		float t29 = t4*t10*t12*0.5f;
		float t34 = t3*t4*t13;
		float t30 = t14+t29-t34;
		float t32 = t5*t10*t12*0.5f;
		float t40 = t3*t5*t13;
		float t33 = t14+t32-t40;
		float t36 = rotY*rotZ*t10*t12*0.5f;
		float t39 = t17-t36;
		float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;
		float t37 = t15-t23;
		float t38 = t17-t36;
		float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25);
		float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39;
		float t43 = t16-t25;
		float t44 = t17-t36;

		// zero all the quaternion covariances
		zeroRows(P,0,3);
		zeroCols(P,0,3);

		// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
		P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
		P[0][1] = t22;
		P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;
		P[0][3] = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f;
		P[1][0] = t22;
		P[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26);
		P[1][2] = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
		P[1][3] = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
		P[2][0] = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f;
		P[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
		P[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38);
		P[2][3] = t42;
		P[3][0] = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f;
		P[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
		P[3][2] = t42;
		P[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44);

	} else {
		// the equations are badly conditioned so use a small angle approximation
		P[0][0] = 0.0f;
		P[0][1] = 0.0f;
		P[0][2] = 0.0f;
		P[0][3] = 0.0f;
		P[1][0] = 0.0f;
		P[1][1] = 0.25f*rot_vec_var(0);
		P[1][2] = 0.0f;
		P[1][3] = 0.0f;
		P[2][0] = 0.0f;
		P[2][1] = 0.0f;
		P[2][2] = 0.25f*rot_vec_var(1);
		P[2][3] = 0.0f;
		P[3][0] = 0.0f;
		P[3][1] = 0.0f;
		P[3][2] = 0.0f;
		P[3][3] = 0.25f*rot_vec_var(2);

	}
}
示例#13
0
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
	// save a copy of the quaternion state for later use in calculating the amount of reset change
	Quaternion quat_before_reset = _state.quat_nominal;

	// calculate the variance for the rotation estimate expressed as a rotation vector
	// this will be used later to reset the quaternion state covariances
	Vector3f angle_err_var_vec = calcRotVecVariances();

	// update transformation matrix from body to world frame using the current estimate
	_R_to_earth = quat_to_invrotmat(_state.quat_nominal);

	// calculate the initial quaternion
	// determine if a 321 or 312 Euler sequence is best
	if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
		// use a 321 sequence

		// rotate the magnetometer measurement into earth frame
		matrix::Euler<float> euler321(_state.quat_nominal);

		// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
		euler321(2) = 0.0f;
		matrix::Dcm<float> R_to_earth(euler321);

		// calculate the observed yaw angle
		if (_params.fusion_mode & MASK_USE_EVYAW) {
			// convert the observed quaternion to a rotation matrix
			matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
			// calculate the yaw angle for a 312 sequence
			euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
			// rotate the magnetometer measurements into earth frame using a zero yaw angle
			Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
			// the angle of the projection onto the horizontal gives the yaw angle
			euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
		} else {
			// there is no yaw observation
			return false;
		}

		// calculate initial quaternion states for the ekf
		// we don't change the output attitude to avoid jumps
		_state.quat_nominal = Quaternion(euler321);

	} else {
		// use a 312 sequence

		// Calculate the 312 sequence euler angles that rotate from earth to body frame
		// See http://www.atacolorado.com/eulersequences.doc
		Vector3f euler312;
		euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
		euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
		euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)

		// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
		euler312(0) = 0.0f;

		// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
		float c2 = cosf(euler312(2));
		float s2 = sinf(euler312(2));
		float s1 = sinf(euler312(1));
		float c1 = cosf(euler312(1));
		float s0 = sinf(euler312(0));
		float c0 = cosf(euler312(0));

		matrix::Dcm<float> R_to_earth;
		R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
		R_to_earth(1, 1) = c0 * c1;
		R_to_earth(2, 2) = c2 * c1;
		R_to_earth(0, 1) = -c1 * s0;
		R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
		R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
		R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
		R_to_earth(2, 0) = -s2 * c1;
		R_to_earth(2, 1) = s1;

		// calculate the observed yaw angle
		if (_params.fusion_mode & MASK_USE_EVYAW) {
			// convert the observed quaternion to a rotation matrix
			matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
			// calculate the yaw angle for a 312 sequence
			euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
			// rotate the magnetometer measurements into earth frame using a zero yaw angle
			Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
			// the angle of the projection onto the horizontal gives the yaw angle
			euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
		} else {
			// there is no yaw observation
			return false;
		}

		// re-calculate the rotation matrix using the updated yaw angle
		s0 = sinf(euler312(0));
		c0 = cosf(euler312(0));
		R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
		R_to_earth(1, 1) = c0 * c1;
		R_to_earth(2, 2) = c2 * c1;
		R_to_earth(0, 1) = -c1 * s0;
		R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
		R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
		R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
		R_to_earth(2, 0) = -s2 * c1;
		R_to_earth(2, 1) = s1;

		// calculate initial quaternion states for the ekf
		// we don't change the output attitude to avoid jumps
		_state.quat_nominal = Quaternion(R_to_earth);
	}

	// update transformation matrix from body to world frame using the current estimate
	_R_to_earth = quat_to_invrotmat(_state.quat_nominal);

	// update the yaw angle variance using the variance of the measurement
	if (_params.fusion_mode & MASK_USE_EVYAW) {
		// using error estimate from external vision data
		angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
	} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
		// using magnetic heading tuning parameter
		angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
	}

	// reset the quaternion covariances using the rotation vector variances
	initialiseQuatCovariances(angle_err_var_vec);

	// calculate initial earth magnetic field states
	_state.mag_I = _R_to_earth * mag_init;

	// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
	zeroRows(P, 16, 21);
	zeroCols(P, 16, 21);

	for (uint8_t index = 16; index <= 21; index ++) {
		P[index][index] = sq(_params.mag_noise);
	}

	// calculate the amount that the quaternion has changed by
	_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();

	// add the reset amount to the output observer buffered data
	outputSample output_states;
	unsigned output_length = _output_buffer.get_length();
	for (unsigned i=0; i < output_length; i++) {
		output_states = _output_buffer.get_from_index(i);
		output_states.quat_nominal *= _state_reset_status.quat_change;
		_output_buffer.push_to_index(i,output_states);
	}

	// apply the change in attitude quaternion to our newest quaternion estimate
	// which was already taken out from the output buffer
	_output_new.quat_nominal *= _state_reset_status.quat_change;

	// capture the reset event
	_state_reset_status.quat_counter++;

	return true;
}
示例#14
0
// Reset height state using the last height measurement
void Ekf::resetHeight()
{
	// Get the most recent GPS data
	gpsSample gps_newest = _gps_buffer.get_newest();

	// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
	float old_vert_pos = _state.pos(2);
	bool vert_pos_reset = false;
	float old_vert_vel = _state.vel(2);
	bool vert_vel_reset = false;

	// reset the vertical position
	if (_control_status.flags.rng_hgt) {
		rangeSample range_newest = _range_buffer.get_newest();

		if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
			// calculate the new vertical position using range sensor
			float new_pos_down = _hgt_sensor_offset - range_newest.rng;

			// update the state and assoicated variance
			_state.pos(2) = new_pos_down;

			// reset the associated covariance values
			zeroRows(P, 9, 9);
			zeroCols(P, 9, 9);

			// the state variance is the same as the observation
			P[9][9] = sq(_params.range_noise);

			vert_pos_reset = true;

			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
			baroSample baro_newest = _baro_buffer.get_newest();
			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);

		} else {
			// TODO: reset to last known range based estimate
		}


	} else if (_control_status.flags.baro_hgt) {
		// initialize vertical position with newest baro measurement
		baroSample baro_newest = _baro_buffer.get_newest();

		if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
			_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;

			// reset the associated covariance values
			zeroRows(P, 9, 9);
			zeroCols(P, 9, 9);

			// the state variance is the same as the observation
			P[9][9] = sq(_params.baro_noise);

			vert_pos_reset = true;

		} else {
			// TODO: reset to last known baro based estimate
		}

	} else if (_control_status.flags.gps_hgt) {
		// initialize vertical position and velocity with newest gps measurement
		if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
			_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;

			// reset the associated covarince values
			zeroRows(P, 9, 9);
			zeroCols(P, 9, 9);

			// the state variance is the same as the observation
			P[9][9] = sq(gps_newest.hacc);

			vert_pos_reset = true;

			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
			baroSample baro_newest = _baro_buffer.get_newest();
			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);

		} else {
			// TODO: reset to last known gps based estimate
		}

	} else if (_control_status.flags.ev_hgt) {
		// initialize vertical position with newest measurement
		extVisionSample ev_newest = _ext_vision_buffer.get_newest();

		// use the most recent data if it's time offset from the fusion time horizon is smaller
		int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
		int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
		if (abs(dt_newest) < abs(dt_delayed)) {
			_state.pos(2) = ev_newest.posNED(2);
		} else {
			_state.pos(2) = _ev_sample_delayed.posNED(2);
		}

	}

	// reset the vertical velocity covariance values
	zeroRows(P, 6, 6);
	zeroCols(P, 6, 6);

	// reset the vertical velocity state
	if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
		// If we are using GPS, then use it to reset the vertical velocity
		_state.vel(2) = gps_newest.vel(2);

		// the state variance is the same as the observation
		P[6][6] = sq(1.5f * gps_newest.sacc);

	} else {
		// we don't know what the vertical velocity is, so set it to zero
		_state.vel(2) = 0.0f;

		// Set the variance to a value large enough to allow the state to converge quickly
		// that does not destabilise the filter
		P[6][6] = 10.0f;

	}
	vert_vel_reset = true;

	// store the reset amount and time to be published
	if (vert_pos_reset) {
		_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
		_state_reset_status.posD_counter++;
	}

	if (vert_vel_reset) {
		_state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
		_state_reset_status.velD_counter++;
	}

	// apply the change in height / height rate to our newest height / height rate estimate
	// which have already been taken out from the output buffer
	if (vert_pos_reset) {
		_output_new.pos(2) += _state_reset_status.posD_change;
	}

	if (vert_vel_reset) {
		_output_new.vel(2) += _state_reset_status.velD_change;
	}

	// add the reset amount to the output observer buffered data
	outputSample output_states;
	unsigned output_length = _output_buffer.get_length();
	for (unsigned i=0; i < output_length; i++) {
		output_states = _output_buffer.get_from_index(i);

		if (vert_pos_reset) {
			output_states.pos(2) += _state_reset_status.posD_change;
		}

		if (vert_vel_reset) {
			output_states.vel(2) += _state_reset_status.velD_change;
		}

		_output_buffer.push_to_index(i,output_states);

	}
}
示例#15
0
void Ekf::fuseAirspeed()
{
    // Initialize variables
    float vn; // Velocity in north direction
    float ve; // Velocity in east direction
    float vd; // Velocity in downwards direction
    float vwn; // Wind speed in north direction
    float vwe; // Wind speed in east direction
    float v_tas_pred; // Predicted measurement
    float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f,
                     10.0f)); // Variance for true airspeed measurement - (m/sec)^2
    float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian
    float H_TAS[24] = {}; // Observation Jacobian
    float SK_TAS[2] = {}; // Varialbe used to optimise calculations of the Kalman gain vector
    float Kfusion[24] = {}; // Kalman gain vector

    // Copy required states to local variable names
    vn = _state.vel(0);
    ve = _state.vel(1);
    vd = _state.vel(2);
    vwn = _state.wind_vel(0);
    vwe = _state.wind_vel(1);

    // Calculate the predicted airspeed
    v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);

    // Perform fusion of True Airspeed measurement
    if (v_tas_pred > 1.0f) {
        // Calculate the observation jacobian
        // intermediate variable from algebraic optimisation
        SH_TAS[0] = 1.0f/v_tas_pred;
        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;

        for (uint8_t i = 0; i < _k_num_states; i++) {
            H_TAS[i] = 0.0f;
        }

        H_TAS[4] = SH_TAS[2];
        H_TAS[5] = SH_TAS[1];
        H_TAS[6] = vd*SH_TAS[0];
        H_TAS[22] = -SH_TAS[2];
        H_TAS[23] = -SH_TAS[1];

        // We don't want to update the innovation variance if the calculation is ill conditioned
        float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));

        if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation
            SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
            _fault_status.flags.bad_airspeed = false;

        } else { // Reset the estimator covarinace matrix
            _fault_status.flags.bad_airspeed = true;
            initialiseCovariance();
            ECL_ERR("EKF airspeed fusion numerical error - covariance reset");
            return;
        }

        SK_TAS[1] = SH_TAS[1];

        Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
        Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
        Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
        Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
        Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
        Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
        Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
        Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
        Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
        Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
        Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
        Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
        Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
        Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]);
        Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]);
        Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]);
        Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
        Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
        Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
        Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]);
        Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]);
        Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]);
        Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
        Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);


        // Calculate measurement innovation
        _airspeed_innov = v_tas_pred -
                          _airspeed_sample_delayed.true_airspeed;

        // Calculate the innovation variance
        _airspeed_innov_var = 1.0f / SK_TAS[0];

        // Compute the ratio of innovation to gate size
        _tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);

        // If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health
        if (_tas_test_ratio > 1.0f) {
            _innov_check_fail_status.flags.reject_airspeed = true;
            return;
        }
        else {
            _innov_check_fail_status.flags.reject_airspeed = false;
        }

        // Airspeed measurement sample has passed check so record it
        _time_last_arsp_fuse = _time_last_imu;

        // apply covariance correction via P_new = (I -K*H)*P
        // first calculate expression for KHP
        // then calculate P - KHP
        for (unsigned row = 0; row < _k_num_states; row++) {
            KH[row][4] = Kfusion[row] * H_TAS[4];
            KH[row][5] = Kfusion[row] * H_TAS[5];
            KH[row][6] = Kfusion[row] * H_TAS[6];
            KH[row][22] = Kfusion[row] * H_TAS[22];
            KH[row][23] = Kfusion[row] * H_TAS[23];
        }

        for (unsigned row = 0; row < _k_num_states; row++) {
            for (unsigned column = 0; column < _k_num_states; column++) {
                float tmp = KH[row][4] * P[4][column];
                tmp += KH[row][5] * P[5][column];
                tmp += KH[row][6] * P[6][column];
                tmp += KH[row][22] * P[22][column];
                tmp += KH[row][23] * P[23][column];
                KHP[row][column] = tmp;
            }
        }

        // if the covariance correction will result in a negative variance, then
        // the covariance marix is unhealthy and must be corrected
        bool healthy = true;
        _fault_status.flags.bad_airspeed = false;
        for (int i = 0; i < _k_num_states; i++) {
            if (P[i][i] < KHP[i][i]) {
                // zero rows and columns
                zeroRows(P,i,i);
                zeroCols(P,i,i);

                //flag as unhealthy
                healthy = false;

                // update individual measurement health status
                _fault_status.flags.bad_airspeed = true;

            }
        }

        // only apply covariance and state corrrections if healthy
        if (healthy) {
            // apply the covariance corrections
            for (unsigned row = 0; row < _k_num_states; row++) {
                for (unsigned column = 0; column < _k_num_states; column++) {
                    P[row][column] = P[row][column] - KHP[row][column];
                }
            }

            // correct the covariance marix for gross errors
            fixCovarianceErrors();

            // apply the state corrections
            fuse(Kfusion, _airspeed_innov);

        }
    }
}