bool Ekf::initialiseFilter(void) { // Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter // Sum the IMU delta angle measurements imuSample imu_init = _imu_buffer.get_newest(); _delVel_sum += imu_init.delta_vel; // Sum the magnetometer measurements if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { if (_mag_counter == 0) { _mag_filt_state = _mag_sample_delayed.mag; } _mag_counter ++; _mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f; } // set the default height source from the adjustable parameter if (_hgt_counter == 0) { _primary_hgt_source = _params.vdist_sensor_type; } if (_primary_hgt_source == VDIST_SENSOR_RANGE) { if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { if (_hgt_counter == 0) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; _hgt_filt_state = _range_sample_delayed.rng; } _hgt_counter ++; _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng; } } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) { if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_hgt_counter == 0) { _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _hgt_filt_state = _baro_sample_delayed.hgt; } _hgt_counter ++; _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt; } } else { return false; } // check to see if we have enough measurements and return false if not if (_hgt_counter <= 10 || _mag_counter <= 10) { return false; } else { // reset variables that are shared with post alignment GPS checks _gps_drift_velD = 0.0f; _gps_alt_ref = 0.0f; // Zero all of the states _state.ang_error.setZero(); _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); _state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f; _state.accel_z_bias = 0.0f; _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); // get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static float pitch = 0.0f; float roll = 0.0f; if (_delVel_sum.norm() > 0.001f) { _delVel_sum.normalize(); pitch = asinf(_delVel_sum(0)); roll = atan2f(-_delVel_sum(1), -_delVel_sum(2)); } else { return false; } // calculate initial tilt alignment matrix::Euler<float> euler_init(roll, pitch, 0.0f); _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; // initialise the filtered alignment error estimate to a larger starting value _tilt_err_length_filt = 1.0f; // calculate the averaged magnetometer reading Vector3f mag_init = _mag_filt_state; // calculate the initial magnetic field and yaw alignment resetMagHeading(mag_init); // calculate the averaged height reading to calulate the height of the origin _hgt_sensor_offset = _hgt_filt_state; // if we are not using the baro height as the primary source, then calculate an offset relative to the origin // so it can be used as a backup if (!_control_status.flags.baro_hgt) { baroSample baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset; } else { _baro_hgt_offset = 0.0f; } // initialise the state covariance matrix initialiseCovariance(); // initialise the terrain estimator initHagl(); return true; } }
bool Ekf::initialiseFilter(void) { _state.ang_error.setZero(); _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); _state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f; _state.accel_z_bias = 0.0f; _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); // get initial roll and pitch estimate from accel vector, assuming vehicle is static Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt; float pitch = 0.0f; float roll = 0.0f; if (accel_init.norm() > 0.001f) { accel_init.normalize(); pitch = asinf(accel_init(0)); roll = -asinf(accel_init(1) / cosf(pitch)); } matrix::Euler<float> euler_init(roll, pitch, 0.0f); // Get the latest magnetic field measurement. // If we don't have a measurement then we cannot initialise the filter magSample mag_init = _mag_buffer.get_newest(); if (mag_init.time_us == 0) { return false; } // rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination // TODO use declination if available matrix::Dcm<float> R_to_earth_zeroyaw(euler_init); Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag; float declination = 0.0f; euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0)); // calculate initial quaternion states _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; // calculate initial earth magnetic field states matrix::Dcm<float> R_to_earth(euler_init); _state.mag_I = R_to_earth * mag_init.mag; resetVelocity(); resetPosition(); // initialize vertical position with newest baro measurement baroSample baro_init = _baro_buffer.get_newest(); if (baro_init.time_us == 0) { return false; } _state.pos(2) = -baro_init.hgt; _output_new.pos(2) = -baro_init.hgt; initialiseCovariance(); return true; }
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); } } }