// run a 9-state EKF used to calculate orientation
void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
{
imuSampleTime_ms = hal.scheduler->millis();
dtIMU = delta_time;
// initialise variables and constants
if (!FiltInit) {
StartTime_ms = imuSampleTime_ms;
newDataMag = false;
YawAligned = false;
state.quat[0] = 1.0f;
const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
const float Sigma_dAngBias = 0.01745f*dtIMU; // 1 Sigma uncertainty in delta angle bias
const float Sigma_angErr = 1.0f; // 1 Sigma uncertainty in angular misalignment (rad)
for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
FiltInit = true;
hal.console->printf("SmallEKF Alignment Started\n");
}
// We are using the IMU data from the flight vehicle and setting joint angles to zero for the time being
gSense.gPsi = joint_angles.z; // yaw
gSense.gPhi = joint_angles.x; // roll
gSense.gTheta = joint_angles.y; // pitch
cosPhi = cosf(gSense.gPhi);
cosTheta = cosf(gSense.gTheta);
sinPhi = sinf(gSense.gPhi);
sinTheta = sinf(gSense.gTheta);
sinPsi = sinf(gSense.gPsi);
cosPsi = cosf(gSense.gPsi);
gSense.delAng = delta_angles;
gSense.delVel = delta_velocity;
// predict states
predictStates();
// predict the covariance
predictCovariance();
// fuse SmallEKF velocity data
fuseVelocity(YawAligned);
// Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
if ((((imuSampleTime_ms - StartTime_ms) > 5000 && TiltCorrection < 1e-4f) || ((imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
//calculate the initial heading using magnetometer, estimated tilt and declination
alignHeading();
YawAligned = true;
hal.console->printf("SmallEKF Alignment Completed\n");
}
// Fuse magnetometer data if we have new measurements and an aligned heading
readMagData();
if (newDataMag && YawAligned) {
fuseCompass();
newDataMag = false;
}
}
// run a 9-state EKF used to calculate orientation
void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
{
imuSampleTime_ms = AP_HAL::millis();
dtIMU = delta_time;
// initialise variables and constants
if (!FiltInit) {
// Note: the start time is initialised to 0 in the constructor
if (StartTime_ms == 0) {
StartTime_ms = imuSampleTime_ms;
}
// Set data to pre-initialsation defaults
FiltInit = false;
newDataMag = false;
YawAligned = false;
memset(&state, 0, sizeof(state));
state.quat[0] = 1.0f;
bool main_ekf_healthy = false;
nav_filter_status main_ekf_status;
if (_ahrs.get_filter_status(main_ekf_status)) {
if (main_ekf_status.flags.attitude) {
main_ekf_healthy = true;
}
}
// Wait for gimbal to stabilise to body fixed position for a few seconds before starting small EKF
// Also wait for navigation EKF to be healthy beasue we are using the velocity output data
// This prevents jerky gimbal motion from degrading the EKF initial state estimates
if (imuSampleTime_ms - StartTime_ms < 5000 || !main_ekf_healthy) {
return;
}
Quaternion ned_to_vehicle_quat;
ned_to_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
Quaternion vehicle_to_gimbal_quat;
vehicle_to_gimbal_quat.from_vector312(joint_angles.x,joint_angles.y,joint_angles.z);
// calculate initial orientation
state.quat = ned_to_vehicle_quat * vehicle_to_gimbal_quat;
const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
const float Sigma_dAngBias = 0.002f*dtIMU; // 1 Sigma uncertainty in delta angle bias (rad)
const float Sigma_angErr = 0.1f; // 1 Sigma uncertainty in angular misalignment (rad)
for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
FiltInit = true;
hal.console->printf("\nSoloGimbalEKF Alignment Started\n");
// Don't run the filter in this timestep becasue we have already used the delta velocity data to set an initial orientation
return;
}
// We are using IMU data and joint angles from the gimbal
gSense.gPsi = joint_angles.z; // yaw
gSense.gPhi = joint_angles.x; // roll
gSense.gTheta = joint_angles.y; // pitch
cosPhi = cosf(gSense.gPhi);
cosTheta = cosf(gSense.gTheta);
sinPhi = sinf(gSense.gPhi);
sinTheta = sinf(gSense.gTheta);
sinPsi = sinf(gSense.gPsi);
cosPsi = cosf(gSense.gPsi);
gSense.delAng = delta_angles;
gSense.delVel = delta_velocity;
// predict states
predictStates();
// predict the covariance
predictCovariance();
// fuse SoloGimbalEKF velocity data
fuseVelocity();
// Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
// Force it to align if too much time has lapsed
if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
//calculate the initial heading using magnetometer, estimated tilt and declination
alignHeading();
YawAligned = true;
hal.console->printf("\nSoloGimbalEKF Alignment Completed\n");
}
// Fuse magnetometer data if we have new measurements and an aligned heading
readMagData();
if (newDataMag && YawAligned) {
fuseCompass();
newDataMag = false;
}
}
