// update IMU delta angle and delta velocity measurements void NavEKF2_core::readIMUData() { const AP_InertialSensor &ins = _ahrs->get_ins(); // average IMU sampling rate dtIMUavg = 1.0f/ins.get_sample_rate(); // the imu sample time is used as a common time reference throughout the filter imuSampleTime_ms = hal.scheduler->millis(); // use the nominated imu or primary if not available if (ins.use_accel(imu_index)) { readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT); } else { readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); } // Get delta angle data from primary gyro or primary if not available if (ins.use_gyro(imu_index)) { readDeltaAngle(imu_index, imuDataNew.delAng); } else { readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); } imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f); // get current time stamp imuDataNew.time_ms = imuSampleTime_ms; // save data in the FIFO buffer StoreIMU(); // extract the oldest available data from the FIFO buffer imuDataDelayed = storedIMU[fifoIndexDelayed]; }
/* * Read IMU delta angle and delta velocity measurements and downsample to 100Hz * for storage in the data buffers used by the EKF. If the IMU data arrives at * lower rate than 100Hz, then no downsampling or upsampling will be performed. * Downsampling is done using a method that does not introduce coning or sculling * errors. */ void NavEKF3_core::readIMUData() { const AP_InertialSensor &ins = _ahrs->get_ins(); // average IMU sampling rate dtIMUavg = ins.get_loop_delta_t(); // the imu sample time is used as a common time reference throughout the filter imuSampleTime_ms = AP_HAL::millis(); // use the nominated imu or primary if not available if (ins.use_accel(imu_index)) { readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT); accelPosOffset = ins.get_imu_pos_offset(imu_index); } else { readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel()); } // Get delta angle data from primary gyro or primary if not available if (ins.use_gyro(imu_index)) { readDeltaAngle(imu_index, imuDataNew.delAng); } else { readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); } imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f); // Get current time stamp imuDataNew.time_ms = imuSampleTime_ms; // Accumulate the measurement time interval for the delta velocity and angle data imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT; imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT; // Rotate quaternon atitude from previous to new and normalise. // Accumulation using quaternions prevents introduction of coning errors due to downsampling imuQuatDownSampleNew.rotate(imuDataNew.delAng); imuQuatDownSampleNew.normalize(); // Rotate the latest delta velocity into body frame at the start of accumulation Matrix3f deltaRotMat; imuQuatDownSampleNew.rotation_matrix(deltaRotMat); // Apply the delta velocity to the delta velocity accumulator imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel; // Keep track of the number of IMU frames since the last state prediction framesSincePredict++; // If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data // to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) { // convert the accumulated quaternion to an equivalent delta angle imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); // Time stamp the data imuDataDownSampledNew.time_ms = imuSampleTime_ms; // Write data to the FIFO IMU buffer storedIMU.push_youngest_element(imuDataDownSampledNew); // calculate the achieved average time step rate for the EKF float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT); dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; // zero the accumulated IMU data and quaternion imuDataDownSampledNew.delAng.zero(); imuDataDownSampledNew.delVel.zero(); imuDataDownSampledNew.delAngDT = 0.0f; imuDataDownSampledNew.delVelDT = 0.0f; imuQuatDownSampleNew[0] = 1.0f; imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f; // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle framesSincePredict = 0; // set the flag to let the filter know it has new IMU data nad needs to run runUpdates = true; // extract the oldest available data from the FIFO buffer imuDataDelayed = storedIMU.pop_oldest_element(); // protect against delta time going to zero // TODO - check if calculations can tolerate 0 float minDT = 0.1f*dtEkfAvg; imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); // correct the extracted IMU data for sensor errors delAngCorrected = imuDataDelayed.delAng; delVelCorrected = imuDataDelayed.delVel; correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT); correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT); } else { // we don't have new IMU data in the buffer so don't run filter updates on this time step runUpdates = false; } }
/* * Read IMU delta angle and delta velocity measurements and downsample to 100Hz * for storage in the data buffers used by the EKF. If the IMU data arrives at * lower rate than 100Hz, then no downsampling or upsampling will be performed. * Downsampling is done using a method that does not introduce coning or sculling * errors. */ void NavEKF2_core::readIMUData() { const AP_InertialSensor &ins = _ahrs->get_ins(); // average IMU sampling rate dtIMUavg = 1.0f/ins.get_sample_rate(); // the imu sample time is used as a common time reference throughout the filter imuSampleTime_ms = AP_HAL::millis(); // use the nominated imu or primary if not available if (ins.use_accel(imu_index)) { readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT); } else { readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); } // Get delta angle data from primary gyro or primary if not available if (ins.use_gyro(imu_index)) { readDeltaAngle(imu_index, imuDataNew.delAng); } else { readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); } imuDataNew.delAngDT = MAX(ins.get_delta_time(),1.0e-4f); // Get current time stamp imuDataNew.time_ms = imuSampleTime_ms; // remove gyro scale factor errors imuDataNew.delAng.x = imuDataNew.delAng.x * stateStruct.gyro_scale.x; imuDataNew.delAng.y = imuDataNew.delAng.y * stateStruct.gyro_scale.y; imuDataNew.delAng.z = imuDataNew.delAng.z * stateStruct.gyro_scale.z; // remove sensor bias errors imuDataNew.delAng -= stateStruct.gyro_bias; imuDataNew.delVel.z -= stateStruct.accel_zbias; // Accumulate the measurement time interval for the delta velocity and angle data imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT; imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT; // Rotate quaternon atitude from previous to new and normalise. // Accumulation using quaternions prevents introduction of coning errors due to downsampling Quaternion deltaQuat; deltaQuat.rotate(imuDataNew.delAng); imuQuatDownSampleNew = imuQuatDownSampleNew*deltaQuat; imuQuatDownSampleNew.normalize(); // Rotate the accumulated delta velocity into the new frame of reference created by the latest delta angle // This prevents introduction of sculling errors due to downsampling Matrix3f deltaRotMat; deltaQuat.inverse().rotation_matrix(deltaRotMat); imuDataDownSampledNew.delVel = deltaRotMat*imuDataDownSampledNew.delVel; // accumulate the latest delta velocity imuDataDownSampledNew.delVel += imuDataNew.delVel; // Keep track of the number of IMU frames since the last state prediction framesSincePredict++; // If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data // to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed if ((dtIMUavg*(float)framesSincePredict >= 0.01f && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 0.02f)) { // convert the accumulated quaternion to an equivalent delta angle imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); // Time stamp the data imuDataDownSampledNew.time_ms = imuSampleTime_ms; // Write data to the FIFO IMU buffer storedIMU.push_youngest_element(imuDataDownSampledNew); // zero the accumulated IMU data and quaternion imuDataDownSampledNew.delAng.zero(); imuDataDownSampledNew.delVel.zero(); imuDataDownSampledNew.delAngDT = 0.0f; imuDataDownSampledNew.delVelDT = 0.0f; imuQuatDownSampleNew[0] = 1.0f; imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f; // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle framesSincePredict = 0; // set the flag to let the filter know it has new IMU data nad needs to run runUpdates = true; } else { // we don't have new IMU data in the buffer so don't run filter updates on this time step runUpdates = false; } // extract the oldest available data from the FIFO buffer imuDataDelayed = storedIMU.pop_oldest_element(); float minDT = 0.1f*dtEkfAvg; imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); }
// update IMU delta angle and delta velocity measurements void NavEKF2_core::readIMUData() { const AP_InertialSensor &ins = _ahrs->get_ins(); // average IMU sampling rate dtIMUavg = 1.0f/ins.get_sample_rate(); // the imu sample time is used as a common time reference throughout the filter imuSampleTime_ms = hal.scheduler->millis(); if (ins.use_accel(0) && ins.use_accel(1)) { // dual accel mode // delta time from each IMU float dtDelVel0 = dtIMUavg; float dtDelVel1 = dtIMUavg; // delta velocity vector from each IMU Vector3f delVel0, delVel1; // Get delta velocity and time data from each IMU readDeltaVelocity(0, delVel0, dtDelVel0); readDeltaVelocity(1, delVel1, dtDelVel1); // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 0 float alpha = 1.0f - 5.0f*dtDelVel0; imuNoiseFiltState0 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState0); // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 1 alpha = 1.0f - 5.0f*dtDelVel1; imuNoiseFiltState1 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState1); // calculate the filtered difference between acceleration vectors from IMU 0 and 1 // apply a LPF filter with a 1.0 second time constant alpha = constrain_float(0.5f*(dtDelVel0 + dtDelVel1),0.0f,1.0f); accelDiffFilt = (ins.get_accel(0) - ins.get_accel(1)) * alpha + accelDiffFilt * (1.0f - alpha); float accelDiffLength = accelDiffFilt.length(); // Check the difference for excessive error and use the IMU with less noise // Apply hysteresis to prevent rapid switching if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != IMUSWITCH_MIXED)) { if (lastImuSwitchState == IMUSWITCH_MIXED) { // no previous fail so switch to the IMU with least noise if (imuNoiseFiltState0 < imuNoiseFiltState1) { lastImuSwitchState = IMUSWITCH_IMU0; // Get data from IMU 0 imuDataNew.delVel = delVel0; imuDataNew.delVelDT = dtDelVel0; } else { lastImuSwitchState = IMUSWITCH_IMU1; // Get data from IMU 1 imuDataNew.delVel = delVel1; imuDataNew.delVelDT = dtDelVel1; } } else if (lastImuSwitchState == IMUSWITCH_IMU0) { // IMU 1 previously failed so require 5 m/s/s less noise on IMU 1 to switch if (imuNoiseFiltState0 - imuNoiseFiltState1 > 5.0f) { // IMU 1 is significantly less noisy, so switch lastImuSwitchState = IMUSWITCH_IMU1; // Get data from IMU 1 imuDataNew.delVel = delVel1; imuDataNew.delVelDT = dtDelVel1; } } else { // IMU 0 previously failed so require 5 m/s/s less noise on IMU 0 to switch across if (imuNoiseFiltState1 - imuNoiseFiltState0 > 5.0f) { // IMU 0 is significantly less noisy, so switch lastImuSwitchState = IMUSWITCH_IMU0; // Get data from IMU 0 imuDataNew.delVel = delVel0; imuDataNew.delVelDT = dtDelVel0; } } } else { lastImuSwitchState = IMUSWITCH_MIXED; // Use a blend of both accelerometers imuDataNew.delVel = (delVel0 + delVel1)*0.5f; imuDataNew.delVelDT = (dtDelVel0 + dtDelVel1)*0.5f; } } else { // single accel mode - one of the first two accelerometers are unhealthy, not available or de-selected by the user // set the switch state based on the IMU we are using to make the data source selection visible if (ins.use_accel(0)) { readDeltaVelocity(0, imuDataNew.delVel, imuDataNew.delVelDT); lastImuSwitchState = IMUSWITCH_IMU0; } else if (ins.use_accel(1)) { readDeltaVelocity(1, imuDataNew.delVel, imuDataNew.delVelDT); lastImuSwitchState = IMUSWITCH_IMU1; } else { readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); switch (ins.get_primary_accel()) { case 0: lastImuSwitchState = IMUSWITCH_IMU0; break; case 1: lastImuSwitchState = IMUSWITCH_IMU1; break; default: // we must be using an IMU which can't be properly represented so we set to "mixed" lastImuSwitchState = IMUSWITCH_MIXED; break; } } } // Get delta angle data from promary gyro readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f); // get current time stamp imuDataNew.time_ms = imuSampleTime_ms; // save data in the FIFO buffer StoreIMU(); // extract the oldest available data from the FIFO buffer imuDataDelayed = storedIMU[fifoIndexDelayed]; }